From f52d615196e8a766ea40c113cce508011643626c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 17:32:51 +0100 Subject: [PATCH 01/20] disabled LTO for host release build on windows --- Makefile-Hosted | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Makefile-Hosted b/Makefile-Hosted index 9dff3677..807fe5ec 100644 --- a/Makefile-Hosted +++ b/Makefile-Hosted @@ -227,7 +227,6 @@ DEAD_CODE_REMOVAL = -Wl,--gc-sections # Link time is larger and size of object files can not be retrieved # but resulting binary is smaller. Could be used in mission/deployment build # Requires -ffunction-section in linker call -LINK_TIME_OPTIMIZATION = -flto OPTIMIZATION += $(PROTOTYPE_OPTIMIZATION) endif @@ -309,7 +308,10 @@ all: executable # Build target configuration release: OPTIMIZATION = -Os $(PROTOTYPE_OPTIMIZATION) $(LINK_TIME_OPTIMIZATION) +# Problematic on MinGW +ifneq ($(OS),Windows_NT) release: LINK_TIME_OPTIMIZATION = -flto +endif release: TARGET = Release release: OPTIMIZATION_MESSAGE = On with Link Time Optimization release: DEBUG_LEVEL = -g0 From 5d2da2bf60d6c3e9a1dc5f4581ec99462400a58e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 17:52:53 +0100 Subject: [PATCH 02/20] makefile hosted update --- Makefile-Hosted | 12 ++++++++++-- tmtc | 2 +- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/Makefile-Hosted b/Makefile-Hosted index 807fe5ec..49460898 100644 --- a/Makefile-Hosted +++ b/Makefile-Hosted @@ -17,12 +17,12 @@ CUSTOM_DEFINES := # (can be overriden by adding CHIP=chip and BOARD=board to the command-line) BOARD_FILE_ROOT = bsp_hosted BOARD = host -OS_FSFW = host -CUSTOM_DEFINES += -D$(OS_FSFW) +DETECTED_OS := # Copied from stackoverflow, can be used to differentiate between Windows # and Linux ifeq ($(OS),Windows_NT) + DETECTED_OS = WINDOWS CUSTOM_DEFINES += -DWIN32 ifeq ($(PROCESSOR_ARCHITEW6432),AMD64) CUSTOM_DEFINES += -DAMD64 @@ -55,6 +55,14 @@ else endif endif +ifeq (${DETECTED_OS}, WINDOWS) +OS_FSFW = host +else +OS_FSFW = linux +endif + +CUSTOM_DEFINES += -D$(OS_FSFW) + # General folder paths FRAMEWORK_PATH = fsfw MISSION_PATH = mission diff --git a/tmtc b/tmtc index e0c896e6..07b6a9df 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit e0c896e62d25286d00599ca57b71d0a3495ed95f +Subproject commit 07b6a9df18baff999ca52c2f2781f8f77f8dcb65 From a6067268242604b904f2c19b8df5cd59da0309e3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 18:14:03 +0100 Subject: [PATCH 03/20] reparied arduino stuff --- Makefile-Hosted | 14 ++++----- bsp_hosted/comIF/ArduinoComIF.cpp | 15 ++++++---- misc/eclipse/eive-linux-host-debug.launch | 33 +++++++++++++++++++++ misc/eclipse/eive-linux-host-release.launch | 33 +++++++++++++++++++++ 4 files changed, 82 insertions(+), 13 deletions(-) create mode 100644 misc/eclipse/eive-linux-host-debug.launch create mode 100644 misc/eclipse/eive-linux-host-release.launch diff --git a/Makefile-Hosted b/Makefile-Hosted index 49460898..3a93f625 100644 --- a/Makefile-Hosted +++ b/Makefile-Hosted @@ -187,10 +187,10 @@ $(foreach S,$(SUBDIRS),$(eval $(INCLUDE_FILE))) # VPATH += mission/pus/ -ifeq ($(DETECTED_OS), LINUX) -CXXSRC += $(FRAMEWORK_PATH)/osal/linux/TcUnixUdpPollingTask.cpp -CXXSRC += $(FRAMEWORK_PATH)/osal/linux/TmTcUnixUdpBridge.cpp -endif +#ifeq ($(DETECTED_OS), LINUX) +#CXXSRC += $(FRAMEWORK_PATH)/osal/linux/TcUnixUdpPollingTask.cpp +#CXXSRC += $(FRAMEWORK_PATH)/osal/linux/TmTcUnixUdpBridge.cpp +#endif # All C Sources included by .mk files are assigned here # Add the objects to sources so dependency handling works @@ -268,12 +268,12 @@ ASFLAGS = -Wall -g $(OPTIMIZATION) $(I_INCLUDES) -D__ASSEMBLY__ # LINK_INCLUDES specify the path to used libraries and the linker script # LINK_LIBRARIES link HCC and HAL library and enable float support LDFLAGS := -g3 -pthread $(DEAD_CODE_REMOVAL) $(OPTIMIZATION) - -LINK_INCLUDES := - LINK_LIBRARIES := + ifeq ($(OS),Windows_NT) LINK_LIBRARIES += -lwsock32 -lws2_32 +else +LINK_LIBRARIES += -lrt endif # Gnu Coverage Tools Flags diff --git a/bsp_hosted/comIF/ArduinoComIF.cpp b/bsp_hosted/comIF/ArduinoComIF.cpp index 047a60ae..2db293e6 100644 --- a/bsp_hosted/comIF/ArduinoComIF.cpp +++ b/bsp_hosted/comIF/ArduinoComIF.cpp @@ -1,5 +1,6 @@ -#include -#include +#include "ArduinoComIF.h" +#include "ArduinoCookie.h" + #include #include #include @@ -7,6 +8,8 @@ // This only works on Linux #ifdef LINUX #include +#include +#include #elif WIN32 #include #include @@ -254,7 +257,7 @@ ReturnValue_t ArduinoComIF::sendMessage(uint8_t command, encodedLen = sizeof(sendBuffer) - remainingLen; #ifdef LINUX - ssize_t writtenlen = write(serialPort, sendBuffer, encodedLen); + ssize_t writtenlen = ::write(serialPort, sendBuffer, encodedLen); if (writtenlen < 0) { //we could try to find out what happened... return RETURN_FAILED; @@ -285,7 +288,7 @@ void ArduinoComIF::handleSerialPortRx() { rxBuffer.writeData(dataFromSerial, bytesRead); - uint8_t dataReceivedSoFar[rxBuffer.maxSize()]; + uint8_t dataReceivedSoFar[rxBuffer.getMaxSize()]; uint32_t dataLenReceivedSoFar = 0; @@ -295,11 +298,11 @@ void ArduinoComIF::handleSerialPortRx() { //look for STX size_t firstSTXinRawData = 0; while ((firstSTXinRawData < dataLenReceivedSoFar) - && (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX)) { + && (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX_CHAR)) { firstSTXinRawData++; } - if (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX) { + if (dataReceivedSoFar[firstSTXinRawData] != DleEncoder::STX_CHAR) { //there is no STX in our data, throw it away... rxBuffer.deleteData(dataLenReceivedSoFar); return; diff --git a/misc/eclipse/eive-linux-host-debug.launch b/misc/eclipse/eive-linux-host-debug.launch new file mode 100644 index 00000000..f5ab1ac7 --- /dev/null +++ b/misc/eclipse/eive-linux-host-debug.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/misc/eclipse/eive-linux-host-release.launch b/misc/eclipse/eive-linux-host-release.launch new file mode 100644 index 00000000..7548f831 --- /dev/null +++ b/misc/eclipse/eive-linux-host-release.launch @@ -0,0 +1,33 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 58b440109b622b23b25ca9614ddeffc19eb4fe03 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 18:15:27 +0100 Subject: [PATCH 04/20] updated project file --- misc/eclipse/.cproject | 205 ++++++++++++++++++++++++++++++++--------- 1 file changed, 159 insertions(+), 46 deletions(-) diff --git a/misc/eclipse/.cproject b/misc/eclipse/.cproject index 776ea205..6a50d831 100644 --- a/misc/eclipse/.cproject +++ b/misc/eclipse/.cproject @@ -1,52 +1,12 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + @@ -58,16 +18,22 @@ - + + + @@ -88,6 +54,7 @@ + @@ -99,16 +66,22 @@ - + + + @@ -124,15 +97,123 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + @@ -141,4 +222,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file From 93971ad1c9a2fb7d1285e32ed174dd94548f48b4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 19:50:01 +0100 Subject: [PATCH 05/20] started rm3100 handler --- mission/devices/MGMHandlerRM3100.cpp | 9 ++++ mission/devices/MGMHandlerRM3100.h | 12 ++++- .../MGMHandlerRM3100Definitions.h | 46 +++++++++++++++++++ 3 files changed, 66 insertions(+), 1 deletion(-) diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index fbcb578d..0b18dc91 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -17,6 +17,15 @@ ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( } void MGMHandlerRM3100::doStartUp() { + if(internalState == STATE_NONE) { + internalState = STATE_CONFIGURE_CMM; + } + + switch(internalState) { + case(STATE_CONFIGURE_CMM): { + + } + } } void MGMHandlerRM3100::doShutDown() { diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index d2081fa1..cf459056 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -31,9 +31,19 @@ protected: private: enum InternalState { - STATE_NONE, STATE_FIRST_CONTACT, STATE_SETUP, STATE_CHECK_REGISTERS + STATE_NONE, + STATE_CONFIGURE_CMM, + STATE_READ_CMM, + // The cycle count states are propably not going to be used because + // the default cycle count will be used. + STATE_CONFIGURE_CYCLE_COUNT, + STATE_READ_CYCLE_COUNT, + STATE_CONFIGURE_TMRC, + STATE_READ_TMRC, + STATE_NORMAL }; InternalState internalState = InternalState::STATE_NONE; + bool commandExecuted = false; }; #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h index 5bf9e8b3..5fc70cbe 100644 --- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -1,8 +1,54 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ +#include + namespace RM3100 { +static constexpr uint8_t READ_MASK = 0b1000'0000; + +/*----------------------------------------------------------------------------*/ +/* CMM Register +/*----------------------------------------------------------------------------*/ +static constexpr uint8_t SET_CMM_CMZ = 1 << 6; +static constexpr uint8_t SET_CMM_CMY = 1 << 5; +static constexpr uint8_t SET_CMM_CMX = 1 << 4; +static constexpr uint8_t SET_CMM_DRDM = 1 << 2; +static constexpr uint8_t SET_CMM_START = 1; +static constexpr uint8_t CMM_REGISTER = 0x01; + +static constexpr uint8_t CMM_VALUE = SET_CMM_CMZ | SET_CMM_CMY | SET_CMM_CMX | + SET_CMM_DRDM | SET_CMM_START; + +/*----------------------------------------------------------------------------*/ +/* Cycle count register +/*----------------------------------------------------------------------------*/ +static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8; + +static constexpr uint8_t GAIN = CYCLE_COUNT_VALUE / 100 * 38; +static constexpr uint8_t CYCLE_COUNT_START_REGISTER = 0x04; + +/*----------------------------------------------------------------------------*/ +/* TMRC register +/*----------------------------------------------------------------------------*/ +static constexpr uint8_t TMRC_150HZ_VALUE = 0x94; +static constexpr uint8_t TMRC_75HZ_VALUE = 0x95; +static constexpr uint8_t TMRC_DEFAULT_37HZ_VALUE = 0x96; + +static constexpr uint8_t TMRC_REGISTER = 0x0B; +static constexpr uint8_t TMRC_DEFAULT_VALUE = TMRC_75HZ_VALUE; + +static constexpr uint8_t MEASUREMENT_REG_START = 0x24; +static constexpr uint8_t BIST_REGISTER = 0x33; +static constexpr uint8_t DATA_READY_VAL = 0b1000'0000; +static constexpr uint8_t STATUS_REGISTER = 0x34; +static constexpr uint8_t REVID_REGISTER = 0x36; + +// Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM) +static constexpr uint8_t RANGE = 800; + + + } From 4364ef18f3c76a7d0b9d93401cee28964de344a2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 19:50:50 +0100 Subject: [PATCH 06/20] type correction --- mission/devices/MGMHandlerRM3100.cpp | 11 ++++++++++- .../devicedefinitions/MGMHandlerRM3100Definitions.h | 2 +- 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 0b18dc91..7c546ffa 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -23,7 +23,16 @@ void MGMHandlerRM3100::doStartUp() { switch(internalState) { case(STATE_CONFIGURE_CMM): { - + break; + } + case(STATE_READ_CMM): { + break; + } + case(STATE_CONFIGURE_TMRC): { + break; + } + case(STATE_READ_TMRC): { + break; } } } diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h index 5fc70cbe..dd519697 100644 --- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -45,7 +45,7 @@ static constexpr uint8_t STATUS_REGISTER = 0x34; static constexpr uint8_t REVID_REGISTER = 0x36; // Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM) -static constexpr uint8_t RANGE = 800; +static constexpr uint16_t RANGE = 800; From f0b542e9cec4e0c035554c75cc011912a34c78ad Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 19:52:00 +0100 Subject: [PATCH 07/20] rm3100 continued --- fsfw | 2 +- mission/devices/MGMHandlerLIS3MDL.cpp | 2 +- mission/devices/MGMHandlerRM3100.cpp | 60 +++++++++++++++++-- mission/devices/MGMHandlerRM3100.h | 6 ++ .../MGMHandlerRM3100Definitions.h | 7 +++ tmtc | 2 +- 6 files changed, 70 insertions(+), 9 deletions(-) diff --git a/fsfw b/fsfw index 086cbe1e..31b82975 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 086cbe1e3950cf6c904609352408a7fa48cc83ef +Subproject commit 31b82975c7ef701fa7f1ba3413cfa19cc73aa2ca diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 1cc83bf8..160f43ce 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -283,7 +283,7 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, } default: { - return DeviceHandlerIF::UNKNOW_DEVICE_REPLY; + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } } diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 7c546ffa..3e6fffd3 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -11,11 +11,6 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, MGMHandlerRM3100::~MGMHandlerRM3100() {} -ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( - DeviceCommandId_t *id) { - return RETURN_OK; -} - void MGMHandlerRM3100::doStartUp() { if(internalState == STATE_NONE) { internalState = STATE_CONFIGURE_CMM; @@ -40,6 +35,27 @@ void MGMHandlerRM3100::doStartUp() { void MGMHandlerRM3100::doShutDown() { } +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; +} + + ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand( DeviceCommandId_t *id) { return RETURN_OK; @@ -54,10 +70,42 @@ ReturnValue_t MGMHandlerRM3100::buildCommandFromCommand( ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return RETURN_OK; + + *foundId = this->getPendingCommand(); + *foundLen = this->rawPacketLen; + + // Data with SPI Interface has always this answer + if (start[0] == 0b11111111) { + return RETURN_OK; + } + else { + return DeviceHandlerIF::INVALID_DATA; + } } 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(packet[1] == cmmRegValue) { + commandExecuted = true; + } + } + else if(id == RM3100::READ_TMRC) { + if(packet[1] == tmrcRegValue) { + commandExecuted = true; + } + else { + // Attempt reconfiguration. + internalState = STATE_CONFIGURE_TMRC; + return HasReturnvaluesIF::RETURN_FAILED; + } + } + else { + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + return RETURN_OK; } diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index cf459056..ed948560 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -44,6 +44,12 @@ private: }; InternalState internalState = InternalState::STATE_NONE; bool commandExecuted = false; + + uint8_t commandBuffer[9]; + uint8_t commandBufferLen = 0; + + uint8_t cmmRegValue = RM3100::CMM_VALUE; + uint8_t tmrcRegValue = RM3100::TMRC_DEFAULT_VALUE; }; #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h index dd519697..4f38a217 100644 --- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ +#include #include namespace RM3100 { @@ -47,7 +48,13 @@ 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 CONFIGURE_CYC + +static constexpr DeviceCommandId_t CONFIGURE_TMRC = 5; +static constexpr DeviceCommandId_t READ_TMRC = 6; } diff --git a/tmtc b/tmtc index 07b6a9df..e0c896e6 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 07b6a9df18baff999ca52c2f2781f8f77f8dcb65 +Subproject commit e0c896e62d25286d00599ca57b71d0a3495ed95f From f70c767a1c25893c2dc9cbee07c18333e326fc3b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Dec 2020 23:09:35 +0100 Subject: [PATCH 08/20] rm3100 continued --- bsp_hosted/InitMission.cpp | 4 +- bsp_hosted/fsfwconfig/OBSWConfig.h | 3 + .../fsfwconfig/events/subsystemIdRanges.h | 1 + bsp_hosted/fsfwconfig/fsfwconfig.mk | 10 +- bsp_hosted/fsfwconfig/returnvalues/classIds.h | 3 +- bsp_hosted/main.cpp | 5 +- fsfwconfig/OBSWConfig.h | 3 + fsfwconfig/events/subsystemIdRanges.h | 3 +- fsfwconfig/returnvalues/classIds.h | 3 +- mission/devices/MGMHandlerLIS3MDL.cpp | 2 +- mission/devices/MGMHandlerRM3100.cpp | 199 +++++++++++++++--- mission/devices/MGMHandlerRM3100.h | 20 +- .../MGMHandlerRM3100Definitions.h | 78 ++++++- 13 files changed, 282 insertions(+), 52 deletions(-) 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); +}; } From 71853fe1aa6bde8495542e533e2198d48a4f1cc2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 00:24:19 +0100 Subject: [PATCH 09/20] rm3100 finished, lis3 set normal datapool entries invalid removed, will be replaced by default implementation --- mission/devices/MGMHandlerLIS3MDL.cpp | 5 +- mission/devices/MGMHandlerLIS3MDL.h | 24 +++--- mission/devices/MGMHandlerRM3100.cpp | 115 ++++++++++++++++++++++---- mission/devices/MGMHandlerRM3100.h | 41 +++++++-- 4 files changed, 143 insertions(+), 42 deletions(-) diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 1833ef54..35b71c79 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -259,6 +259,7 @@ ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id, dataset.fieldStrengthX = mgmX; dataset.fieldStrengthY = mgmY; dataset.fieldStrengthZ = mgmZ; + dataset.setValidity(true, true); dataset.commit(20); } break; @@ -406,10 +407,6 @@ ReturnValue_t MGMHandlerLIS3MDL::prepareCtrlRegisterWrite() { return RETURN_OK; } -void MGMHandlerLIS3MDL::setNormalDatapoolEntriesInvalid() { - // TODO: use new distributed datapools here. -} - void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { } diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h index b400daee..c48b1fc2 100644 --- a/mission/devices/MGMHandlerLIS3MDL.h +++ b/mission/devices/MGMHandlerLIS3MDL.h @@ -35,28 +35,26 @@ public: protected: /** DeviceHandlerBase overrides */ - virtual void doShutDown() override; - virtual void doStartUp() override; - virtual void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override; - virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; - virtual ReturnValue_t buildCommandFromCommand( + void doShutDown() override; + void doStartUp() override; + void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) override; - virtual ReturnValue_t buildTransitionDeviceCommand( + ReturnValue_t buildTransitionDeviceCommand( DeviceCommandId_t *id) override; - virtual ReturnValue_t buildNormalDeviceCommand( + ReturnValue_t buildNormalDeviceCommand( DeviceCommandId_t *id) override; - virtual ReturnValue_t scanForReply(const uint8_t *start, size_t len, + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; - virtual void fillCommandAndReplyMap() override; - virtual void modeChanged(void) override; - void setNormalDatapoolEntriesInvalid() override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; ReturnValue_t initializeLocalDataPool(LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; - private: MGMLIS3MDL::MgmPrimaryDataset dataset; diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 2ff57d85..6040afad 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -7,7 +7,11 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie) { + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + primaryDataset(this) { +#if OBSW_ENHANCED_PRINTOUT == 1 + debugDivider = new PeriodicOperationDivider(10); +#endif } MGMHandlerRM3100::~MGMHandlerRM3100() {} @@ -140,7 +144,9 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( DeviceCommandId_t id, const uint8_t *packet) { - if(id == RM3100::READ_CMM) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + switch(id) { + case(RM3100::READ_CMM): { if(packet[1] == cmmRegValue) { commandExecuted = true; } @@ -149,13 +155,14 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( internalState = STATE_CONFIGURE_CMM; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } + break; } - else if(id == RM3100::READ_TMRC) { + case(RM3100::READ_TMRC): { if(packet[1] == tmrcRegValue) { commandExecuted = true; - // Reading TMRC was commanded. - if(mode == MODE_NORMAL) { - + // Reading TMRC was commanded. Trigger event to inform ground. + if(mode != _MODE_START_UP) { + triggerEvent(tmrcSet, tmrcRegValue, 0); } } else { @@ -163,22 +170,33 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( internalState = STATE_CONFIGURE_TMRC; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } + break; } - 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; + case(RM3100::READ_CYCLE_COUNT): { + uint16_t cycleCountX = packet[1] << 8 | packet[2]; + uint16_t cycleCountY = packet[3] << 8 | packet[4]; + uint16_t cycleCountZ = packet[5] << 8 | packet[6]; + if(cycleCountX != cycleCountRegValueX or + cycleCountY != cycleCountRegValueY or + cycleCountZ != cycleCountRegValueZ) { + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + // Reading TMRC was commanded. Trigger event to inform ground. + if(mode != _MODE_START_UP) { + uint32_t eventParam1 = cycleCountX << 16 | cycleCountY; + triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); + } + break; } - else { + case(RM3100::READ_DATA): { + result = handleDataReadout(packet); + break; + } + default: return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; } - return RETURN_OK; + return result; } ReturnValue_t MGMHandlerRM3100::handleCycleCountConfigCommand( @@ -252,3 +270,66 @@ ReturnValue_t MGMHandlerRM3100::handleTmrcConfigCommand( return HasReturnvaluesIF::RETURN_OK; } +void MGMHandlerRM3100::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 1); + insertInCommandAndReplyMap(RM3100::READ_CMM, 1); + + insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 1); + insertInCommandAndReplyMap(RM3100::READ_TMRC, 1); + + insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 1); + insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 1); + + insertInCommandAndReplyMap(RM3100::READ_DATA, 1, &primaryDataset); +} + +void MGMHandlerRM3100::modeChanged(void) { + internalState = STATE_NONE; +} + +ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( + LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, + new PoolEntry(0.0)); + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, + new PoolEntry(0.0)); + localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, + new PoolEntry(0.0)); + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) { + return 5000; +} + +ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) { + // 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; + +#if OBSW_ENHANCED_PRINTOUT == 1 + if(debugDivider->checkAndIncrement()) { + sif::info << "MGMHandlerLIS3: Magnetic field strength in" + " microtesla:" << std::endl; + // Set terminal to utf-8 if there is an issue with micro printout. + sif::info << "X: " << fieldStrengthX << " \xC2\xB5T" << std::endl; + sif::info << "Y: " << fieldStrengthY << " \xC2\xB5T" << std::endl; + sif::info << "Z: " << fieldStrengthZ << " \xC2\xB5T" << std::endl; + } +#endif + + ReturnValue_t result = primaryDataset.read(20); + if(result == HasReturnvaluesIF::RETURN_OK) { + primaryDataset.fieldStrengthX = fieldStrengthX; + primaryDataset.fieldStrengthY = fieldStrengthY; + primaryDataset.fieldStrengthZ = fieldStrengthZ; + primaryDataset.setValidity(true, true); + result = primaryDataset.commit(20); + } + return result; +} diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index 605ac890..58096a8d 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -6,10 +6,23 @@ #include +#if OBSW_ENHANCED_PRINTOUT == 1 +#include +#endif + 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; + + //! P1: TMRC value which was set, P2: 0 + static constexpr Event tmrcSet = event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, + 0x00, severity::INFO); + + //! P1: First two bytes new Cycle Count X + //! P1: Second two bytes new Cycle Count Y + //! P2: New cycle count Z + static constexpr Event cycleCountersSet = event::makeEvent( + SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO); MGMHandlerRM3100(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie); @@ -18,20 +31,26 @@ public: protected: /* DeviceHandlerBase overrides */ - virtual ReturnValue_t buildTransitionDeviceCommand( + ReturnValue_t buildTransitionDeviceCommand( DeviceCommandId_t *id) override; - virtual void doStartUp() override; - virtual void doShutDown() override; - virtual ReturnValue_t buildNormalDeviceCommand( + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand( DeviceCommandId_t *id) override; - virtual ReturnValue_t buildCommandFromCommand( + ReturnValue_t buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) override; - virtual ReturnValue_t scanForReply(const uint8_t *start, size_t len, + ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(LocalDataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + private: enum InternalState { @@ -48,6 +67,7 @@ private: }; InternalState internalState = InternalState::STATE_NONE; bool commandExecuted = false; + RM3100::Rm3100PrimaryDataset primaryDataset; uint8_t commandBuffer[10]; uint8_t commandBufferLen = 0; @@ -68,6 +88,11 @@ private: ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, const uint8_t *commandData,size_t commandDataLen); + + ReturnValue_t handleDataReadout(const uint8_t* packet); +#if OBSW_ENHANCED_PRINTOUT == 1 + PeriodicOperationDivider* debugDivider; +#endif }; #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */ From 1ed82f6a4b8d984f4006cab77615a7cd3119acc7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 00:30:50 +0100 Subject: [PATCH 10/20] added gyro handler stub --- mission/devices/GyroHandler.cpp | 57 ++++++++++++++++++++++++++++ mission/devices/GyroHandler.h | 41 ++++++++++++++++++++ mission/devices/MGMHandlerRM3100.cpp | 6 +-- 3 files changed, 101 insertions(+), 3 deletions(-) create mode 100644 mission/devices/GyroHandler.cpp create mode 100644 mission/devices/GyroHandler.h diff --git a/mission/devices/GyroHandler.cpp b/mission/devices/GyroHandler.cpp new file mode 100644 index 00000000..f3064ebc --- /dev/null +++ b/mission/devices/GyroHandler.cpp @@ -0,0 +1,57 @@ +#include "GyroHandler.h" + +GyroHandler::GyroHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie): + DeviceHandlerBase(objectId, deviceCommunication, comCookie) { +} + +GyroHandler::~GyroHandler() {} + +void GyroHandler::doStartUp() { + +} + +void GyroHandler::doShutDown() { + +} + +ReturnValue_t GyroHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GyroHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GyroHandler::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GyroHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GyroHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t GyroHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return 5000; +} + +ReturnValue_t GyroHandler::initializeLocalDataPool( + LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + return HasReturnvaluesIF::RETURN_OK; +} + +void GyroHandler::fillCommandAndReplyMap() { + +} + +void GyroHandler::modeChanged() { + +} diff --git a/mission/devices/GyroHandler.h b/mission/devices/GyroHandler.h new file mode 100644 index 00000000..aa8be90c --- /dev/null +++ b/mission/devices/GyroHandler.h @@ -0,0 +1,41 @@ +#ifndef MISSION_DEVICES_GYROHANDLER_H_ +#define MISSION_DEVICES_GYROHANDLER_H_ + +#include + +class GyroHandler: public DeviceHandlerBase { +public: + GyroHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF* comCookie); + virtual ~GyroHandler(); + +protected: + + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand( + DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand( + DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged() override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(LocalDataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + +private: + +}; + + + +#endif /* MISSION_DEVICES_GYROHANDLER_H_ */ diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 6040afad..4051e107 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -290,11 +290,11 @@ void MGMHandlerRM3100::modeChanged(void) { ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, - new PoolEntry(0.0)); + new PoolEntry({0.0})); localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, - new PoolEntry(0.0)); + new PoolEntry({0.0})); localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, - new PoolEntry(0.0)); + new PoolEntry({0.0})); return HasReturnvaluesIF::RETURN_OK; } From b55f857634d5eb3d16ac61886b87fffa1ab9173d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 00:32:11 +0100 Subject: [PATCH 11/20] added gps handler stub --- mission/devices/GPSHandler.cpp | 57 ++++++++++++++++++++++++++++++++++ mission/devices/GPSHandler.h | 41 ++++++++++++++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 mission/devices/GPSHandler.cpp create mode 100644 mission/devices/GPSHandler.h diff --git a/mission/devices/GPSHandler.cpp b/mission/devices/GPSHandler.cpp new file mode 100644 index 00000000..fb549eb7 --- /dev/null +++ b/mission/devices/GPSHandler.cpp @@ -0,0 +1,57 @@ +#include "GPSHandler.h" + +GPSHandler::GPSHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie): + DeviceHandlerBase(objectId, deviceCommunication, comCookie) { +} + +GPSHandler::~GPSHandler() {} + +void GPSHandler::doStartUp() { + +} + +void GPSHandler::doShutDown() { + +} + +ReturnValue_t GPSHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GPSHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GPSHandler::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GPSHandler::scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GPSHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t GPSHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return 5000; +} + +ReturnValue_t GPSHandler::initializeLocalDataPool( + LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + return HasReturnvaluesIF::RETURN_OK; +} + +void GPSHandler::fillCommandAndReplyMap() { + +} + +void GPSHandler::modeChanged() { + +} diff --git a/mission/devices/GPSHandler.h b/mission/devices/GPSHandler.h new file mode 100644 index 00000000..52ac25be --- /dev/null +++ b/mission/devices/GPSHandler.h @@ -0,0 +1,41 @@ +#ifndef MISSION_DEVICES_GPSHANDLER_H_ +#define MISSION_DEVICES_GPSHANDLER_H_ + +#include + +class GPSHandler: public DeviceHandlerBase { +public: + GPSHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF* comCookie); + virtual ~GPSHandler(); + +protected: + + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand( + DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand( + DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged() override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(LocalDataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + +private: + +}; + + + +#endif /* MISSION_DEVICES_GPSHANDLER_H_ */ From 483481b44040195d2e18af04e311e75c8d98a7d0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 00:36:39 +0100 Subject: [PATCH 12/20] replaced last sent command with getPendingCommand() --- mission/devices/MGMHandlerLIS3MDL.cpp | 6 +----- mission/devices/MGMHandlerLIS3MDL.h | 7 ------- 2 files changed, 1 insertion(+), 12 deletions(-) diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 35b71c79..7bb4fec1 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -105,13 +105,11 @@ ReturnValue_t MGMHandlerLIS3MDL::buildNormalDeviceCommand( DeviceCommandId_t *id) { // Data/config register will be read in an alternating manner. if(communicationStep == CommunicationStep::DATA) { - lastSentCommand = MGMLIS3MDL::READ_CONFIG_AND_DATA; *id = MGMLIS3MDL::READ_CONFIG_AND_DATA; communicationStep = CommunicationStep::TEMPERATURE; return buildCommandFromCommand(*id, NULL, 0); } else { - lastSentCommand = MGMLIS3MDL::READ_TEMPERATURE; *id = MGMLIS3MDL::READ_TEMPERATURE; communicationStep = CommunicationStep::DATA; return buildCommandFromCommand(*id, NULL, 0); @@ -122,7 +120,6 @@ ReturnValue_t MGMHandlerLIS3MDL::buildNormalDeviceCommand( ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { - lastSentCommand = deviceCommand; switch(deviceCommand) { case(MGMLIS3MDL::READ_CONFIG_AND_DATA): { std::memset(commandBuffer, 0, sizeof(commandBuffer)); @@ -154,7 +151,6 @@ ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand( return setOperatingMode(commandData, commandDataLen); } default: - lastSentCommand = DeviceHandlerIF::NO_COMMAND; return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } return HasReturnvaluesIF::RETURN_FAILED; @@ -198,7 +194,7 @@ ReturnValue_t MGMHandlerLIS3MDL::scanForReply(const uint8_t *start, } else if (len == SINGLE_COMMAND_ANSWER_LEN) { *foundLen = len; - *foundId = lastSentCommand; + *foundId = getPendingCommand(); } else { return DeviceHandlerIF::INVALID_DATA; diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h index c48b1fc2..875d453d 100644 --- a/mission/devices/MGMHandlerLIS3MDL.h +++ b/mission/devices/MGMHandlerLIS3MDL.h @@ -137,13 +137,6 @@ private: uint8_t statusRegister = 0; - - /** - * As this is a SPI Device, we get the Answer of the last sent command in - * the next read cycle, so we could check the command for identification. - */ - DeviceCommandId_t lastSentCommand = DeviceHandlerIF::NO_COMMAND; - /** * We always update all registers together, so this method updates * the rawpacket and rawpacketLen, so we just manipulate the local From afb6ee829ff3b570940795c3a2cb3ad59bf940e0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 13:32:40 +0100 Subject: [PATCH 13/20] added top comment --- mission/devices/MGMHandlerLIS3MDL.h | 2 +- mission/devices/MGMHandlerRM3100.h | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h index 875d453d..e5b29284 100644 --- a/mission/devices/MGMHandlerLIS3MDL.h +++ b/mission/devices/MGMHandlerLIS3MDL.h @@ -44,7 +44,7 @@ protected: size_t commandDataLen) override; ReturnValue_t buildTransitionDeviceCommand( DeviceCommandId_t *id) override; - ReturnValue_t buildNormalDeviceCommand( + ReturnValue_t buildNormalDeviceCommand( DeviceCommandId_t *id) override; ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index 58096a8d..124fc1a2 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -10,6 +10,13 @@ #include #endif +/** + * @brief Device Handler for the RM3100 geomagnetic magnetometer sensor + * (https://www.pnicorp.com/rm3100/) + * @details + * Advanced documentation: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM + */ class MGMHandlerRM3100: public DeviceHandlerBase { public: static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100; From 23ce7dc5c3fade02ec45de844308ad1439dcb2f6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 13:36:10 +0100 Subject: [PATCH 14/20] added top comment block --- mission/devices/GyroHandler.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/mission/devices/GyroHandler.h b/mission/devices/GyroHandler.h index aa8be90c..5ced880e 100644 --- a/mission/devices/GyroHandler.h +++ b/mission/devices/GyroHandler.h @@ -3,6 +3,13 @@ #include +/** + * @brief Device Handler for the L3GD20H gyroscope sensor + * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) + * @details + * Advanced documentation: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/L3GD20H_Gyro + */ class GyroHandler: public DeviceHandlerBase { public: GyroHandler(object_id_t objectId, object_id_t deviceCommunication, From 6f3a1c845c5410f36a3ce5bc6330ef5f28c8cc39 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 13:41:01 +0100 Subject: [PATCH 15/20] started gyro handler, renamed --- .../{GyroHandler.cpp => GyroL3GD20Handler.cpp} | 13 +++++++++++-- .../devices/{GyroHandler.h => GyroL3GD20Handler.h} | 14 +++++++++++--- .../devicedefinitions/GyroL3GD20Definitions.h | 11 +++++++++++ 3 files changed, 33 insertions(+), 5 deletions(-) rename mission/devices/{GyroHandler.cpp => GyroL3GD20Handler.cpp} (79%) rename mission/devices/{GyroHandler.h => GyroL3GD20Handler.h} (82%) create mode 100644 mission/devices/devicedefinitions/GyroL3GD20Definitions.h diff --git a/mission/devices/GyroHandler.cpp b/mission/devices/GyroL3GD20Handler.cpp similarity index 79% rename from mission/devices/GyroHandler.cpp rename to mission/devices/GyroL3GD20Handler.cpp index f3064ebc..772d0263 100644 --- a/mission/devices/GyroHandler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -1,4 +1,4 @@ -#include "GyroHandler.h" +#include GyroHandler::GyroHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie): @@ -8,11 +8,20 @@ GyroHandler::GyroHandler(object_id_t objectId, object_id_t deviceCommunication, GyroHandler::~GyroHandler() {} void GyroHandler::doStartUp() { + if(internalState == InternalState::STATE_NONE) { + internalState = InternalState::STATE_CONFIGURE; + } + if(internalState == InternalState::STATE_CONFIGURE) { + if(commandExecuted) { + internalState = InternalState::STATE_NORMAL; + commandExecuted = false; + } + } } void GyroHandler::doShutDown() { - + setMode(_MODE_POWER_DOWN); } ReturnValue_t GyroHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { diff --git a/mission/devices/GyroHandler.h b/mission/devices/GyroL3GD20Handler.h similarity index 82% rename from mission/devices/GyroHandler.h rename to mission/devices/GyroL3GD20Handler.h index 5ced880e..e1ced8d7 100644 --- a/mission/devices/GyroHandler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -1,5 +1,5 @@ -#ifndef MISSION_DEVICES_GYROHANDLER_H_ -#define MISSION_DEVICES_GYROHANDLER_H_ +#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ +#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ #include @@ -41,8 +41,16 @@ protected: private: + enum InternalState { + STATE_NONE, + STATE_CONFIGURE, + STATE_NORMAL + }; + InternalState internalState = InternalState::STATE_NONE; + bool commandExecuted = false; + }; -#endif /* MISSION_DEVICES_GYROHANDLER_H_ */ +#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h new file mode 100644 index 00000000..598803fa --- /dev/null +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -0,0 +1,11 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ + +namespace L3GD20H { + + +} + + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ From e527aa52f6e950f68b69689f21a05e777c2c1e38 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Dec 2020 13:44:32 +0100 Subject: [PATCH 16/20] started with l3dg20h definitions --- .../devicedefinitions/GyroL3GD20Definitions.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index 598803fa..d331a8d9 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -1,8 +1,23 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ +#include + namespace L3GD20H { +static constexpr uint8_t READ_MASK = 0b1000'0000; + +static constexpr uint8_t AUTO_INCREMENT_MASK = 0b0100'0000; + +static constexpr uint8_t WHO_AM_I_REG = 0b0000'1111; +static constexpr uint8_t WHO_AM_I_VAL = 0b1101'0111; + +static constexpr uint8_t CTRL_REG_1 = 0b0010'0000; +static constexpr uint8_t CTRL_REG_2 = 0b0010'0001; +static constexpr uint8_t CTRL_REG_3 = 0b0010'0010; +static constexpr uint8_t CTRL_REG_4 = 0b0010'0011; +static constexpr uint8_t CTRL_REG_5 = 0b0010'0100; + } From efd0823da2c082547526c7bf9ad506d18f78cda7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 23 Dec 2020 20:17:39 +0100 Subject: [PATCH 17/20] gyro handler finished --- Makefile | 2 +- fsfw | 2 +- mission/devices/GyroL3GD20Handler.cpp | 163 +++++++++++++++++- mission/devices/GyroL3GD20Handler.h | 15 +- mission/devices/MGMHandlerLIS3MDL.cpp | 37 ++-- mission/devices/MGMHandlerLIS3MDL.h | 10 +- mission/devices/MGMHandlerRM3100.cpp | 62 ++++--- mission/devices/MGMHandlerRM3100.h | 2 +- .../devicedefinitions/GyroL3GD20Definitions.h | 103 +++++++++++ 9 files changed, 345 insertions(+), 51 deletions(-) diff --git a/Makefile b/Makefile index 04a263ef..5380c5da 100644 --- a/Makefile +++ b/Makefile @@ -278,7 +278,7 @@ cleanbin: -rm -rf $(BUILDPATH)/$(OUTPUT_FOLDER) # Build target configuration -release: OPTIMIZATION = -Os $(PROTOTYPE_OPTIMIZATION) $(LINK_TIME_OPTIMIZATION) +release: OPTIMIZATION = -O2 $(PROTOTYPE_OPTIMIZATION) $(LINK_TIME_OPTIMIZATION) release: LINK_TIME_OPTIMIZATION = -flto release: TARGET = Release release: OPTIMIZATION_MESSAGE = On with Link Time Optimization diff --git a/fsfw b/fsfw index 31b82975..dcc111e4 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit 31b82975c7ef701fa7f1ba3413cfa19cc73aa2ca +Subproject commit dcc111e4facf39137fe52d8234361b7d99bdde06 diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp index 772d0263..b6067e5b 100644 --- a/mission/devices/GyroL3GD20Handler.cpp +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -2,7 +2,8 @@ GyroHandler::GyroHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie) { + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this) { } GyroHandler::~GyroHandler() {} @@ -25,42 +26,192 @@ void GyroHandler::doShutDown() { } ReturnValue_t GyroHandler::buildTransitionDeviceCommand(DeviceCommandId_t *id) { + switch(internalState) { + case(InternalState::STATE_NONE): + case(InternalState::STATE_NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::STATE_CONFIGURE): { + *id = L3GD20H::CONFIGURE_CTRL_REGS; + uint8_t command [5]; + command[0] = L3GD20H::CTRL_REG_1_VAL; + command[1] = L3GD20H::CTRL_REG_2_VAL; + command[2] = L3GD20H::CTRL_REG_3_VAL; + command[3] = L3GD20H::CTRL_REG_4_VAL; + command[4] = L3GD20H::CTRL_REG_5_VAL; + return buildCommandFromCommand(*id, command, 5); + break; + } + default: + // might be a configuration error. + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " + << "internal state!" << std::endl; + return HasReturnvaluesIF::RETURN_OK; + } return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GyroHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - return HasReturnvaluesIF::RETURN_OK; + *id = L3GD20H::READ_REGS; + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t GyroHandler::buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) { + switch(deviceCommand) { + case(L3GD20H::READ_REGS): { + commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | + L3GD20H::READ_MASK; + + std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN); + rawPacket = commandBuffer; + rawPacketLen = L3GD20H::READ_LEN + 1; + break; + } + case(L3GD20H::CONFIGURE_CTRL_REGS): { + commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK; + if(commandData == nullptr or commandDataLen != 5) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + ctrlReg1Value = commandData[0]; + ctrlReg2Value = commandData[1]; + ctrlReg3Value = commandData[2]; + ctrlReg4Value = commandData[3]; + ctrlReg5Value = commandData[4]; + + bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1; + bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0; + + if(not fsH and not fsL) { + scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; + } + else if(not fsH and fsL) { + scaleFactor = static_cast(L3GD20H::RANGE_DPS_01) / INT16_MAX; + } + else { + scaleFactor = static_cast(L3GD20H::RANGE_DPS_11) / INT16_MAX; + } + + commandBuffer[1] = ctrlReg1Value; + commandBuffer[2] = ctrlReg2Value; + commandBuffer[3] = ctrlReg3Value; + commandBuffer[4] = ctrlReg4Value; + commandBuffer[5] = ctrlReg5Value; + + rawPacket = commandBuffer; + rawPacketLen = 6; + break; + } + case(L3GD20H::READ_CTRL_REGS): { + commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | + L3GD20H::READ_MASK; + + std::memset(commandBuffer + 1, 0, 5); + rawPacket = commandBuffer; + rawPacketLen = 6; + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GyroHandler::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - return HasReturnvaluesIF::RETURN_OK; + // SPI, ID will always be the one of the last sent command. + *foundId = this->getPendingCommand(); + *foundLen = this->rawPacketLen; + + // Data with SPI Interface has always this answer + if (start[0] == 0b11111111) { + return HasReturnvaluesIF::RETURN_OK; + } + return DeviceHandlerIF::INVALID_DATA; } ReturnValue_t GyroHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { - return HasReturnvaluesIF::RETURN_OK; + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + switch(id) { + case(L3GD20H::CONFIGURE_CTRL_REGS): { + break; + } + case(L3GD20H::READ_CTRL_REGS): { + if(packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and + packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and + packet[5] == ctrlReg5Value) { + commandExecuted = true; + } + else { + // Attempt reconfiguration. + internalState = InternalState::STATE_CONFIGURE; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + case(L3GD20H::READ_START): { + if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and + packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and + packet[5] != ctrlReg5Value) { + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + + statusReg = packet[L3GD20H::STATUS_IDX]; + + float angVelocX = (packet[L3GD20H::OUT_X_H] << 8 | + packet[L3GD20H::OUT_X_L]) * scaleFactor; + float angVelocY = (packet[L3GD20H::OUT_Y_H] << 8 | + packet[L3GD20H::OUT_Y_L]) * scaleFactor; + float angVelocZ = (packet[L3GD20H::OUT_Z_H] << 8 | + packet[L3GD20H::OUT_Z_L]) * scaleFactor; + + int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; + float temperature = 25.0 + temperaturOffset; + + ReturnValue_t result = dataset.read(20); + if(result == HasReturnvaluesIF::RETURN_OK) { + dataset.angVelocX = angVelocX; + dataset.angVelocY = angVelocX; + dataset.angVelocZ = angVelocX; + dataset.temperature = temperature; + dataset.setValidity(true, true); + result = dataset.commit(20); + } + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return result; } + uint32_t GyroHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { return 5000; } ReturnValue_t GyroHandler::initializeLocalDataPool( LocalDataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, + new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, + new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, + new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::TEMPERATURE, + new PoolEntry({0.0})); return HasReturnvaluesIF::RETURN_OK; } void GyroHandler::fillCommandAndReplyMap() { - + insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); + insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); + insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); } void GyroHandler::modeChanged() { - + internalState = InternalState::STATE_NONE; } diff --git a/mission/devices/GyroL3GD20Handler.h b/mission/devices/GyroL3GD20Handler.h index e1ced8d7..5905b26a 100644 --- a/mission/devices/GyroL3GD20Handler.h +++ b/mission/devices/GyroL3GD20Handler.h @@ -2,6 +2,7 @@ #define MISSION_DEVICES_GYROL3GD20HANDLER_H_ #include +#include "devicedefinitions/GyroL3GD20Definitions.h" /** * @brief Device Handler for the L3GD20H gyroscope sensor @@ -40,8 +41,9 @@ protected: LocalDataPoolManager &poolManager) override; private: + L3GD20H::GyroPrimaryDataset dataset; - enum InternalState { + enum class InternalState { STATE_NONE, STATE_CONFIGURE, STATE_NORMAL @@ -49,6 +51,17 @@ private: InternalState internalState = InternalState::STATE_NONE; bool commandExecuted = false; + uint8_t statusReg = 0; + + uint8_t ctrlReg1Value = L3GD20H::CTRL_REG_1_VAL; + uint8_t ctrlReg2Value = L3GD20H::CTRL_REG_2_VAL; + uint8_t ctrlReg3Value = L3GD20H::CTRL_REG_3_VAL; + uint8_t ctrlReg4Value = L3GD20H::CTRL_REG_4_VAL; + uint8_t ctrlReg5Value = L3GD20H::CTRL_REG_5_VAL; + + uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; + + float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; }; diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 7bb4fec1..64b6d855 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -23,19 +23,19 @@ MGMHandlerLIS3MDL::~MGMHandlerLIS3MDL() { void MGMHandlerLIS3MDL::doStartUp() { switch (internalState) { - case STATE_NONE: - internalState = STATE_FIRST_CONTACT; + case(InternalState::STATE_NONE): + internalState = InternalState::STATE_FIRST_CONTACT; break; - case STATE_FIRST_CONTACT: - internalState = STATE_SETUP; + case(InternalState::STATE_FIRST_CONTACT): + internalState = InternalState::STATE_SETUP; break; - case STATE_SETUP: - internalState = STATE_CHECK_REGISTERS; + case(InternalState::STATE_SETUP): + internalState = InternalState::STATE_CHECK_REGISTERS; break; - case STATE_CHECK_REGISTERS: { + case(InternalState::STATE_CHECK_REGISTERS): { // Set up cached registers which will be used to configure the MGM. if(commandExecuted) { commandExecuted = false; @@ -56,20 +56,27 @@ void MGMHandlerLIS3MDL::doShutDown() { ReturnValue_t MGMHandlerLIS3MDL::buildTransitionDeviceCommand( DeviceCommandId_t *id) { switch (internalState) { - case STATE_FIRST_CONTACT: + case(InternalState::STATE_NONE): + case(InternalState::STATE_NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::STATE_FIRST_CONTACT): { *id = MGMLIS3MDL::IDENTIFY_DEVICE; break; - - case STATE_SETUP: + } + case(InternalState::STATE_SETUP): { *id = MGMLIS3MDL::SETUP_MGM; break; - - case STATE_CHECK_REGISTERS: + } + case(InternalState::STATE_CHECK_REGISTERS): { *id = MGMLIS3MDL::READ_CONFIG_AND_DATA; break; - + } default: - break; + // might be a configuration error. + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " + << "internal state!" << std::endl; + return HasReturnvaluesIF::RETURN_OK; } return buildCommandFromCommand(*id, NULL, 0); } @@ -412,7 +419,7 @@ uint32_t MGMHandlerLIS3MDL::getTransitionDelayMs(Mode_t from, Mode_t to) { } void MGMHandlerLIS3MDL::modeChanged(void) { - internalState = STATE_NONE; + internalState = InternalState::STATE_NONE; } ReturnValue_t MGMHandlerLIS3MDL::initializeLocalDataPool( diff --git a/mission/devices/MGMHandlerLIS3MDL.h b/mission/devices/MGMHandlerLIS3MDL.h index e5b29284..414731db 100644 --- a/mission/devices/MGMHandlerLIS3MDL.h +++ b/mission/devices/MGMHandlerLIS3MDL.h @@ -145,11 +145,15 @@ private: */ ReturnValue_t prepareCtrlRegisterWrite(); - enum InternalState { - STATE_NONE, STATE_FIRST_CONTACT, STATE_SETUP, STATE_CHECK_REGISTERS + enum class InternalState { + STATE_NONE, + STATE_FIRST_CONTACT, + STATE_SETUP, + STATE_CHECK_REGISTERS, + STATE_NORMAL }; - InternalState internalState = STATE_NONE; + InternalState internalState = InternalState::STATE_NONE; CommunicationStep communicationStep = CommunicationStep::DATA; bool commandExecuted = false; diff --git a/mission/devices/MGMHandlerRM3100.cpp b/mission/devices/MGMHandlerRM3100.cpp index 4051e107..eebbac39 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -17,25 +17,25 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId, MGMHandlerRM3100::~MGMHandlerRM3100() {} void MGMHandlerRM3100::doStartUp() { - if(internalState == STATE_NONE) { - internalState = STATE_CONFIGURE_CMM; + if(internalState == InternalState::STATE_NONE) { + internalState = InternalState::STATE_CONFIGURE_CMM; } - if(internalState == STATE_CONFIGURE_CMM) { - internalState = STATE_READ_CMM; + if(internalState == InternalState::STATE_CONFIGURE_CMM) { + internalState = InternalState::STATE_READ_CMM; } - else if(internalState == STATE_READ_CMM) { + else if(internalState == InternalState::STATE_READ_CMM) { if(commandExecuted) { - internalState = STATE_CONFIGURE_TMRC; + internalState = InternalState::STATE_CONFIGURE_TMRC; } } - if(internalState == STATE_CONFIGURE_TMRC) { - internalState = STATE_READ_TMRC; + if(internalState == InternalState::STATE_CONFIGURE_TMRC) { + internalState = InternalState::STATE_READ_TMRC; } - else if(internalState == STATE_READ_TMRC) { + else if(internalState == InternalState::STATE_READ_TMRC) { if(commandExecuted) { - internalState = STATE_NORMAL; + internalState = InternalState::STATE_NORMAL; setMode(_MODE_TO_ON); } } @@ -47,20 +47,32 @@ void MGMHandlerRM3100::doShutDown() { ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( DeviceCommandId_t *id) { - if(internalState == STATE_CONFIGURE_CMM) { + switch(internalState) { + case(InternalState::STATE_NONE): + case(InternalState::STATE_NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::STATE_CONFIGURE_CMM): { *id = RM3100::CONFIGURE_CMM; + break; } - - if(internalState == STATE_READ_CMM) { + case(InternalState::STATE_READ_CMM): { *id = RM3100::READ_CMM; + break; } - - if(internalState == STATE_CONFIGURE_TMRC) { + case(InternalState::STATE_CONFIGURE_TMRC): { *id = RM3100::CONFIGURE_TMRC; + break; } - - if(internalState == STATE_READ_TMRC) { + case(InternalState::STATE_READ_TMRC): { *id = RM3100::READ_TMRC; + break; + } + default: + // might be a configuration error. + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown " + << "internal state!" << std::endl; + return HasReturnvaluesIF::RETURN_OK; } return buildCommandFromCommand(*id, nullptr, 0); @@ -137,22 +149,26 @@ ReturnValue_t MGMHandlerRM3100::scanForReply(const uint8_t *start, if (start[0] == 0b11111111) { return RETURN_OK; } - else { - return DeviceHandlerIF::INVALID_DATA; - } + return DeviceHandlerIF::INVALID_DATA; } ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( DeviceCommandId_t id, const uint8_t *packet) { ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; switch(id) { + case(RM3100::CONFIGURE_CMM): + case(RM3100::CONFIGURE_CYCLE_COUNT): + case(RM3100::CONFIGURE_TMRC): { + // We can only check whether write was sucessful with read operation. + break; + } case(RM3100::READ_CMM): { if(packet[1] == cmmRegValue) { commandExecuted = true; } else { // Attempt reconfiguration. - internalState = STATE_CONFIGURE_CMM; + internalState = InternalState::STATE_CONFIGURE_CMM; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } break; @@ -167,7 +183,7 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( } else { // Attempt reconfiguration. - internalState = STATE_CONFIGURE_TMRC; + internalState = InternalState::STATE_CONFIGURE_TMRC; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } break; @@ -284,7 +300,7 @@ void MGMHandlerRM3100::fillCommandAndReplyMap() { } void MGMHandlerRM3100::modeChanged(void) { - internalState = STATE_NONE; + internalState = InternalState::STATE_NONE; } ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool( diff --git a/mission/devices/MGMHandlerRM3100.h b/mission/devices/MGMHandlerRM3100.h index 124fc1a2..d735ca45 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -60,7 +60,7 @@ protected: private: - enum InternalState { + enum class InternalState { STATE_NONE, STATE_CONFIGURE_CMM, STATE_READ_CMM, diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h index d331a8d9..c6042f58 100644 --- a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -1,6 +1,7 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ +#include #include namespace L3GD20H { @@ -12,12 +13,114 @@ static constexpr uint8_t AUTO_INCREMENT_MASK = 0b0100'0000; static constexpr uint8_t WHO_AM_I_REG = 0b0000'1111; static constexpr uint8_t WHO_AM_I_VAL = 0b1101'0111; +/*------------------------------------------------------------------------*/ +/* Control registers +/*------------------------------------------------------------------------*/ static constexpr uint8_t CTRL_REG_1 = 0b0010'0000; static constexpr uint8_t CTRL_REG_2 = 0b0010'0001; static constexpr uint8_t CTRL_REG_3 = 0b0010'0010; static constexpr uint8_t CTRL_REG_4 = 0b0010'0011; static constexpr uint8_t CTRL_REG_5 = 0b0010'0100; +// Register 1 +static constexpr uint8_t SET_DR_1 = 1 << 7; +static constexpr uint8_t SET_DR_0 = 1 << 6; +static constexpr uint8_t SET_BW_1 = 1 << 5; +static constexpr uint8_t SET_BW_0 = 1 << 4; +static constexpr uint8_t SET_POWER_NORMAL_MODE = 1 << 3; +static constexpr uint8_t SET_Z_ENABLE = 1 << 2; +static constexpr uint8_t SET_X_ENABLE = 1 << 1; +static constexpr uint8_t SET_Y_ENABLE = 1; + +static constexpr uint8_t CTRL_REG_1_VAL = SET_POWER_NORMAL_MODE | SET_Z_ENABLE | + SET_Y_ENABLE | SET_X_ENABLE; + +// Register 2 +static constexpr uint8_t EXTERNAL_EDGE_ENB = 1 << 7; +static constexpr uint8_t LEVEL_SENSITIVE_TRIGGER = 1 << 6; +static constexpr uint8_t SET_HPM_1 = 1 << 5; +static constexpr uint8_t SET_HPM_0 = 1 << 4; +static constexpr uint8_t SET_HPCF_3 = 1 << 3; +static constexpr uint8_t SET_HPCF_2 = 1 << 2; +static constexpr uint8_t SET_HPCF_1 = 1 << 1; +static constexpr uint8_t SET_HPCF_0 = 1; + +static constexpr uint8_t CTRL_REG_2_VAL = 0b0000'0000; + +// Register 3 +static constexpr uint8_t CTRL_REG_3_VAL = 0b0000'0000; + +// Register 4 +static constexpr uint8_t SET_BNU = 1 << 7; +static constexpr uint8_t SET_BLE = 1 << 6; +static constexpr uint8_t SET_FS_1 = 1 << 5; +static constexpr uint8_t SET_FS_0 = 1 << 4; +static constexpr uint8_t SET_IMP_ENB = 1 << 3; +static constexpr uint8_t SET_SELF_TEST_ENB_1 = 1 << 2; +static constexpr uint8_t SET_SELF_TEST_ENB_0 = 1 << 1; +static constexpr uint8_t SET_SPI_IF_SELECT = 1; + +static constexpr uint8_t CTRL_REG_4_VAL = 0b0000'0000; + +// Register 5 +static constexpr uint8_t SET_REBOOT_MEM = 1 << 7; +static constexpr uint8_t SET_FIFO_ENB = 1 << 6; + +static constexpr uint8_t CTRL_REG_5_VAL = 0b0000'0000; + +// In degrees per second (DPS) for now. +static constexpr uint16_t RANGE_DPS_00 = 245; +static constexpr uint16_t RANGE_DPS_01 = 500; +static constexpr uint16_t RANGE_DPS_11 = 2000; + +static constexpr uint8_t READ_START = CTRL_REG_1; +static constexpr size_t READ_LEN = 14; + +// Indexing +static constexpr uint8_t REFERENCE_IDX = 6; +static constexpr uint8_t TEMPERATURE_IDX = 7; +static constexpr uint8_t STATUS_IDX = 8; +static constexpr uint8_t OUT_X_L = 9; +static constexpr uint8_t OUT_X_H = 10; +static constexpr uint8_t OUT_Y_L = 11; +static constexpr uint8_t OUT_Y_H = 12; +static constexpr uint8_t OUT_Z_L = 13; +static constexpr uint8_t OUT_Z_H = 14; + +/*------------------------------------------------------------------------*/ +/* Device Handler specific +/*------------------------------------------------------------------------*/ +static constexpr DeviceCommandId_t READ_REGS = 0; +static constexpr DeviceCommandId_t CONFIGURE_CTRL_REGS = 1; +static constexpr DeviceCommandId_t READ_CTRL_REGS = 2; + +static constexpr uint32_t GYRO_DATASET_ID = READ_REGS; + +enum GyroPoolIds: lp_id_t { + ANG_VELOC_X, + ANG_VELOC_Y, + ANG_VELOC_Z, + TEMPERATURE +}; + +class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +public: + GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, GYRO_DATASET_ID) {} + + GyroPrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, GYRO_DATASET_ID)) {} + + // Angular velocities in degrees per second (DPS) + lp_var_t angVelocX = lp_var_t(sid.objectId, + ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, + ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, + ANG_VELOC_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, + TEMPERATURE, this); +}; } From 20fcebf7c88ab377ff72c547f0eeaeb04879af55 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 23 Dec 2020 20:20:22 +0100 Subject: [PATCH 18/20] fsfw update --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index dcc111e4..d7efa1b4 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit dcc111e4facf39137fe52d8234361b7d99bdde06 +Subproject commit d7efa1b4d717a576659044781e8520e1b9c97f7e From 1be985cb49641e6bc5d9003962b1db55ec107846 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 23 Dec 2020 20:26:37 +0100 Subject: [PATCH 19/20] valid fsfw now --- fsfw | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fsfw b/fsfw index d7efa1b4..dcc111e4 160000 --- a/fsfw +++ b/fsfw @@ -1 +1 @@ -Subproject commit d7efa1b4d717a576659044781e8520e1b9c97f7e +Subproject commit dcc111e4facf39137fe52d8234361b7d99bdde06 From 3688b2b4659101c825e916cd4d35217bf493d77e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 27 Dec 2020 17:04:15 +0100 Subject: [PATCH 20/20] tmtc update --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index e0c896e6..9be8713f 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit e0c896e62d25286d00599ca57b71d0a3495ed95f +Subproject commit 9be8713fa0cc6128e9469da2e635365b03146de3