diff --git a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h index a9598142..8c87dcfc 100644 --- a/bsp_q7s/devices/startracker/StarTrackerDefinitions.h +++ b/bsp_q7s/devices/startracker/StarTrackerDefinitions.h @@ -92,7 +92,7 @@ static const DeviceCommandId_t REQ_INTERFACE = 3; static const DeviceCommandId_t REQ_TIME = 4; static const DeviceCommandId_t ERASE = 5; static const DeviceCommandId_t UNLOCK = 6; -static const DeviceCommandId_t REBOOT = 7; +static const DeviceCommandId_t SWITCH_TO_BOOTLOADER_PROGRAM = 7; static const DeviceCommandId_t DOWNLOAD_IMAGE = 9; static const DeviceCommandId_t UPLOAD_IMAGE = 10; static const DeviceCommandId_t REQ_POWER = 11; @@ -120,6 +120,7 @@ static const DeviceCommandId_t DOWNLOAD_DB_IMAGE = 54; static const DeviceCommandId_t STOP_IMAGE_LOADER = 55; static const DeviceCommandId_t RESET_ERROR = 56; static const DeviceCommandId_t CHANGE_DOWNLOAD_FILE = 57; +static const DeviceCommandId_t SET_JSON_FILE_NAME = 58; static const DeviceCommandId_t NONE = 0xFFFFFFFF; static const uint32_t VERSION_SET_ID = REQ_VERSION; @@ -171,6 +172,11 @@ namespace ID { static const uint8_t ERROR_RESET = 12; } +namespace Program { + static const uint8_t BOOTLOADER = 1; + static const uint8_t FIRMWARE = 2; +} + /** * @brief This dataset can be used to store the temperature of a reaction wheel. */ diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp index 3e18a6ba..f957f9fc 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.cpp +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.cpp @@ -81,9 +81,19 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu if (imageLoaderExecuting == true) { return IMAGE_LOADER_EXECUTING; } + + result = checkMode(actionId); + if (result != RETURN_OK) { + return result; + } + // Intercept image loader commands which do not follow the common DHB communication flow switch(actionId) { case(StarTracker::UPLOAD_IMAGE): { + result = DeviceHandlerBase::acceptExternalDeviceCommands(); + if (result != RETURN_OK) { + return result; + } if (size > MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } @@ -96,6 +106,10 @@ ReturnValue_t StarTrackerHandler::executeAction(ActionId_t actionId, MessageQueu return EXECUTION_FINISHED; } case(StarTracker::DOWNLOAD_IMAGE): { + result = DeviceHandlerBase::acceptExternalDeviceCommands(); + if (result != RETURN_OK) { + return result; + } if (size > MAX_PATH_SIZE) { return FILE_PATH_TOO_LONG; } @@ -138,19 +152,33 @@ void StarTrackerHandler::performOperationHook() { } void StarTrackerHandler::doStartUp() { - -#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 -// setMode(MODE_NORMAL); + switch(startupState) { + case StartupState::IDLE: + startupState = StartupState::CHECK_BOOT_STATE; + return; + case StartupState::BOOT_DELAY: + if (bootCountdown.hasTimedOut()) { + startupState = StartupState::LIMITS; + } + return; + case StartupState::DONE: + break; + default: + return; + } setMode(_MODE_TO_ON); -#else - setMode(_MODE_TO_ON); -#endif } void StarTrackerHandler::doShutDown() { + // If star tracker is shutdown also stop all running processes in the image loader task + strImageLoader->stopProcess(); setMode(_MODE_POWER_DOWN); } +void StarTrackerHandler::doOffActivity() { + startupState = StartupState::IDLE; +} + ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * id) { switch (internalState) { case InternalState::TEMPERATURE_REQUEST: @@ -165,6 +193,72 @@ ReturnValue_t StarTrackerHandler::buildNormalDeviceCommand(DeviceCommandId_t * i } ReturnValue_t StarTrackerHandler::buildTransitionDeviceCommand(DeviceCommandId_t * id) { + if (mode != _MODE_START_UP) { + return NOTHING_TO_SEND; + } + switch (startupState) { + case StartupState::CHECK_BOOT_STATE: + *id = StarTracker::REQ_VERSION; + startupState = StartupState::WAIT_FOR_EXECUTION; + return buildCommandFromCommand(*id, nullptr, 0); + case StartupState::BOOT: + *id = StarTracker::BOOT; + bootCountdown.setTimeout(BOOT_TIMEOUT); + startupState = StartupState::BOOT_DELAY; + return buildCommandFromCommand(*id, nullptr, 0); + case StartupState::LIMITS: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::LIMITS; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::TRACKING: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::TRACKING; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::MOUNTING: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::MOUNTING; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::CAMERA: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::CAMERA; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::BLOB: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::BLOB; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::CENTROIDING: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::CENTROIDING; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::LISA: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::LISA; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::MATCHING: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::MATCHING; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::VALIDATION: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::VALIDATION; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + case StartupState::ALGO: + startupState = StartupState::WAIT_FOR_EXECUTION; + *id = StarTracker::ALGO; + return buildCommandFromCommand(*id, reinterpret_cast(paramJsonFile.c_str()), + paramJsonFile.size()); + default: + break; + } return NOTHING_TO_SEND; } @@ -196,7 +290,7 @@ ReturnValue_t StarTrackerHandler::buildCommandFromCommand(DeviceCommandId_t devi preparePowerRequest(); return RETURN_OK; } - case (StarTracker::REBOOT): { + case (StarTracker::SWITCH_TO_BOOTLOADER_PROGRAM): { prepareRebootCommand(); return RETURN_OK; } @@ -293,7 +387,7 @@ void StarTrackerHandler::fillCommandAndReplyMap() { this->insertInCommandAndReplyMap(StarTracker::REQ_INTERFACE, 3, &interfaceSet, StarTracker::MAX_FRAME_SIZE * 2 + 2); // Reboot has no reply. Star tracker reboots immediately - this->insertInCommandMap(StarTracker::REBOOT); + this->insertInCommandMap(StarTracker::SWITCH_TO_BOOTLOADER_PROGRAM); this->insertInCommandAndReplyMap(StarTracker::SUBSCRIBE_TO_TM, 3, nullptr, StarTracker::MAX_FRAME_SIZE * 2 + 2); this->insertInCommandAndReplyMap(StarTracker::REQ_SOLUTION, 3, &solutionSet, @@ -406,6 +500,13 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id, con } case (StarTracker::REQ_VERSION): { result = handleVersionTm(); + if (result != RETURN_OK) { + return result; + } + result = checkProgram(); + if (result != RETURN_OK) { + return result; + } break; } case (StarTracker::REQ_INTERFACE): { @@ -535,21 +636,29 @@ ReturnValue_t StarTrackerHandler::initializeLocalDataPool(localpool::DataPool& l } size_t StarTrackerHandler::getNextReplyLength(DeviceCommandId_t commandId){ - // Prevent DHB from polling UART during upload command. Because UART is used by image loader - // task - if (commandId == StarTracker::UPLOAD_IMAGE) { - return 0; - } return StarTracker::MAX_FRAME_SIZE; } ReturnValue_t StarTrackerHandler::doSendReadHook() { + // Prevent DHB from polling UART during commands executed by the image loader task if(imageLoaderExecuting) { return RETURN_FAILED; } return RETURN_OK; } +ReturnValue_t StarTrackerHandler::checkMode(ActionId_t actionId) { + switch(actionId) { + case StarTracker::UPLOAD_IMAGE: + case StarTracker::DOWNLOAD_IMAGE: { + return DeviceHandlerBase::acceptExternalDeviceCommands(); + default: + break; + } + } + return RETURN_OK; +} + ReturnValue_t StarTrackerHandler::scanForActionReply(DeviceCommandId_t *foundId) { const uint8_t* reply = dataLinkLayer.getReply(); switch (*reply) { @@ -836,8 +945,14 @@ ReturnValue_t StarTrackerHandler::handleSetParamReply() { sif::warning << "StarTrackerHandler::handleSetParamReply: Failed to execute parameter set " " command with parameter ID" << static_cast(*(reply + PARAMETER_ID_OFFSET)) << std::endl; + if (startupState != StartupState::IDLE) { + startupState = StartupState::IDLE; + } return SET_PARAM_FAILED; } + if (startupState != StartupState::IDLE) { + handleStartup(reply + PARAMETER_ID_OFFSET); + } return RETURN_OK; } @@ -863,13 +978,18 @@ ReturnValue_t StarTrackerHandler::handlePingReply() { size_t size = sizeof(pingId); SerializeAdapter::deSerialize(&pingId, &buffer, &size, SerializeIF::Endianness::LITTLE); #if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 - sif::info << "Ping status: "<< static_cast(status) << std::endl; + sif::info << "StarTracker: Ping status: "<< static_cast(status) << std::endl; sif::info << "Ping id: 0x"<< std::hex << pingId << std::endl; #endif /* OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 */ if (status != StarTracker::STATUS_OK || pingId != PING_ID) { sif::warning << "StarTrackerHandler::handlePingReply: Ping failed" << std::endl; result = PING_FAILED; } + else { +#if OBSW_VERBOSE_LEVEL >= 1 && OBSW_DEBUG_STARTRACKER == 1 + sif::info << "StarTracker: Ping successful" << std::endl; +#endif + } return result; } @@ -885,7 +1005,7 @@ ReturnValue_t StarTrackerHandler::handleTimeTm() { uint64_t time = 0; getTmHeaderData(&status, &ticks, &time); if(status != StarTracker::STATUS_OK) { - sif::warning << "StarTrackerHandler::handleVersionTm: Reply error: " + sif::warning << "StarTrackerHandler::handleTimeTm: Reply error: " << static_cast(status) << std::endl; result = VERSION_REQ_FAILED; return result; @@ -941,6 +1061,30 @@ ReturnValue_t StarTrackerHandler::handleVersionTm() { return result; } +ReturnValue_t StarTrackerHandler::checkProgram() { + PoolReadGuard pg(&versionSet); + switch(versionSet.program.value) { + case StarTracker::Program::BOOTLOADER: + // Star tracker currently in bootloader program. Need to send boot command to switch to + // firmware program + if (startupState != StartupState::IDLE) { + startupState = StartupState::BOOT; + } + break; + case StarTracker::Program::FIRMWARE: + // Firmware already booted + if (startupState != StartupState::IDLE) { + startupState = StartupState::LIMITS; + } + break; + default: + sif::warning << "StarTrackerHandler::checkProgram: Version set has invalid program ID" + << std::endl; + return INVALID_PROGRAM; + } + return RETURN_OK; +} + ReturnValue_t StarTrackerHandler::handleInterfaceTm() { ReturnValue_t result = RETURN_OK; uint32_t offset = TM_DATA_FIELD_OFFSET; @@ -1203,3 +1347,53 @@ uint64_t StarTrackerHandler::deserializeUint64(const uint8_t* buffer) { | static_cast(*(buffer)); return word; } + +void StarTrackerHandler::handleStartup(const uint8_t* parameterId) { + switch(*parameterId) { + case (StarTracker::ID::LIMITS): { + startupState = StartupState::TRACKING; + break; + } + case (StarTracker::ID::TRACKING): { + startupState = StartupState::MOUNTING; + break; + } + case (StarTracker::ID::MOUNTING): { + startupState = StartupState::CAMERA; + break; + } + case (StarTracker::ID::CAMERA): { + startupState = StartupState::BLOB; + break; + } + case (StarTracker::ID::BLOB): { + startupState = StartupState::CENTROIDING; + break; + } + case (StarTracker::ID::CENTROIDING): { + startupState = StartupState::LISA; + break; + } + case (StarTracker::ID::LISA): { + startupState = StartupState::MATCHING; + break; + } + case (StarTracker::ID::MATCHING): { + startupState = StartupState::VALIDATION; + break; + } + case (StarTracker::ID::VALIDATION): { + startupState = StartupState::ALGO; + break; + } + case (StarTracker::ID::ALGO): { + startupState = StartupState::DONE; + break; + } + default: { + sif::debug << "StarTrackerHandler::handleStartup: Received parameter reply with unexpected" + << " parameter ID" << std::endl; + break; + } + } +} diff --git a/bsp_q7s/devices/startracker/StarTrackerHandler.h b/bsp_q7s/devices/startracker/StarTrackerHandler.h index 76ca6ea1..e22ff739 100644 --- a/bsp_q7s/devices/startracker/StarTrackerHandler.h +++ b/bsp_q7s/devices/startracker/StarTrackerHandler.h @@ -3,6 +3,7 @@ #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/src/fsfw/serialize/SerializeAdapter.h" +#include "fsfw/timemanager/Countdown.h" #include "thirdparty/arcsec_star_tracker/common/SLIP.h" #include #include "StrImageLoader.h" @@ -49,6 +50,7 @@ public: protected: void doStartUp() override; void doShutDown() override; + void doOffActivity() override; ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t * id) override; ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t * id) override; void fillCommandAndReplyMap() override; @@ -62,12 +64,10 @@ protected: uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) override; - /** * @brief Overwritten here to always read all available data from the UartComIF. */ virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override; - virtual ReturnValue_t doSendReadHook() override; private: @@ -109,6 +109,8 @@ private: static const ReturnValue_t FILE_PATH_TOO_LONG = MAKE_RETURN_CODE(0xAB); //! [EXPORT] : [COMMENT] Name of file received with command is too long static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xAC); + //! [EXPORT] : [COMMENT] Received version reply with invalid program ID + static const ReturnValue_t INVALID_PROGRAM = MAKE_RETURN_CODE(0xAD); static const size_t MAX_PATH_SIZE = 50; static const size_t MAX_FILE_NAME = 30; @@ -124,13 +126,12 @@ private: static const uint8_t PARAMETER_ID_OFFSET = 0; static const uint8_t ACTION_ID_OFFSET = 0; static const uint8_t ACTION_DATA_OFFSET = 2; - // Ping request will reply ping with this ID (data field) static const uint32_t PING_ID = 0x55; static const uint32_t BOOT_REGION_ID = 1; - - static const MutexIF::TimeoutType TIMEOUT_TYPE= MutexIF::TimeoutType::WAITING; + static const MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING; static const uint32_t MUTEX_TIMEOUT = 20; + static const uint32_t BOOT_TIMEOUT = 1000; MessageQueueIF* eventQueue = nullptr; @@ -148,14 +149,52 @@ private: uint8_t commandBuffer[StarTracker::MAX_FRAME_SIZE]; + // Countdown to insert delay for star tracker to switch from bootloader to firmware program + Countdown bootCountdown; + + std::string paramJsonFile = "/mnt/sd0/startracker/full.json"; + enum class InternalState { TEMPERATURE_REQUEST }; InternalState internalState = InternalState::TEMPERATURE_REQUEST; + enum class StartupState { + IDLE, + CHECK_BOOT_STATE, + BOOT, + BOOT_DELAY, + LIMITS, + TRACKING, + MOUNTING, + CAMERA, + BLOB, + CENTROIDING, + LISA, + MATCHING, + VALIDATION, + ALGO, + WAIT_FOR_EXECUTION, + DONE + }; + + StartupState startupState = StartupState::IDLE; + bool imageLoaderExecuting = false; + /** + * @brief Handles internal state + */ + void handleInternalState(); + + /** + * @brief Checks mode for commands requiring MODE_ON of MODE_NORMAL for execution. + * + * @param actionId Action id of command to execute + */ + ReturnValue_t checkMode(ActionId_t actionId); + /** * @brief This function initializes the serial link ip protocol struct slipInfo. */ @@ -279,6 +318,11 @@ private: */ ReturnValue_t handleVersionTm(); + /** + * @brief Checks the loaded program by means of the version set + */ + ReturnValue_t checkProgram(); + /** * @brief Handles reply to request interface telemetry command. */ @@ -315,6 +359,11 @@ private: * @note Deserialization will be performed in little endian byte order */ uint64_t deserializeUint64(const uint8_t* buffer); + + /** + * @brief Handles the startup state machine + */ + void handleStartup(const uint8_t* parameterId); }; #endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */ diff --git a/bsp_q7s/devices/startracker/StrImageLoader.cpp b/bsp_q7s/devices/startracker/StrImageLoader.cpp index 6f306684..495ec3a8 100644 --- a/bsp_q7s/devices/startracker/StrImageLoader.cpp +++ b/bsp_q7s/devices/startracker/StrImageLoader.cpp @@ -272,7 +272,7 @@ ReturnValue_t StrImageLoader::sendAndRead(size_t size, uint32_t position) { ReturnValue_t StrImageLoader::checkReply() { uint8_t type = datalinkLayer.getReplyFrameType(); if (type != TMTC_ACTIONREPLY) { - sif::warning << "StrImageLoader::checkUploadReply: Received reply with invalid type ID" + sif::warning << "StrImageLoader::checkReply: Received reply with invalid type ID" << std::endl; triggerEvent(INVALID_TYPE_ID); return RETURN_FAILED; @@ -280,7 +280,7 @@ ReturnValue_t StrImageLoader::checkReply() { uint8_t status = datalinkLayer.getStatusField(); if (status != ArcsecDatalinkLayer::STATUS_OK) { triggerEvent(STATUS_ERROR); - sif::warning << "StrImageLoader::checkUploadReply: Status failure" << std::endl; + sif::warning << "StrImageLoader::checkReply: Status failure" << std::endl; return RETURN_FAILED; } return RETURN_OK; diff --git a/bsp_q7s/devices/startracker/StrImageLoader.h b/bsp_q7s/devices/startracker/StrImageLoader.h index 5945b7f5..50fe3e92 100644 --- a/bsp_q7s/devices/startracker/StrImageLoader.h +++ b/bsp_q7s/devices/startracker/StrImageLoader.h @@ -170,7 +170,12 @@ private: ReturnValue_t performImageUpload(); /** - * @bried Performs download of last taken image from the star tracker. + * @brief Performs download of last taken image from the star tracker. + * + * @details Download is split over multiple packets transporting each a maximum of 1024 bytes. + * In case the download of one position fails, the same packet will be again + * requested. If the download of the packet fails CONFIG_MAX_DOWNLOAD_RETRIES times, + * the download will be stopped. */ ReturnValue_t performImageDownload(); diff --git a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp index 6d020181..22356d0e 100644 --- a/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp +++ b/linux/fsfwconfig/pollingsequence/pollingSequenceFactory.cpp @@ -568,7 +568,6 @@ ReturnValue_t pst::pstI2c(FixedTimeslotTaskIF *thisSequence) { ReturnValue_t pst::pstUart(FixedTimeslotTaskIF *thisSequence) { // Length of a communication cycle uint32_t length = thisSequence->getPeriodMs(); - static_cast(length); bool uartPstEmpty = true; #if OBSW_ADD_PLOC_MPSOC == 1