diff --git a/bsp_hosted/InitMission.cpp b/bsp_hosted/InitMission.cpp index 29101d08..cfc33db9 100644 --- a/bsp_hosted/InitMission.cpp +++ b/bsp_hosted/InitMission.cpp @@ -8,8 +8,8 @@ #include #include #include -#include -#include +#include +#include #include diff --git a/bsp_hosted/fsfwconfig/OBSWConfig.h b/bsp_hosted/fsfwconfig/OBSWConfig.h index edfa1bca..f9e76160 100644 --- a/bsp_hosted/fsfwconfig/OBSWConfig.h +++ b/bsp_hosted/fsfwconfig/OBSWConfig.h @@ -6,6 +6,9 @@ #ifndef CONFIG_OBSWCONFIG_H_ #define CONFIG_OBSWCONFIG_H_ +#include "returnvalues/classIds.h" +#include "events/subsystemIdRanges.h" + #define OBSW_ADD_TEST_CODE 0 // These defines should be disabled for mission code but are useful for diff --git a/bsp_hosted/fsfwconfig/events/subsystemIdRanges.h b/bsp_hosted/fsfwconfig/events/subsystemIdRanges.h index 2255b345..1128b26b 100644 --- a/bsp_hosted/fsfwconfig/events/subsystemIdRanges.h +++ b/bsp_hosted/fsfwconfig/events/subsystemIdRanges.h @@ -18,6 +18,7 @@ enum: uint8_t { PUS_SERVICE_8, PUS_SERVICE_23, MGM_LIS3MDL, + MGM_RM3100, DUMMY_DEVICE, }; diff --git a/bsp_hosted/fsfwconfig/fsfwconfig.mk b/bsp_hosted/fsfwconfig/fsfwconfig.mk index 105c3fba..53cb0fc7 100644 --- a/bsp_hosted/fsfwconfig/fsfwconfig.mk +++ b/bsp_hosted/fsfwconfig/fsfwconfig.mk @@ -4,12 +4,4 @@ CXXSRC += $(wildcard $(CURRENTPATH)/objects/*.cpp) CXXSRC += $(wildcard $(CURRENTPATH)/pollingsequence/*.cpp) CXXSRC += $(wildcard $(CURRENTPATH)/events/*.cpp) -INCLUDES += $(CURRENTPATH) -INCLUDES += $(CURRENTPATH)/objects -INCLUDES += $(CURRENTPATH)/ipc -INCLUDES += $(CURRENTPATH)/pollingsequence -INCLUDES += $(CURRENTPATH)/returnvalues -INCLUDES += $(CURRENTPATH)/tmtc -INCLUDES += $(CURRENTPATH)/events -INCLUDES += $(CURRENTPATH)/devices -INCLUDES += $(CURRENTPATH)/cdatapool \ No newline at end of file +INCLUDES += $(CURRENTPATH) \ No newline at end of file diff --git a/bsp_hosted/fsfwconfig/returnvalues/classIds.h b/bsp_hosted/fsfwconfig/returnvalues/classIds.h index f6427a1f..21371c15 100644 --- a/bsp_hosted/fsfwconfig/returnvalues/classIds.h +++ b/bsp_hosted/fsfwconfig/returnvalues/classIds.h @@ -11,7 +11,8 @@ namespace CLASS_ID { enum { MISSION_CLASS_ID_START = FW_CLASS_ID_COUNT, - MGM_LIS3MDL + MGM_LIS3MDL, + MGM_RM3100 }; } diff --git a/bsp_hosted/main.cpp b/bsp_hosted/main.cpp index 50d1e7f2..02d793f9 100644 --- a/bsp_hosted/main.cpp +++ b/bsp_hosted/main.cpp @@ -1,5 +1,6 @@ -#include -#include +#include "InitMission.h" + +#include #include #include diff --git a/fsfwconfig/OBSWConfig.h b/fsfwconfig/OBSWConfig.h index d8784551..7b9dfbc4 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -6,6 +6,9 @@ #ifndef FSFWCONFIG_OBSWCONFIG_H_ #define FSFWCONFIG_OBSWCONFIG_H_ +#include "returnvalues/classIds.h" +#include "events/subsystemIdRanges.h" + #define OBSW_ADD_TEST_CODE 0 // These defines should be disabled for mission code but are useful for diff --git a/fsfwconfig/events/subsystemIdRanges.h b/fsfwconfig/events/subsystemIdRanges.h index ece990b2..47e04816 100644 --- a/fsfwconfig/events/subsystemIdRanges.h +++ b/fsfwconfig/events/subsystemIdRanges.h @@ -17,7 +17,8 @@ enum: uint8_t { PUS_SERVICE_6, PUS_SERVICE_8, PUS_SERVICE_23, - MGM_LIS3MDL + MGM_LIS3MDL, + MGM_RM3100 }; } diff --git a/fsfwconfig/returnvalues/classIds.h b/fsfwconfig/returnvalues/classIds.h index b5835c84..6d68d954 100644 --- a/fsfwconfig/returnvalues/classIds.h +++ b/fsfwconfig/returnvalues/classIds.h @@ -11,7 +11,8 @@ namespace CLASS_ID { enum { MISSION_CLASS_ID_START = FW_CLASS_ID_COUNT, - MGM_LIS3MDL + MGM_LIS3MDL, + MGM_RM3100 }; } diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 160f43ce..1833ef54 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -39,7 +39,7 @@ void MGMHandlerLIS3MDL::doStartUp() { // Set up cached registers which will be used to configure the MGM. if(commandExecuted) { commandExecuted = false; - setMode(MODE_NORMAL); + setMode(_MODE_TO_ON); } break; } diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 3e6fffd3..2ff57d85 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -1,4 +1,5 @@ #include "MGMHandlerRM3100.h" + #include #include #include @@ -16,61 +17,115 @@ void MGMHandlerRM3100::doStartUp() { internalState = STATE_CONFIGURE_CMM; } - switch(internalState) { - case(STATE_CONFIGURE_CMM): { - break; + if(internalState == STATE_CONFIGURE_CMM) { + internalState = STATE_READ_CMM; } - case(STATE_READ_CMM): { - break; + else if(internalState == STATE_READ_CMM) { + if(commandExecuted) { + internalState = STATE_CONFIGURE_TMRC; + } } - case(STATE_CONFIGURE_TMRC): { - break; - } - case(STATE_READ_TMRC): { - break; + + if(internalState == STATE_CONFIGURE_TMRC) { + internalState = STATE_READ_TMRC; } + else if(internalState == STATE_READ_TMRC) { + if(commandExecuted) { + internalState = STATE_NORMAL; + setMode(_MODE_TO_ON); + } } } void MGMHandlerRM3100::doShutDown() { + setMode(_MODE_POWER_DOWN); } ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( DeviceCommandId_t *id) { if(internalState == STATE_CONFIGURE_CMM) { *id = RM3100::CONFIGURE_CMM; - commandBuffer[0] = RM3100::CMM_REGISTER; - commandBuffer[1] = RM3100::CMM_VALUE; - this->rawPacket = commandBuffer; - this->rawPacketLen = 2; } if(internalState == STATE_READ_CMM) { *id = RM3100::READ_CMM; - commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK; - commandBuffer[1] = 0; - this->rawPacket = commandBuffer; - this->rawPacketLen = 2; } - return RETURN_OK; -} + if(internalState == STATE_CONFIGURE_TMRC) { + *id = RM3100::CONFIGURE_TMRC; + } -ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand( - DeviceCommandId_t *id) { - return RETURN_OK; + if(internalState == STATE_READ_TMRC) { + *id = RM3100::READ_TMRC; + } + + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t MGMHandlerRM3100::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { + switch(deviceCommand) { + case(RM3100::CONFIGURE_CMM): { + commandBuffer[0] = RM3100::CMM_REGISTER; + commandBuffer[1] = RM3100::CMM_VALUE; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::READ_CMM): { + commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK; + commandBuffer[1] = 0; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::CONFIGURE_TMRC): { + return handleTmrcConfigCommand(deviceCommand, commandData, + commandDataLen); + } + case(RM3100::READ_TMRC): { + commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK; + commandBuffer[1] = 0; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::CONFIGURE_CYCLE_COUNT): { + return handleCycleCountConfigCommand(deviceCommand, commandData, + commandDataLen); + } + case(RM3100::READ_CYCLE_COUNT): { + commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | + RM3100::READ_MASK; + std::memset(commandBuffer + 1, 0, 6); + rawPacket = commandBuffer; + rawPacketLen = 7; + break; + } + case(RM3100::READ_DATA): { + commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK; + std::memset(commandBuffer + 1, 0, 9); + rawPacketLen = 10; + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } return RETURN_OK; } +ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand( + DeviceCommandId_t *id) { + *id = RM3100::READ_DATA; + return buildCommandFromCommand(*id, nullptr, 0); +} + ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { + // SPI, ID will always be the one of the last sent command. *foundId = this->getPendingCommand(); *foundLen = this->rawPacketLen; @@ -85,27 +140,115 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( DeviceCommandId_t id, const uint8_t *packet) { - if(id == RM3100::CONFIGURE_CMM or id == RM3100::CONFIGURE_TMRC) { - commandExecuted = true; - } - else if(id == RM3100::READ_CMM) { + if(id == RM3100::READ_CMM) { if(packet[1] == cmmRegValue) { commandExecuted = true; } + else { + // Attempt reconfiguration. + internalState = STATE_CONFIGURE_CMM; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } } else if(id == RM3100::READ_TMRC) { if(packet[1] == tmrcRegValue) { commandExecuted = true; + // Reading TMRC was commanded. + if(mode == MODE_NORMAL) { + + } } else { // Attempt reconfiguration. internalState = STATE_CONFIGURE_TMRC; - return HasReturnvaluesIF::RETURN_FAILED; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; } } + else if(id == RM3100::READ_DATA) { + // analyze data here. + // Field strengths in micro Tesla + int32_t fieldStrengthX = (packet[1] << 16 | packet[2] << 8 | packet[3]) + * scaleFactorX; + int32_t fieldStrengthY = (packet[4] << 16 | packet[5] << 8 | packet[6]) + * scaleFactorY; + int32_t fieldStrengthZ = (packet[7] << 16 | packet[8] << 8 | packet[9]) + * scaleFactorZ; + } else { return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } return RETURN_OK; } + +ReturnValue_t MGMHandlerRM3100::handleCycleCountConfigCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + if(commandData == nullptr) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + // Set cycle count + if(commandDataLen == 2) { + handleCycleCommand(true, commandData, commandDataLen); + } + else if(commandDataLen == 6) { + handleCycleCommand(false, commandData, commandDataLen); + } + else { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE; + std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2); + std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2); + std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2); + rawPacketLen = 7; + rawPacket = commandBuffer; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MGMHandlerRM3100::handleCycleCommand(bool oneCycleValue, + const uint8_t *commandData, size_t commandDataLen) { + RM3100::CycleCountCommand command(oneCycleValue); + ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::BIG); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + // Data sheet p.30 + // "while noise limits the useful upper range to ~400 cycle counts." + if(command.cycleCountX > 450 ) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + if(not oneCycleValue and + (command.cycleCountY > 450 or command.cycleCountZ > 450)) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + cycleCountRegValueX = command.cycleCountX; + cycleCountRegValueY = command.cycleCountY; + cycleCountRegValueZ = command.cycleCountZ; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + if(commandData == nullptr) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + if(commandDataLen != 1) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + commandBuffer[0] = RM3100::TMRC_REGISTER; + commandBuffer[1] = commandData[1]; + rawPacketLen = 2; + rawPacket = commandBuffer; + return HasReturnvaluesIF::RETURN_OK; +} + diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index ed948560..605ac890 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -4,8 +4,12 @@ #include "devicedefinitions/MGMHandlerRM3100Definitions.h" #include +#include + class MGMHandlerRM3100: public DeviceHandlerBase { public: + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_RM3100; MGMHandlerRM3100(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie); @@ -45,11 +49,25 @@ private: InternalState internalState = InternalState::STATE_NONE; bool commandExecuted = false; - uint8_t commandBuffer[9]; + uint8_t commandBuffer[10]; uint8_t commandBufferLen = 0; uint8_t cmmRegValue = RM3100::CMM_VALUE; uint8_t tmrcRegValue = RM3100::TMRC_DEFAULT_VALUE; + uint16_t cycleCountRegValueX = RM3100::CYCLE_COUNT_VALUE; + uint16_t cycleCountRegValueY = RM3100::CYCLE_COUNT_VALUE; + uint16_t cycleCountRegValueZ = RM3100::CYCLE_COUNT_VALUE; + float scaleFactorX = 1 / RM3100::DEFAULT_GAIN; + float scaleFactorY = 1 / RM3100::DEFAULT_GAIN; + float scaleFactorZ = 1 / RM3100::DEFAULT_GAIN; + + ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData,size_t commandDataLen); + ReturnValue_t handleCycleCommand(bool oneCycleValue, + const uint8_t *commandData, size_t commandDataLen); + + ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData,size_t commandDataLen); }; #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h index 4f38a217..60a107d8 100644 --- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -1,7 +1,10 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ +#include +#include #include +#include #include namespace RM3100 { @@ -24,9 +27,11 @@ static constexpr uint8_t CMM_VALUE = SET_CMM_CMZ | SET_CMM_CMY | SET_CMM_CMX | /*----------------------------------------------------------------------------*/ /* Cycle count register /*----------------------------------------------------------------------------*/ +// Default value (200) static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8; -static constexpr uint8_t GAIN = CYCLE_COUNT_VALUE / 100 * 38; +static constexpr float DEFAULT_GAIN = static_cast(CYCLE_COUNT_VALUE) / + 100 * 38; static constexpr uint8_t CYCLE_COUNT_START_REGISTER = 0x04; /*----------------------------------------------------------------------------*/ @@ -48,13 +53,74 @@ static constexpr uint8_t REVID_REGISTER = 0x36; // Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM) static constexpr uint16_t RANGE = 800; -static constexpr DeviceCommandId_t CONFIGURE_CMM = 3; -static constexpr DeviceCommandId_t READ_CMM = 4; +static constexpr DeviceCommandId_t READ_DATA = 0; -//static constexpr DeviceCommandId_t CONFIGURE_CYC +static constexpr DeviceCommandId_t CONFIGURE_CMM = 1; +static constexpr DeviceCommandId_t READ_CMM = 2; -static constexpr DeviceCommandId_t CONFIGURE_TMRC = 5; -static constexpr DeviceCommandId_t READ_TMRC = 6; +static constexpr DeviceCommandId_t CONFIGURE_TMRC = 3; +static constexpr DeviceCommandId_t READ_TMRC = 4; + +static constexpr DeviceCommandId_t CONFIGURE_CYCLE_COUNT = 5; +static constexpr DeviceCommandId_t READ_CYCLE_COUNT = 6; + +class CycleCountCommand: public SerialLinkedListAdapter { +public: + CycleCountCommand(bool oneCycleCount = true): oneCycleCount(oneCycleCount) { + setLinks(oneCycleCount); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override { + ReturnValue_t result = SerialLinkedListAdapter::deSerialize(buffer, + size, streamEndianness); + if(oneCycleCount) { + cycleCountY = cycleCountX; + cycleCountZ = cycleCountX; + } + return result; + } + + SerializeElement cycleCountX; + SerializeElement cycleCountY; + SerializeElement cycleCountZ; + +private: + void setLinks(bool oneCycleCount) { + setStart(&cycleCountX); + if(not oneCycleCount) { + cycleCountX.setNext(&cycleCountY); + cycleCountY.setNext(&cycleCountZ); + } + } + + bool oneCycleCount; +}; + +static constexpr uint32_t MGM_DATASET_ID = READ_DATA; + +enum MgmPoolIds: lp_id_t { + FIELD_STRENGTH_X, + FIELD_STRENGTH_Y, + FIELD_STRENGTH_Z, +}; + +class Rm3100PrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +public: + Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {} + + Rm3100PrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, MGM_DATASET_ID)) {} + + // Field strengths in micro Tesla. + lp_var_t fieldStrengthX = lp_var_t(sid.objectId, + FIELD_STRENGTH_X, this); + lp_var_t fieldStrengthY = lp_var_t(sid.objectId, + FIELD_STRENGTH_Y, this); + lp_var_t fieldStrengthZ = lp_var_t(sid.objectId, + FIELD_STRENGTH_Z, this); +}; }