From 9a55eb5508eb52be30fef5ccb736451592a017da Mon Sep 17 00:00:00 2001 From: "Jakob.Meier" Date: Wed, 17 Mar 2021 11:14:48 +0100 Subject: [PATCH] IMQT wip --- fsfwconfig/OBSWConfig.h | 3 +- fsfwconfig/returnvalues/classIds.h | 1 + mission/devices/IMTQHandler.cpp | 241 ++++++++++++++++++ mission/devices/IMTQHandler.h | 79 ++++++ .../IMTQHandlerDefinitions.h | 110 ++++++++ 5 files changed, 433 insertions(+), 1 deletion(-) create mode 100644 mission/devices/IMTQHandler.cpp create mode 100644 mission/devices/IMTQHandler.h create mode 100644 mission/devices/devicedefinitions/IMTQHandlerDefinitions.h diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index 5a9106b5..c64e1a56 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -25,7 +25,8 @@ debugging. */ #define PDU1_DEBUG 0 #define PDU2_DEBUG 0 #define ACU_DEBUG 0 -#define SYRLINKS_DEBUG 1 +#define SYRLINKS_DEBUG 0 +#define IMQT_DEBUG 1 #include "OBSWVersion.h" diff --git a/fsfwconfig/returnvalues/classIds.h b/fsfwconfig/returnvalues/classIds.h index 51412cbf..9a47ea19 100644 --- a/fsfwconfig/returnvalues/classIds.h +++ b/fsfwconfig/returnvalues/classIds.h @@ -19,6 +19,7 @@ enum { HEATER_HANDLER, SA_DEPL_HANDLER, SYRLINKS_HANDLER, + IMQT_HANDLER, }; } diff --git a/mission/devices/IMTQHandler.cpp b/mission/devices/IMTQHandler.cpp new file mode 100644 index 00000000..7ebeff05 --- /dev/null +++ b/mission/devices/IMTQHandler.cpp @@ -0,0 +1,241 @@ +#include "IMTQHandler.h" + +#include +#include +#include +#include + +IMTQHandler::IMTQHandler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) : + DeviceHandlerBase(objectId, comIF, comCookie), engHkDataset(this) { + if (comCookie == NULL) { + sif::error << "IMTQHandler: Invalid com cookie" << std::endl; + } +} + +IMTQHandler::~IMTQHandler() { +} + + +void IMTQHandler::doStartUp(){ + if(mode == _MODE_START_UP){ + setMode(MODE_ON); + } +} + +void IMTQHandler::doShutDown(){ + +} + +ReturnValue_t IMTQHandler::buildNormalDeviceCommand( + DeviceCommandId_t * id) { + *id = IMTQ::GET_ENG_HK_DATA; + return buildCommandFromCommand(*id, NULL, 0); +} + +ReturnValue_t IMTQHandler::buildTransitionDeviceCommand( + DeviceCommandId_t * id){ + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t IMTQHandler::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t * commandData, + size_t commandDataLen) { + switch(deviceCommand) { + case(IMTQ::GET_ENG_HK_DATA): { + commandBuffer[0] = IMTQ::CC::GET_ENG_HK_DATA; + rawPacket = commandBuffer; + return RETURN_OK; + } + case(IMTQ::START_ACTUATION_DIPOLE): { + commandBuffer[0] = IMTQ::CC::START_ACTUATION_DIPOLE; + commandBuffer[1] = *(commandData + 1); + commandBuffer[2] = *(commandData + 2); + rawPacket = commandBuffer; + return RETURN_OK; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_FAILED; +} + +void IMTQHandler::fillCommandAndReplyMap() { + this->insertInCommandAndReplyMap(IMTQ::GET_ENG_HK_DATA, 1, &engHkDataset, + IMTQ::SIZE_ENG_HK_COMMAND, false, true, IMTQ::SIZE_ENG_HK_DATA); +} + +ReturnValue_t IMTQHandler::scanForReply(const uint8_t *start, + size_t remainingSize, DeviceCommandId_t *foundId, size_t *foundLen) { + + ReturnValue_t result = RETURN_OK; + + switch(*start) { + case(IMTQ::CC::GET_ENG_HK_DATA): + *foundLen = IMTQ::SIZE_ENG_HK_DATA_REPLY; + *foundId = IMTQ::GET_ENG_HK_DATA; + break; + default: + sif::debug << "IMTQHandler::scanForReply: Reply contains invalid command code" << std::endl; + result = IGNORE_REPLY_DATA; + break; + } + + return result; +} + +ReturnValue_t IMTQHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + + ReturnValue_t result = RETURN_OK; + + result = parseStatusByte(packet); + + if (result != RETURN_OK) { + return result; + } + + switch (id) { + case (IMTQ::GET_ENG_HK_DATA): + fillEngHkDataset(packet); + break; + default: { + sif::debug << "IMTQHandler::interpretDeviceReply: Unknown device reply id" << std::endl; + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + } + + return RETURN_OK; +} + +ReturnValue_t IMTQHandler::parseStatusByte(const uint8_t* packet) { + uint8_t cmdErrorField = *(packet + 1) & 0xF; + switch (cmdErrorField) { + case 0: + return RETURN_OK; + case 1: + return REJECTED_WITHOUT_REASON; + case 2: + return INVALID_COMMAND_CODE; + case 3: + return PARAMETER_MISSING; + case 4: + return PARAMETER_INVALID; + case 5: + return CC_UNAVAILABLE; + case 7: + return INTERNAL_PROCESSING_ERROR; + default: + sif::error << "IMTQHandler::parseStatusByte: CMD Error field contains unknown error code " + << cmdErrorField << std::endl; + return CMD_ERR_UNKNOWN; + } +} + +void IMTQHandler::fillEngHkDataset(const uint8_t* packet) { + uint8_t offset = 2; + engHkDataset.digitalVoltageMv = *(packet + offset + 1) | *(packet + offset); + offset += 2; + engHkDataset.analogVoltageMv = *(packet + offset + 1) | *(packet + offset); + offset += 2; + engHkDataset.digitalCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.analogCurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilXcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilYcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilZcurrentA = (*(packet + offset + 1) | *(packet + offset)) * 0.0001; + offset += 2; + engHkDataset.coilXTemperature = (*(packet + offset + 1) | *(packet + offset)); + offset += 2; + engHkDataset.coilYTemperature = (*(packet + offset + 1) | *(packet + offset)); + offset += 2; + engHkDataset.coilZTemperature = (*(packet + offset + 1) | *(packet + offset)); + offset += 2; + engHkDataset.mcuTemperature = (*(packet + offset + 1) | *(packet + offset)); + +#if OBSW_VERBOSE_LEVEL >= 1 && IMTQ_DEBUG == 1 + sif::info << "IMTQ digital voltage: " << engHkDataset.digitalVoltageMv << " mV" << std::endl; + sif::info << "IMTQ analog voltage: " << engHkDataset.analogVoltageMv << " mV" << std::endl; + sif::info << "IMTQ digital current: " << engHkDataset.digitalCurrentA << " A" << std::endl; + sif::info << "IMTQ analog current: " << engHkDataset.analogCurrentA << " A" << std::endl; + sif::info << "IMTQ coil X current: " << engHkDataset.coilXcurrentA << " A" << std::endl; + sif::info << "IMTQ coil Y current: " << engHkDataset.coilYcurrentA << " A" << std::endl; + sif::info << "IMTQ coil Z current: " << engHkDataset.coilZcurrentA << " A" << std::endl; + sif::info << "IMTQ coil X temperature: " << engHkDataset.coilXTemperature << " °C" + << std::endl; + sif::info << "IMTQ coil Y temperature: " << engHkDataset.coilYTemperature << " °C" + << std::endl; + sif::info << "IMTQ coil Z temperature: " << engHkDataset.coilZTemperature << " °C" + << std::endl; + sif::info << "IMTQ coil MCU temperature: " << engHkDataset.mcuTemperature << " °C" + << std::endl; +#endif +} + + +void IMTQHandler::parseRxStatusRegistersReply(const uint8_t* packet) { + PoolReadHelper readHelper(&rxDataset); + uint16_t offset = SYRLINKS::MESSAGE_HEADER_SIZE; + rxDataset.rxStatus = convertHexStringToUint8(reinterpret_cast(packet + offset)); + offset += 2; + rxDataset.rxSensitivity = convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxFrequencyShift = convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxIqPower = convertHexStringToUint16(reinterpret_cast(packet + offset)); + offset += 4; + rxDataset.rxAgcValue = convertHexStringToUint16(reinterpret_cast(packet + offset)); + offset += 4; + offset += 2; // reserved register + rxDataset.rxDemodEb= convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxDemodN0= convertHexStringToUint32(reinterpret_cast(packet + offset), 6); + offset += 6; + rxDataset.rxDataRate = convertHexStringToUint8(reinterpret_cast(packet + offset)); + +#if OBSW_VERBOSE_LEVEL >= 1 && SYRLINKS_DEBUG == 1 + sif::info << "Syrlinks RX Status: 0x" << std::hex << (unsigned int)rxDataset.rxStatus.value << std::endl; + sif::info << "Syrlinks RX Sensitivity: " << std::dec << rxDataset.rxSensitivity << std::endl; + sif::info << "Syrlinks RX Frequency Shift: " << rxDataset.rxFrequencyShift << std::endl; + sif::info << "Syrlinks RX IQ Power: " << rxDataset.rxIqPower << std::endl; + sif::info << "Syrlinks RX AGC Value: " << rxDataset.rxAgcValue << std::endl; + sif::info << "Syrlinks RX Demod Eb: " << rxDataset.rxDemodEb << std::endl; + sif::info << "Syrlinks RX Demod N0: " << rxDataset.rxDemodN0 << std::endl; + sif::info << "Syrlinks RX Datarate: " << (unsigned int)rxDataset.rxDataRate.value << std::endl; +#endif +} + + + +void IMTQHandler::setNormalDatapoolEntriesInvalid(){ + +} + +uint32_t IMTQHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo){ + return 500; +} + +ReturnValue_t IMTQHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) { + + localDataPoolMap.emplace(IMTQ::DIGITAL_VOLTAGE_MV, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::ANALOG_VOLTAGE_MV, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::DIGITAL_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::ANALOG_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_X_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Y_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Z_CURRENT_A, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_X_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Y_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::COIL_Z_TEMPERATURE, new PoolEntry( { 0 })); + localDataPoolMap.emplace(IMTQ::MCU_TEMPERATURE, new PoolEntry( { 0 })); + + return HasReturnvaluesIF::RETURN_OK; +} + +void IMTQHandler::setModeNormal() { + mode = MODE_NORMAL; +} + diff --git a/mission/devices/IMTQHandler.h b/mission/devices/IMTQHandler.h new file mode 100644 index 00000000..207b7560 --- /dev/null +++ b/mission/devices/IMTQHandler.h @@ -0,0 +1,79 @@ +#ifndef MISSION_DEVICES_IMTQHANDLER_H_ +#define MISSION_DEVICES_IMTQHANDLER_H_ + +#include +#include +#include + +/** + * @brief This is the device handler for the ISIS Magnetorquer iMTQ. + * + * @author J. Meier + */ +class IMTQHandler: public DeviceHandlerBase { +public: + + IMTQHandler(object_id_t objectId, object_id_t comIF, + CookieIF * comCookie); + virtual ~IMTQHandler(); + + /** + * @brief Sets mode to MODE_NORMAL. Can be used for debugging. + */ + void setModeNormal(); + +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 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; + LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; + +private: + + static const uint8_t INTERFACE_ID = CLASS_ID::IMTQ_HANDLER; + + static const ReturnValue_t INVALID_COMMAND_CODE = MAKE_RETURN_CODE(0xA0); + static const ReturnValue_t PARAMETER_MISSING = MAKE_RETURN_CODE(0xA1); + static const ReturnValue_t PARAMETER_INVALID = MAKE_RETURN_CODE(0xA2); + static const ReturnValue_t CC_UNAVAILABLE = MAKE_RETURN_CODE(0xA3); + static const ReturnValue_t INTERNAL_PROCESSING_ERROR = MAKE_RETURN_CODE(0xA4); + static const ReturnValue_t REJECTED_WITHOUT_REASON = MAKE_RETURN_CODE(0xA5); + static const ReturnValue_t CMD_ERR_UNKNOWN = MAKE_RETURN_CODE(0xA6); + + + IMTQ::EngHkDataset engHkDataset; + + uint8_t commandBuffer[IMTQ::MAX_COMMAND_SIZE]; + + /** + * @brief Each reply contains a status byte giving information about a request. This function + * parses this byte and returns the associated failure message. + * + * @param packet Pointer to the received message containing the status byte. + * + * @return The return code derived from the received status byte. + */ + ReturnValue_t parseStatusByte(const uint8_t* packet); + + /** + * @brief This function fills the engineering housekeeping dataset with the received data. + + * @param packet Pointer to the received data. + * + */ + void fillEngHkDataset(const uint8_t* packet); +}; + +#endif /* MISSION_DEVICES_IMTQHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h new file mode 100644 index 00000000..34e14dc6 --- /dev/null +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -0,0 +1,110 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ + +namespace IMTQ { + + static const DeviceCommandId_t NONE = 0x0; + static const DeviceCommandId_t GET_ENG_HK_DATA = 0x1; + static const DeviceCommandId_t START_ACTUATION_DIPOLE = 0x2; + + static const uint8_t GET_TEMP_REPLY_SIZE = 2; + static const uint8_t CFGR_CMD_SIZE = 3; + static const uint8_t POINTER_REG_SIZE = 1; + + static const uint32_t ENG_HK_DATA_SET_ID = GET_ENG_HK_DATA; + static const uint8_t SIZE_ENG_HK_COMMAND = 1; + static const uint8_t SIZE_ENG_HK_DATA_REPLY = 24; + + static const uint8_t MAX_REPLY_SIZE = SIZE_ENG_HK_DATA_REPLY; + static const uint8_t MAX_COMMAND_SIZE = 9; + + static const uint8_t POOL_ENTRIES = 8; + + /** + * Command code definitions. Each command or reply of an IMTQ request will begin with one of + * the following command codes. + */ + namespace CC { + static const uint8_t START_ACTUATION_DIPOLE = 0x6; + static const uint8_t SOFTWARE_RESET = 0xAA; + static const uint8_t GET_ENG_HK_DATA = 0x4A; + }; + + enum IMTQPoolIds: lp_id_t { + DIGITAL_VOLTAGE_MV, + ANALOG_VOLTAGE_MV, + DIGITAL_CURRENT_A, + ANALOG_CURRENT_A, + COIL_X_CURRENT_A, + COIL_Y_CURRENT_A, + COIL_Z_CURRENT_A, + COIL_X_TEMPERATURE, + COIL_Y_TEMPERATURE, + COIL_Z_TEMPERATURE, + MCU_TEMPERATURE + }; + +class EngHkDataset: + public StaticLocalDataSet { +public: + + EngHkDataset(HasLocalDataPoolIF* owner): + StaticLocalDataSet(owner, ENG_HK_DATA_SET_ID) { + } + + EngHkDataset(object_id_t objectId): + StaticLocalDataSet(sid_t(objectId, ENG_HK_DATA_SET_ID)) { + } + + lp_var_t digitalVoltageMv = lp_var_t(sid.objectId, + DIGITAL_VOLTAGE_MV, this); + lp_var_t analogVoltageMv = lp_var_t(sid.objectId, + ANALOG_VOLTAGE_MV, this); + lp_var_t digitalCurrentA = lp_var_t(sid.objectId, + DIGITAL_CURRENT_A, this); + lp_var_t analogCurrentA = lp_var_t(sid.objectId, + ANALOG_CURRENT_A, this); + lp_var_t coilXcurrentA = lp_var_t(sid.objectId, + COIL_X_CURRENT_A, this); + lp_var_t coilYcurrentA = lp_var_t(sid.objectId, + COIL_Y_CURRENT_A, this); + lp_var_t coilZcurrentA = lp_var_t(sid.objectId, + COIL_Z_CURRENT_A, this); + /** All temperatures in [°C] */ + lp_var_t coilXTemperature = lp_var_t(sid.objectId, + COIL_X_TEMPEARTURE, this); + lp_var_t coilYTemperature = lp_var_t(sid.objectId, + COIL_Y_TEMPERATURE, this); + lp_var_t coilZTemperature = lp_var_t(sid.objectId, + COIL_Z_TEMPERATURE, this); + lp_var_t mcuTemperature = lp_var_t(sid.objectId, + MCU_TEMPERATURE, this); +}; + +/** + * @brief This class can be used to ease the generation of an action message commanding the + * IMTQHandler to configure the magnettorquer with the desired dipoles. + */ +class CommandDipolePacket : public SerialLinkedListAdapter { +public: + + CommandDipolePacket() { + setLinks(); + } + +private: + CommandDipolePacket(uint16_t xDipole, uint16_t yDipole, uint16_t zDipole); + void setLinks() { + setStart(&cspPort); + cspPort.setNext(&querySize); + querySize.setNext(&magic); + } + SerializeElement cspPort = GOMSPACE::P60_PORT_GNDWDT_RESET; + SerializeElement querySize = 1; + /* Sending 0x78 to port 9 of a gomspace device resets the ground watchdog */ + SerializeElement magic = 0x78; +}; +} + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_IMTQDEFINITIONS_H_ */