eive-obsw/bsp_q7s/devices/startracker/StarTrackerHandler.h

#ifndef MISSION_DEVICES_STARTRACKERHANDLER_H_
#define MISSION_DEVICES_STARTRACKERHANDLER_H_

#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 "ArcsecDatalinkLayer.h"
#include "StarTrackerDefinitions.h"
#include "ArcsecJsonParamBase.h"
#include "StrHelper.h"

/**
 * @brief	This is the device handler for the star tracker from arcsec.
 *
 * @details Datasheet: https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_IRS/
 *                     Arbeitsdaten/08_Used%20Components/ArcSec_KULeuven_Startracker/
 *                     Sagitta%201.0%20Datapack&fileid=659181
 * @author	J. Meier
 */
class StarTrackerHandler: public DeviceHandlerBase {
public:

	/**
	 * @brief Constructor
	 *
	 * @param objectId
	 * @param comIF
	 * @param comCookie
	 * @param gpioComIF	Pointer to gpio communication interface
	 * @param enablePin 	GPIO connected to the enable pin of the reaction wheels. Must be pulled
	 * 						to high to enable the device.
	 */
    StarTrackerHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie,
            StrHelper* strHelper);
	virtual ~StarTrackerHandler();

	ReturnValue_t initialize() override;

	/**
	 * @brief   Overwrite this function from DHB to handle commands executed by the str image
	 *          loader task.
	 */
    ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
            const uint8_t* data, size_t size) override;

    void performOperationHook() override;

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;
	ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand,
				const uint8_t * commandData,size_t commandDataLen) override;
	ReturnValue_t scanForReply(const uint8_t *start, size_t remainingSize,
			DeviceCommandId_t *foundId, size_t *foundLen) override;
	ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
			const uint8_t *packet) override;
	void setNormalDatapoolEntriesInvalid() override;
	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:

	static const uint8_t INTERFACE_ID = CLASS_ID::STR_HANDLER;

	//! [EXPORT] : [COMMENT] Received reply is too short
    static const ReturnValue_t REPLY_TOO_SHORT = MAKE_RETURN_CODE(0xB0);
	//! [EXPORT] : [COMMENT] Received reply with invalid CRC
    static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xB1);
	//! [EXPORT] : [COMMENT] Image loader executing
    static const ReturnValue_t IMAGE_LOADER_EXECUTING = MAKE_RETURN_CODE(0xB2);

    static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER;

    //! [EXPORT] : [COMMENT] Status in temperature reply signals error
    static const ReturnValue_t TEMPERATURE_REQ_FAILED = MAKE_RETURN_CODE(0xA0);
    //! [EXPORT] : [COMMENT] Ping command failed
    static const ReturnValue_t PING_FAILED = MAKE_RETURN_CODE(0xA1);
    //! [EXPORT] : [COMMENT] Status in version reply signals error
    static const ReturnValue_t VERSION_REQ_FAILED = MAKE_RETURN_CODE(0xA3);
    //! [EXPORT] : [COMMENT] Status in interface reply signals error
    static const ReturnValue_t INTERFACE_REQ_FAILED = MAKE_RETURN_CODE(0xA4);
    //! [EXPORT] : [COMMENT] Status in power reply signals error
    static const ReturnValue_t POWER_REQ_FAILED = MAKE_RETURN_CODE(0xA5);
    //! [EXPORT] : [COMMENT] Status of reply to parameter set command signals error
    static const ReturnValue_t SET_PARAM_FAILED = MAKE_RETURN_CODE(0xA6);
    //! [EXPORT] : [COMMENT] Status of reply to action command signals error
    static const ReturnValue_t ACTION_FAILED = MAKE_RETURN_CODE(0xA7);
    //! [EXPORT] : [COMMENT] Received upload image command with invalid length
    static const ReturnValue_t UPLOAD_TOO_SHORT = MAKE_RETURN_CODE(0xA8);
    //! [EXPORT] : [COMMENT] Received upload image command with invalid position field
    static const ReturnValue_t UPLOAD_INVALID_POSITION = MAKE_RETURN_CODE(0xA8);
    //! [EXPORT] : [COMMENT] Position value in upload image reply not matching sent position
    static const ReturnValue_t UPLOAD_IMAGE_FAILED = MAKE_RETURN_CODE(0xA9);
    //! [EXPORT] : [COMMENT] Received upload image command with invalid length
    static const ReturnValue_t INVALID_UPLOAD_COMMAND = MAKE_RETURN_CODE(0xAA);
    //! [EXPORT] : [COMMENT] Received invalid path string. Exceeds allowed length
    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);
    //! [EXPORT] : [COMMENT] Status field reply signals error
    static const ReturnValue_t REPLY_ERROR = MAKE_RETURN_CODE(0xAE);
    //! [EXPORT] : [COMMENT] Status field of contrast reply signals error
    static const ReturnValue_t CONTRAST_REQ_FAILED = MAKE_RETURN_CODE(0xAE);
    //! [EXPORT] : [COMMENT] Received command which is too short (some data is missing for proper execution)
    static const ReturnValue_t COMMAND_TOO_SHORT = MAKE_RETURN_CODE(0xAF);
    //! [EXPORT] : [COMMENT] Received command with invalid length (too few or too many parameters)
    static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xB0);
    //! [EXPORT] : [COMMENT] Region mismatch between send and received data
    static const ReturnValue_t REGION_MISMATCH = MAKE_RETURN_CODE(0xB1);
    //! [EXPORT] : [COMMENT] Address mismatch between send and received data
    static const ReturnValue_t ADDRESS_MISMATCH = MAKE_RETURN_CODE(0xB2);
    //! [EXPORT] : [COMMENT] Length field mismatch between send and received data
    static const ReturnValue_t lENGTH_MISMATCH = MAKE_RETURN_CODE(0xB3);
    //! [EXPORT] : [COMMENT] Specified file does not exist
    static const ReturnValue_t FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xB4);
    //! [EXPORT] : [COMMENT] Reply to upload centroid does not match commanded centroid id
    static const ReturnValue_t UPLOAD_CENTROID_ID_MISMATCH = MAKE_RETURN_CODE(0xB5);
    //! [EXPORT] : [COMMENT] Download blob pixel command has invalid type field
    static const ReturnValue_t INVALID_TYPE = MAKE_RETURN_CODE(0xB6);
    //! [EXPORT] : [COMMENT] Received FPGA action command with invalid ID
    static const ReturnValue_t INVALID_ID = MAKE_RETURN_CODE(0xB7);

    static const size_t MAX_PATH_SIZE = 50;
    static const size_t MAX_FILE_NAME = 30;

    // position (uint32) + 1024 image data
    static const size_t UPLOAD_COMMAND_LEN = 1028;
    // Max valid position value in upload image command
    static const uint16_t MAX_POSITION= 4095;
    static const uint8_t STATUS_OFFSET = 1;
    static const uint8_t PARAMS_OFFSET = 1;
    static const uint8_t TICKS_OFFSET = 2;
    static const uint8_t TIME_OFFSET = 6;
    static const uint8_t TM_DATA_FIELD_OFFSET = 14;
    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 uint32_t MUTEX_TIMEOUT = 20;
    static const uint32_t BOOT_TIMEOUT = 1000;

    class WriteCmd {
    public:
        static const uint8_t ADDRESS_OFFSET = 1;
        static const uint8_t FILE_OFFSET = 5;
        // Minimum length of a write command (region, address and filename)
        static const size_t MIN_LENGTH = 7;
    };

    class ReadCmd {
    public:
        static const uint8_t ADDRESS_OFFSET = 1;
        static const uint8_t LENGTH_OFFSET = 5;
        static const uint8_t FILE_OFFSET = 9;
        // Minimum length of a read command (region, address, length and filename)
        static const size_t MIN_LENGTH = 11;
    };

    class EraseCmd {
    public:
        static const uint8_t LENGTH = 1;
        uint8_t rememberRegion = 0;
    };

    EraseCmd eraseCmd;

    class UnlockCmd {
    public:
        static const uint8_t CODE_OFFSET = 1;
    };

    class ChecksumCmd {
    public:
        static const uint8_t ADDRESS_OFFSET = 1;
        static const uint8_t LENGTH_OFFSET = 5;
        // Length of checksum command
        static const size_t LENGTH = 9;
        uint8_t rememberRegion = 0;
        uint32_t rememberAddress = 0;
        uint32_t rememberLength = 0;
    };

    ChecksumCmd checksumCmd;

    class SetTimeCmd {
    public:
        static const uint8_t LENGTH = 8;
    };

    class DownloadCentroidCmd {
    public:
        static const uint8_t LENGTH = 1;
    };

    class UploadCentroid {
    public:
        uint8_t rememberId = 0;
    };

    UploadCentroid uploadCentroid;

    class DownloadMatchedStarCmd {
    public:
        static const uint8_t LENGTH = 1;
    };

    class DownloadDbImageCmd {
    public:
        static const uint8_t LENGTH = 1;
    };

    class DownloadBlobPixCmd {
    public:
        static const uint8_t LENGTH = 2;
        static const uint8_t NORMAL = 0;
        static const uint8_t FAST = 1;
    };

    class FpgaDownloadCmd {
    public:
        static const uint8_t MIN_LENGTH = 10;
    };

    class FpgaActionCmd {
    public:
        static const uint8_t LENGTH = 1;
        static const uint8_t ID = 3;
    };

    MessageQueueIF* eventQueue = nullptr;

    ArcsecDatalinkLayer dataLinkLayer;

    StarTracker::TemperatureSet temperatureSet;
    StarTracker::VersionSet versionSet;
    StarTracker::PowerSet powerSet;
    StarTracker::InterfaceSet interfaceSet;
    StarTracker::TimeSet timeSet;
    StarTracker::SolutionSet solutionSet;
    StarTracker::HistogramSet histogramSet;
    StarTracker::ContrastSet contrastSet;
    StarTracker::ChecksumSet checksumSet;
    StarTracker::DownloadCentroidSet downloadCentroidSet;
    StarTracker::DownloadMatchedStar downloadMatchedStar;
    StarTracker::DownloadDBImage downloadDbImage;
    StarTracker::DownloadBlobPixel downloadBlobPixel;
    StarTracker::CameraSet cameraSet;

    // Pointer to object responsible for uploading and downloading images to/from the star tracker
    StrHelper* strHelper = nullptr;

	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 strHelperExecuting = 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.
	 */
	void slipInit();

	ReturnValue_t scanForActionReply(DeviceCommandId_t *foundId);
	ReturnValue_t scanForSetParameterReply(DeviceCommandId_t *foundId);
	ReturnValue_t scanForGetParameterReply(DeviceCommandId_t *foundId);
	ReturnValue_t scanForTmReply(DeviceCommandId_t *foundId);

	/**
	 * @brief   Fills command buffer with data to ping the star tracker
	 */
	void preparePingRequest();

	/**
	 * @brief   Fills command buffer with data to request the time telemetry.
	 */
	void prepareTimeRequest();

	/**
	 * @brief    Handles all received event messages
	 */
	void handleEvent(EventMessage* eventMessage);

	/**
	 * @brief   Executes the write command
	 *
	 * @param commandData       Pointer to received command data
	 * @param commandDataLen    Size of received command data
	 *
	 * @return  RETURN_OK if start of execution was successful, otherwise error return value
	 */
	ReturnValue_t executeWriteCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Starts the execution of the fpga download command
	 *
	 * @param commandData       Pointer to buffer with command data
	 * @param commandDataLen    Size of received command
	 */
	ReturnValue_t executeFpgaDownloadCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
     * @brief   Extracts information for flash-read-command from TC data and starts execution of
     *          flash-read-procedure
     *
     * @param commandData       Pointer to received command data
     * @param commandDataLen    Size of received command data
     *
     * @return  RETURN_OK if start of execution was successful, otherwise error return value
     */
    ReturnValue_t executeReadCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Fills command buffer with data to boot image (works only when star tracker is
	 *          in bootloader mode).
	 */
	void prepareBootCommand();

	/**
	 * @brief   Fills command buffer with command to erase a flash region
	 */
	ReturnValue_t prepareEraseCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Fills command buffer with command to unlock flash region
	 */
	ReturnValue_t prepareUnlockCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Fills command buffer with command to get the checksum of a flash part
	 */
	ReturnValue_t prepareChecksumCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Fills command buffer with command to set the unix time
	 */
	ReturnValue_t prepareSetTimeCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Fills command buffer with command to request a centroid
	 */
	ReturnValue_t prepareDownloadCentroidCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Fills command buffer with command to upload a centroid for testing purpose
	 */
	ReturnValue_t prepareUploadCentroidCommand(const uint8_t* commandData, size_t commandDataLen);

	/**
	 * @brief   Fills the command buffer with the command to take an image.
	 */
	void prepareTakeImageCommand(const uint8_t* commandData);

	/**
	 * @brief   Fills command buffer with data to request the version telemetry packet
	 */
	void prepareVersionRequest();

	/**
	 * @brief   Fills the command buffer with data to request the interface telemetry packet.
	 */
	void prepareInterfaceRequest();

	/**
	 * @brief   Fills the command buffer with data to request the power telemetry packet.
	 */
	void preparePowerRequest();

	/**
	 * @brief   Fills command buffer with data to reboot star tracker.
	 */
	void prepareRebootCommand();

	/**
	 * @brief   Fills command buffer with data to subscribe to a telemetry packet.
	 *
	 * @param tmId  The ID of the telemetry packet to subscribe to
	 */
	void prepareSubscriptionCommand(const uint8_t* tmId);

	/**
     * @brief   Fills command buffer with data to request solution telemtry packet (contains
     *          attitude information)
     */
	void prepareSolutionRequest();

	/**
     * @brief   Fills command buffer with data to request temperature from star tracker
     */
	void prepareTemperatureRequest();

	/**
     * @brief   Fills command buffer with data to request histogram
     */
	void prepareHistogramRequest();

	void prepareContrastRequest();

	/**
	 * @brief   Fills command buffer with command to reset the error signal of the star tracker
	 */
	void prepareErrorResetRequest();

	/**
	 * @brief   Reads parameters from json file specified by string in commandData and
	 *          prepares the command to apply the parameter set to the star tracker
	 *
	 * @param commandData       Contains string with file name
	 * @param commandDataLen    Length of command
	 * @param paramSet          The object defining the command generation
	 *
	 * @return  RETURN_OK if successful, otherwise error return Value
	 */
    ReturnValue_t prepareParamCommand(const uint8_t* commandData, size_t commandDataLen,
            ArcsecJsonParamBase& paramSet);

    /**
     * @brief   Fills command buffer with data to request matched star.
     */
    ReturnValue_t prepareDownloadMatchedStarCommand(const uint8_t* commandData,
            size_t commandDataLen);

    /**
     * @brief   Fills command buffer with data to request matched star coordinates.
     */
    ReturnValue_t prepareDownloadDbImageCommand(const uint8_t* commandData,
            size_t commandDataLen);

    /**
     * @brief   Fills command buffer with data to request output of the blob filter algorithm.
     */
    ReturnValue_t prepareDownloadBlobPixelCommand(const uint8_t* commandData,
            size_t commandDataLen);

    /**
     * @brief   With this command the FPGA update will be applied to the star tracker
     */
    ReturnValue_t prepareFpgaActionCommand(const uint8_t* commandData, size_t commandDataLen);

    /**
     * @brief   Will fill the command buffer with the command to request the set camera parameters.
     */
    ReturnValue_t prepareRequestCameraParams();

    /**
     * @brief   Handles action replies with datasets.
     */
    ReturnValue_t handleActionReplySet(LocalPoolDataSetBase& dataset, size_t size);

	/**
	 * @brief   Default function to handle action replies
	 */
	ReturnValue_t handleActionReply();

	/**
	 * @brief   Handles reply to upload centroid command
	 */
	ReturnValue_t handleUploadCentroidReply();

	/**
	 * @brief   Handles reply to erase command
	 */
	ReturnValue_t handleEraseReply();

	/**
	 * @brief   Handles reply to checksum command
	 */
	ReturnValue_t handleChecksumReply();

	/**
	 * @brief   Handles all set parameter replies
	 */
	ReturnValue_t handleSetParamReply();

	ReturnValue_t handlePingReply();

	ReturnValue_t handleParamRequest(LocalPoolDataSetBase& dataset, size_t size);

	/**
	 * @brief   Checks the loaded program by means of the version set
	 */
	ReturnValue_t checkProgram();

	/**
	 * @brief   Handles the startup state machine
	 */
	void handleStartup(const uint8_t* parameterId);

	/**
	 * @brief   Handles telemtry replies and fills the appropriate dataset
	 *
	 * @param dataset   Dataset where reply data will be written to
	 * @param size      Size of the dataset
	 *
	 * @return  RETURN_OK if successful, otherwise error return value
	 */
	ReturnValue_t handleTm(LocalPoolDataSetBase& dataset, size_t size);
};

#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */