star tracker improvements

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