star tracker improvements

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.
  */

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() {
-
+    switch(startupState) {
-#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
+    case StartupState::IDLE:
-//    setMode(MODE_NORMAL);
+        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<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::TRACKING:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::TRACKING;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::MOUNTING:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::MOUNTING;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::CAMERA:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::CAMERA;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::BLOB:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::BLOB;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::CENTROIDING:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::CENTROIDING;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::LISA:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::LISA;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::MATCHING:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::MATCHING;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::VALIDATION:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::VALIDATION;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(paramJsonFile.c_str()),
+                paramJsonFile.size());
+    case StartupState::ALGO:
+        startupState = StartupState::WAIT_FOR_EXECUTION;
+        *id = StarTracker::ALGO;
+        return buildCommandFromCommand(*id, reinterpret_cast<const uint8_t*>(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<unsigned int>(*(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<unsigned int>(status) << std::endl;
+    sif::info << "StarTracker: Ping status: "<< static_cast<unsigned int>(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<unsigned int>(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<uint64_t>(*(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;
+    }
+    }
+}

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 <fsfw/datapool/PoolReadGuard.h>
 #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_ */

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;

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();
 

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<void>(length);
     bool uartPstEmpty = true;
 
 #if OBSW_ADD_PLOC_MPSOC == 1