diff --git a/Makefile b/Makefile index eaa08f74..3956d5f1 100644 --- a/Makefile +++ b/Makefile @@ -279,7 +279,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/Makefile-Hosted b/Makefile-Hosted index 9dff3677..3a93f625 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 @@ -179,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 @@ -227,7 +235,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 @@ -261,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 @@ -309,7 +316,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 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/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/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 f2f04c26..ba0fed61 100644 --- a/fsfwconfig/OBSWConfig.h +++ b/fsfwconfig/OBSWConfig.h @@ -6,7 +6,10 @@ #ifndef FSFWCONFIG_OBSWCONFIG_H_ #define FSFWCONFIG_OBSWCONFIG_H_ -#define ADD_TEST_CODE 0 +#include "returnvalues/classIds.h" +#include "events/subsystemIdRanges.h" + +#define ADD_TEST_CODE 0 // These defines should be disabled for mission code but are useful for // debugging. 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/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 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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_ */ diff --git a/mission/devices/GyroL3GD20Handler.cpp b/mission/devices/GyroL3GD20Handler.cpp new file mode 100644 index 00000000..b6067e5b --- /dev/null +++ b/mission/devices/GyroL3GD20Handler.cpp @@ -0,0 +1,217 @@ +#include + +GyroHandler::GyroHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF *comCookie): + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this) { +} + +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) { + 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) { + *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) { + // 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) { + 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 new file mode 100644 index 00000000..5905b26a --- /dev/null +++ b/mission/devices/GyroL3GD20Handler.h @@ -0,0 +1,69 @@ +#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ +#define MISSION_DEVICES_GYROL3GD20HANDLER_H_ + +#include +#include "devicedefinitions/GyroL3GD20Definitions.h" + +/** + * @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, + 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: + L3GD20H::GyroPrimaryDataset dataset; + + enum class InternalState { + STATE_NONE, + STATE_CONFIGURE, + STATE_NORMAL + }; + 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; +}; + + + +#endif /* MISSION_DEVICES_GYROL3GD20HANDLER_H_ */ diff --git a/mission/devices/MGMHandlerLIS3MDL.cpp b/mission/devices/MGMHandlerLIS3MDL.cpp index 5de4f719..64b6d855 100644 --- a/mission/devices/MGMHandlerLIS3MDL.cpp +++ b/mission/devices/MGMHandlerLIS3MDL.cpp @@ -23,23 +23,23 @@ 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; - setMode(MODE_NORMAL); + setMode(_MODE_TO_ON); } break; } @@ -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); } @@ -105,13 +112,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 +127,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 +158,6 @@ ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand( return setOperatingMode(commandData, commandDataLen); } default: - lastSentCommand = DeviceHandlerIF::NO_COMMAND_ID; return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; } return HasReturnvaluesIF::RETURN_FAILED; @@ -198,7 +201,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; @@ -259,6 +262,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 +410,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) { } @@ -419,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 a5a1cadf..414731db 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; @@ -139,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_ID; - /** * We always update all registers together, so this method updates * the rawpacket and rawpacketLen, so we just manipulate the local @@ -154,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 fbcb578d..eebbac39 100644 --- a/mission/devices/MGMHandlerRM3100.cpp +++ b/mission/devices/MGMHandlerRM3100.cpp @@ -1,4 +1,5 @@ #include "MGMHandlerRM3100.h" + #include #include #include @@ -6,40 +7,345 @@ 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() {} -ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( - DeviceCommandId_t *id) { - return RETURN_OK; -} - void MGMHandlerRM3100::doStartUp() { + if(internalState == InternalState::STATE_NONE) { + internalState = InternalState::STATE_CONFIGURE_CMM; + } + + if(internalState == InternalState::STATE_CONFIGURE_CMM) { + internalState = InternalState::STATE_READ_CMM; + } + else if(internalState == InternalState::STATE_READ_CMM) { + if(commandExecuted) { + internalState = InternalState::STATE_CONFIGURE_TMRC; + } + } + + if(internalState == InternalState::STATE_CONFIGURE_TMRC) { + internalState = InternalState::STATE_READ_TMRC; + } + else if(internalState == InternalState::STATE_READ_TMRC) { + if(commandExecuted) { + internalState = InternalState::STATE_NORMAL; + setMode(_MODE_TO_ON); + } + } } void MGMHandlerRM3100::doShutDown() { + setMode(_MODE_POWER_DOWN); } -ReturnValue_t MGMHandlerRM3100::buildNormalDeviceCommand( +ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand( DeviceCommandId_t *id) { - return RETURN_OK; + switch(internalState) { + case(InternalState::STATE_NONE): + case(InternalState::STATE_NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::STATE_CONFIGURE_CMM): { + *id = RM3100::CONFIGURE_CMM; + break; + } + case(InternalState::STATE_READ_CMM): { + *id = RM3100::READ_CMM; + break; + } + case(InternalState::STATE_CONFIGURE_TMRC): { + *id = RM3100::CONFIGURE_TMRC; + break; + } + 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); } 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) { - return 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 RETURN_OK; + } + return DeviceHandlerIF::INVALID_DATA; } ReturnValue_t MGMHandlerRM3100::interpretDeviceReply( DeviceCommandId_t id, const uint8_t *packet) { - return RETURN_OK; + 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 = InternalState::STATE_CONFIGURE_CMM; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + case(RM3100::READ_TMRC): { + if(packet[1] == tmrcRegValue) { + commandExecuted = true; + // Reading TMRC was commanded. Trigger event to inform ground. + if(mode != _MODE_START_UP) { + triggerEvent(tmrcSet, tmrcRegValue, 0); + } + } + else { + // Attempt reconfiguration. + internalState = InternalState::STATE_CONFIGURE_TMRC; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + 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; + } + case(RM3100::READ_DATA): { + result = handleDataReadout(packet); + break; + } + default: + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + + return result; +} + +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; +} + +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 = 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 d2081fa1..d735ca45 100644 --- a/mission/devices/MGMHandlerRM3100.h +++ b/mission/devices/MGMHandlerRM3100.h @@ -4,8 +4,32 @@ #include "devicedefinitions/MGMHandlerRM3100Definitions.h" #include +#include + +#if OBSW_ENHANCED_PRINTOUT == 1 +#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; + + //! 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); @@ -14,26 +38,68 @@ 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 { - STATE_NONE, STATE_FIRST_CONTACT, STATE_SETUP, STATE_CHECK_REGISTERS + enum class InternalState { + 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; + RM3100::Rm3100PrimaryDataset primaryDataset; + + 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); + + ReturnValue_t handleDataReadout(const uint8_t* packet); +#if OBSW_ENHANCED_PRINTOUT == 1 + PeriodicOperationDivider* debugDivider; +#endif }; #endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/GyroL3GD20Definitions.h b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h new file mode 100644 index 00000000..c6042f58 --- /dev/null +++ b/mission/devices/devicedefinitions/GyroL3GD20Definitions.h @@ -0,0 +1,129 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ + +#include +#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; + +/*------------------------------------------------------------------------*/ +/* 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); +}; + +} + + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ diff --git a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h index 5bf9e8b3..60a107d8 100644 --- a/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h +++ b/mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h @@ -1,8 +1,127 @@ #ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ #define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ +#include +#include +#include +#include +#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 +/*----------------------------------------------------------------------------*/ +// Default value (200) +static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8; + +static constexpr float DEFAULT_GAIN = static_cast(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 uint16_t RANGE = 800; + +static constexpr DeviceCommandId_t READ_DATA = 0; + +static constexpr DeviceCommandId_t CONFIGURE_CMM = 1; +static constexpr DeviceCommandId_t READ_CMM = 2; + +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); +}; + }