eive-obsw/mission/acs/str/StarTrackerHandler.h

#ifndef MISSION_DEVICES_STARTRACKERHANDLER_H_
#define MISSION_DEVICES_STARTRACKERHANDLER_H_

#include <fsfw/datapool/PoolReadGuard.h>
#include <linux/acs/StrComHandler.h>
#include <mission/acs/str/ArcsecJsonParamBase.h>
#include <mission/acs/str/strHelpers.h>
#include <mission/acs/str/strJsonCommands.h>
#include <thirdparty/sagittactl/wire/common/genericstructs.h>

#include <set>
#include <thread>

#include "OBSWConfig.h"
#include "devices/powerSwitcherList.h"
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
#include "fsfw/timemanager/Countdown.h"

extern "C" {
#include <wire/common/genericstructs.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
 * @note The STR code is a chaotic inconsistent mess and should be re-written with a simpler base
 *       class. DO NOT USE THIS AS REFERENCE. Stay away from it.
 * @author	J. Meier, R. Mueller
 */
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,
                     StrComHandler* strHelper, power::Switch_t powerSwitch,
                     startracker::SdCardConfigPathGetter& cfgPathGetter);
  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;

  Submode_t getInitialSubmode() override;

 protected:
  void doStartUp() override;
  void doShutDown() 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 isModeCombinationValid(Mode_t mode, Submode_t submode) 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 theSerialComIF.
   */
  virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand) override;
  virtual ReturnValue_t doSendReadHook() override;
  ReturnValue_t getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) override;
  virtual void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override;

 private:
  static const uint8_t INTERFACE_ID = CLASS_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(0xA2);
  //! [EXPORT] : [COMMENT] Status in interface reply signals error
  static const ReturnValue_t INTERFACE_REQ_FAILED = MAKE_RETURN_CODE(0xA3);
  //! [EXPORT] : [COMMENT] Status in power reply signals error
  static const ReturnValue_t POWER_REQ_FAILED = MAKE_RETURN_CODE(0xA4);
  //! [EXPORT] : [COMMENT] Status of reply to parameter set command signals error
  static const ReturnValue_t SET_PARAM_FAILED = MAKE_RETURN_CODE(0xA5);
  //! [EXPORT] : [COMMENT] Status of reply to action command signals error
  static const ReturnValue_t ACTION_FAILED = MAKE_RETURN_CODE(0xA6);
  //! [EXPORT] : [COMMENT] Received invalid path string. Exceeds allowed length
  static const ReturnValue_t FILE_PATH_TOO_LONG = MAKE_RETURN_CODE(0xA7);
  //! [EXPORT] : [COMMENT] Name of file received with command is too long
  static const ReturnValue_t FILENAME_TOO_LONG = MAKE_RETURN_CODE(0xA8);
  //! [EXPORT] : [COMMENT] Received version reply with invalid program ID
  static const ReturnValue_t INVALID_PROGRAM = MAKE_RETURN_CODE(0xA9);
  //! [EXPORT] : [COMMENT] Status field reply signals error
  static const ReturnValue_t REPLY_ERROR = MAKE_RETURN_CODE(0xAA);
  //! [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(0xAB);
  //! [EXPORT] : [COMMENT] Received command with invalid length (too few or too many parameters)
  static const ReturnValue_t INVALID_LENGTH = MAKE_RETURN_CODE(0xAC);
  //! [EXPORT] : [COMMENT] Region mismatch between send and received data
  static const ReturnValue_t REGION_MISMATCH = MAKE_RETURN_CODE(0xAD);
  //! [EXPORT] : [COMMENT] Address mismatch between send and received data
  static const ReturnValue_t ADDRESS_MISMATCH = MAKE_RETURN_CODE(0xAE);
  //! [EXPORT] : [COMMENT] Length field mismatch between send and received data
  static const ReturnValue_t lENGTH_MISMATCH = MAKE_RETURN_CODE(0xAF);
  //! [EXPORT] : [COMMENT] Specified file does not exist
  static const ReturnValue_t FILE_NOT_EXISTS = MAKE_RETURN_CODE(0xB0);
  //! [EXPORT] : [COMMENT] Download blob pixel command has invalid type field
  static const ReturnValue_t INVALID_TYPE = MAKE_RETURN_CODE(0xB1);
  //! [EXPORT] : [COMMENT] Received FPGA action command with invalid ID
  static const ReturnValue_t INVALID_ID = MAKE_RETURN_CODE(0xB2);
  //! [EXPORT] : [COMMENT] Received reply is too short
  static const ReturnValue_t REPLY_TOO_SHORT = MAKE_RETURN_CODE(0xB3);
  //! [EXPORT] : [COMMENT] Received reply with invalid CRC
  static const ReturnValue_t CRC_FAILURE = MAKE_RETURN_CODE(0xB4);
  //! [EXPORT] : [COMMENT] Star tracker handler currently executing a command and using the
  //! communication interface
  static const ReturnValue_t STR_HELPER_EXECUTING = MAKE_RETURN_CODE(0xB5);
  //! [EXPORT] : [COMMENT] Star tracker is already in firmware mode
  static const ReturnValue_t STARTRACKER_ALREADY_BOOTED = MAKE_RETURN_CODE(0xB6);
  //! [EXPORT] : [COMMENT] Star tracker must be in firmware mode to run this command
  static const ReturnValue_t STARTRACKER_NOT_RUNNING_FIRMWARE = MAKE_RETURN_CODE(0xB7);
  //! [EXPORT] : [COMMENT] Star tracker must be in bootloader mode to run this command
  static const ReturnValue_t STARTRACKER_NOT_RUNNING_BOOTLOADER = MAKE_RETURN_CODE(0xB8);

  static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::STR_HANDLER;

  //! [EXPORT] : [COMMENT] Failed to boot firmware
  static const Event BOOTING_FIRMWARE_FAILED_EVENT = MAKE_EVENT(1, severity::LOW);
  //! [EXPORT] : [COMMENT] Failed to boot star tracker into bootloader mode
  static const Event BOOTING_BOOTLOADER_FAILED_EVENT = MAKE_EVENT(2, severity::LOW);
  //! [EXPORT] : [COMMENT] Received COM error. P1:  Communication Error ID (datasheet p32)
  static constexpr Event COM_ERROR_REPLY_RECEIVED = MAKE_EVENT(3, severity::LOW);

  static const uint8_t STATUS_OFFSET = 2;
  static const uint8_t PARAMS_OFFSET = 2;
  static const uint8_t TICKS_OFFSET = 3;
  static const uint8_t TIME_OFFSET = 7;
  static const uint8_t TM_PARAM_OFFSET = 15;
  static const uint8_t PARAMETER_ID_OFFSET = 1;
  static const uint8_t ACTION_ID_OFFSET = 1;
  static const uint8_t ACTION_DATA_OFFSET = 3;

  // 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;
  static const uint32_t DEFAULT_TRANSITION_DELAY = 15000;

  struct FlashReadCmd {
    // Minimum length of a read command (region, length and filename)
    static const size_t MIN_LENGTH = 7;
  };

  struct ChecksumCmd {
    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;

  MessageQueueIF* eventQueue = nullptr;

  startracker::TemperatureSet temperatureSet;
  startracker::VersionSet versionSet;
  startracker::PowerSet powerSet;
  startracker::InterfaceSet interfaceSet;
  startracker::TimeSet timeSet;
  startracker::SolutionSet solutionSet;
  startracker::HistogramSet histogramSet;
  startracker::ChecksumSet checksumSet;
  startracker::CameraSet cameraSet;
  startracker::LimitsSet limitsSet;
  startracker::LogLevelSet loglevelSet;
  startracker::MountingSet mountingSet;
  startracker::ImageProcessorSet imageProcessorSet;
  startracker::CentroidingSet centroidingSet;
  startracker::LisaSet lisaSet;
  startracker::MatchingSet matchingSet;
  startracker::TrackingSet trackingSet;
  startracker::ValidationSet validationSet;
  startracker::AlgoSet algoSet;
  startracker::SubscriptionSet subscriptionSet;
  startracker::LogSubscriptionSet logSubscriptionSet;
  startracker::DebugCameraSet debugCameraSet;
  startracker::AutoBlobSet autoBlobSet;
  startracker::MatchedCentroidsSet matchedCentroidsSet;
  startracker::BlobSet blobSet;
  startracker::BlobsSet blobsSet;
  startracker::CentroidSet centroidSet;
  startracker::CentroidsSet centroidsSet;
  startracker::ContrastSet contrastSet;

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

  uint8_t commandBuffer[startracker::MAX_FRAME_SIZE];

  // Countdown to insert delay for star tracker to switch from bootloader to firmware program
  // Loading firmware requires some time and the command will not trigger a reply when executed
  Countdown bootCountdown;

  struct JsonConfigs {
    Tracking tracking;
    LogLevel logLevel;
    LogSubscription logSubscription;
    DebugCamera debugCamera;
    Algo algo;
    Validation validation;
    Matching matching;
    Lisa lisa;
    Centroiding centroiding;
    Camera camera;
    ImageProcessor imageProcessor;
    Mounting mounting;
    Limits limits;
    Subscription subscription;
    AutoThreshold autoThreshold;
  };
  bool jcfgPending = false;
  JsonConfigs jcfgs;
  Countdown jcfgCountdown = Countdown(1000);
  bool commandExecuted = false;
  std::thread jsonCfgTask;
  static void setUpJsonCfgs(JsonConfigs& cfgs, std::string paramJsonFile);

  std::string paramJsonFile;

  enum class NormalState { SECONDARY_REQUEST, SOLUTION_REQUEST };

  NormalState normalState = NormalState::SECONDARY_REQUEST;

  enum class StartupState {
    IDLE,
    CHECK_PROGRAM,
    WAIT_CHECK_PROGRAM,
    BOOT_BOOTLOADER,
    WAIT_JCFG,
    DONE
  };
  StartupState startupState = StartupState::IDLE;

  enum class InternalState {
    IDLE,
    BOOT_FIRMWARE,
    DONE,
    FAILED_FIRMWARE_BOOT,
    BOOT_BOOTLOADER,
    BOOTLOADER_CHECK,
    FAILED_BOOTLOADER_BOOT
  };

  enum class FwBootState {
    NONE,
    BOOT_DELAY,
    REQ_VERSION,
    VERIFY_BOOT,
    SET_TIME,
    LOGLEVEL,
    LIMITS,
    TRACKING,
    MOUNTING,
    IMAGE_PROCESSOR,
    CAMERA,
    BLOB,
    CENTROIDING,
    LISA,
    MATCHING,
    VALIDATION,
    ALGO,
    LOG_SUBSCRIPTION,
    DEBUG_CAMERA,
    AUTO_THRESHOLD,
    WAIT_FOR_EXECUTION,
    RETRY_CFG_CMD
  };
  FwBootState bootState = FwBootState::NONE;

  InternalState internalState = InternalState::IDLE;

  bool reinitNextSetParam = false;

  bool strHelperHandlingSpecialRequest = false;

  const power::Switch_t powerSwitch = power::NO_SWITCH;

  size_t fullPacketLen = 0;

  std::set<DeviceCommandId_t> additionalRequestedTm{};
  std::set<DeviceCommandId_t>::iterator currentSecondaryTmIter;

  startracker::SdCardConfigPathGetter& cfgPathGetter;

  /**
   * @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(uint8_t replyId, DeviceCommandId_t* foundId);
  ReturnValue_t scanForSetParameterReply(uint8_t replyId, DeviceCommandId_t* foundId);
  ReturnValue_t scanForGetParameterReply(uint8_t replyId, DeviceCommandId_t* foundId);
  ReturnValue_t scanForTmReply(uint8_t replyId, 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   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  returnvalue::OK if start of execution was successful, otherwise error return value
   */
  ReturnValue_t executeFlashReadCommand(const uint8_t* commandData, size_t commandDataLen);

  /**
   * Add a TM request to the list of telemetry which will be polled in the secondary step of
   * the device communication.
   * @param cmd
   */
  void addSecondaryTmForNormalMode(DeviceCommandId_t cmd);

  /**
   * Reset the secondary set, which will only contain a TEMPERATURE set request after the reset.
   */
  void resetSecondaryTmSet();

  /**
   * @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 get the checksum of a flash part
   */
  ReturnValue_t prepareChecksumCommand(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 prepareSwitchToBootloaderCmd();

  /**
   * @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();

  /**
   * @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  returnvalue::OK if successful, otherwise error return Value
   */
  ReturnValue_t prepareParamCommand(const uint8_t* commandData, size_t commandDataLen,
                                    ArcsecJsonParamBase& paramSet, bool reinitSet);

  /**
   * @brief   The following function will fill the command buffer with the command to request
   *          a parameter set.
   */
  ReturnValue_t prepareRequestCameraParams();
  ReturnValue_t prepareRequestLimitsParams();
  ReturnValue_t prepareRequestLogLevelParams();
  ReturnValue_t prepareRequestMountingParams();
  ReturnValue_t prepareRequestImageProcessorParams();
  ReturnValue_t prepareRequestCentroidingParams();
  ReturnValue_t prepareRequestLisaParams();
  ReturnValue_t prepareRequestMatchingParams();
  ReturnValue_t prepareRequestAutoBlobTm();
  ReturnValue_t prepareRequestMatchedCentroidsTm();
  ReturnValue_t prepareRequestBlobTm();
  ReturnValue_t prepareRequestBlobsTm();
  ReturnValue_t prepareRequestCentroidTm();
  ReturnValue_t prepareRequestCentroidsTm();
  ReturnValue_t prepareRequestContrastTm();
  ReturnValue_t prepareRequestTrackingParams();
  ReturnValue_t prepareRequestValidationParams();
  ReturnValue_t prepareRequestAlgoParams();
  ReturnValue_t prepareRequestSubscriptionParams();
  ReturnValue_t prepareRequestLogSubscriptionParams();
  ReturnValue_t prepareRequestDebugCameraParams();

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

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

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

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

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

  ReturnValue_t handlePingReply(const uint8_t* rawFrame);

  ReturnValue_t handleParamRequest(const uint8_t* rawFrame, 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(uint8_t tmType, uint8_t parameterId);

  ReturnValue_t statusFieldCheck(const uint8_t* rawFrame);
  /**
   * @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  returnvalue::OK if successful, otherwise error return value
   */
  ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
                         const char* context);

  ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame);
  ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
  ReturnValue_t handleBlobTm(const uint8_t* rawFrame);
  ReturnValue_t handleBlobsTm(const uint8_t* rawFrame);
  ReturnValue_t handleCentroidTm(const uint8_t* rawFrame);
  ReturnValue_t handleCentroidsTm(const uint8_t* rawFrame);

  /**
   * @brief Checks if star tracker is in valid mode for executing the received command.
   *
   * @param actioId Id of received command
   *
   * @return    returnvalue::OK if star tracker is in valid mode, otherwise error return value
   */
  ReturnValue_t checkCommand(ActionId_t actionId);

  void doOnTransition(Submode_t subModeFrom);
  void doNormalTransition(Mode_t modeFrom, Submode_t subModeFrom);
  void bootFirmware(Mode_t toMode);
  void bootBootloader();
  bool reloadJsonCfgFile();
  ReturnValue_t acceptExternalDeviceCommands() override;
};

#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */