500 lines
18 KiB
C++
500 lines
18 KiB
C++
#ifndef MISSION_DEVICES_STARTRACKERHANDLER_H_
|
|
#define MISSION_DEVICES_STARTRACKERHANDLER_H_
|
|
|
|
#include <fsfw/datapool/PoolReadGuard.h>
|
|
|
|
#include <thread>
|
|
|
|
#include "ArcsecJsonParamBase.h"
|
|
#include "OBSWConfig.h"
|
|
#include "StrComHandler.h"
|
|
#include "arcsec/common/SLIP.h"
|
|
#include "devices/powerSwitcherList.h"
|
|
#include "fsfw/devicehandlers/DeviceHandlerBase.h"
|
|
#include "fsfw/src/fsfw/serialize/SerializeAdapter.h"
|
|
#include "fsfw/timemanager/Countdown.h"
|
|
#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
|
|
#include "strJsonCommands.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,
|
|
const char* jsonFileStr, StrComHandler* strHelper,
|
|
power::Switch_t powerSwitch);
|
|
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;
|
|
|
|
static const Submode_t SUBMODE_BOOTLOADER = 1;
|
|
static const Submode_t SUBMODE_FIRMWARE = 2;
|
|
|
|
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);
|
|
|
|
static const size_t MAX_PATH_SIZE = 50;
|
|
static const size_t MAX_FILE_NAME = 30;
|
|
|
|
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 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;
|
|
|
|
// 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;
|
|
};
|
|
JsonConfigs jcfgs;
|
|
Countdown jcfgCountdown = Countdown(250);
|
|
bool commandExecuted = false;
|
|
std::thread jsonCfgTask;
|
|
static void setUpJsonCfgs(JsonConfigs& cfgs, const char* paramJsonFile);
|
|
|
|
std::string paramJsonFile;
|
|
|
|
enum class NormalState { TEMPERATURE_REQUEST, SOLUTION_REQUEST };
|
|
|
|
NormalState normalState = NormalState::TEMPERATURE_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,
|
|
LOGLEVEL,
|
|
LIMITS,
|
|
TRACKING,
|
|
MOUNTING,
|
|
IMAGE_PROCESSOR,
|
|
CAMERA,
|
|
BLOB,
|
|
CENTROIDING,
|
|
LISA,
|
|
MATCHING,
|
|
VALIDATION,
|
|
ALGO,
|
|
LOG_SUBSCRIPTION,
|
|
DEBUG_CAMERA,
|
|
WAIT_FOR_EXECUTION,
|
|
};
|
|
FwBootState bootState = FwBootState::NONE;
|
|
|
|
InternalState internalState = InternalState::IDLE;
|
|
|
|
bool strHelperHandlingSpecialRequest = false;
|
|
|
|
const power::Switch_t powerSwitch = power::NO_SWITCH;
|
|
|
|
/**
|
|
* @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);
|
|
|
|
/**
|
|
* @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);
|
|
|
|
/**
|
|
* @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 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 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 returnvalue::OK if successful, otherwise error return value
|
|
*/
|
|
ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset, size_t size);
|
|
|
|
/**
|
|
* @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();
|
|
};
|
|
|
|
#endif /* MISSION_DEVICES_STARTRACKERHANDLER_H_ */
|