From 7e0c56edcba7adaee006a96cd271601096ea2544 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 23 Dec 2020 20:17:39 +0100 Subject: [PATCH] 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); +}; }