diff --git a/devicehandlers/GyroL3GD20Handler.cpp b/devicehandlers/GyroL3GD20Handler.cpp index 94e61cc..297b0d1 100644 --- a/devicehandlers/GyroL3GD20Handler.cpp +++ b/devicehandlers/GyroL3GD20Handler.cpp @@ -3,9 +3,9 @@ #include GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie): - DeviceHandlerBase(objectId, deviceCommunication, comCookie), - dataset(this) { + CookieIF *comCookie): + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this) { #if L3GD20_GYRO_DEBUG == 1 debugDivider = new PeriodicOperationDivider(5); #endif @@ -14,188 +14,188 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} void GyroHandlerL3GD20H::doStartUp() { - if(internalState == InternalState::NONE) { - internalState = InternalState::CONFIGURE; - } + if(internalState == InternalState::NONE) { + internalState = InternalState::CONFIGURE; + } - if(internalState == InternalState::CONFIGURE) { - if(commandExecuted) { - internalState = InternalState::CHECK_REGS; - commandExecuted = false; - } - } + if(internalState == InternalState::CONFIGURE) { + if(commandExecuted) { + internalState = InternalState::CHECK_REGS; + commandExecuted = false; + } + } - if(internalState == InternalState::CHECK_REGS) { - if(commandExecuted) { - internalState = InternalState::NORMAL; + if(internalState == InternalState::CHECK_REGS) { + if(commandExecuted) { + internalState = InternalState::NORMAL; #if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 setMode(MODE_NORMAL); #else setMode(_MODE_TO_ON); #endif - commandExecuted = false; - } - } + commandExecuted = false; + } + } } void GyroHandlerL3GD20H::doShutDown() { - setMode(_MODE_POWER_DOWN); + setMode(_MODE_POWER_DOWN); } ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { - switch(internalState) { - case(InternalState::NONE): - case(InternalState::NORMAL): { - return HasReturnvaluesIF::RETURN_OK; - } - case(InternalState::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); - } - case(InternalState::CHECK_REGS): { - *id = L3GD20H::READ_REGS; - return buildCommandFromCommand(*id, nullptr, 0); - } - default: + switch(internalState) { + case(InternalState::NONE): + case(InternalState::NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::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); + } + case(InternalState::CHECK_REGS): { + *id = L3GD20H::READ_REGS; + return buildCommandFromCommand(*id, nullptr, 0); + } + default: #if FSFW_CPP_OSTREAM_ENABLED == 1 - /* Might be a configuration error. */ - sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << - std::endl; + /* Might be a configuration error. */ + sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << + std::endl; #else - sif::printDebug("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n"); + sif::printDebug("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n"); #endif - return HasReturnvaluesIF::RETURN_OK; - } - return HasReturnvaluesIF::RETURN_OK; + return HasReturnvaluesIF::RETURN_OK; + } + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = L3GD20H::READ_REGS; - return buildCommandFromCommand(*id, nullptr, 0); + *id = L3GD20H::READ_REGS; + return buildCommandFromCommand(*id, nullptr, 0); } ReturnValue_t GyroHandlerL3GD20H::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; + 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; - } + 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]; + 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; + 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; - } + if(not fsH and not fsL) { + sensitivity = L3GD20H::SENSITIVITY_00; + } + else if(not fsH and fsL) { + sensitivity = L3GD20H::SENSITIVITY_01; + } + else { + sensitivity = L3GD20H::SENSITIVITY_11; + } - commandBuffer[1] = ctrlReg1Value; - commandBuffer[2] = ctrlReg2Value; - commandBuffer[3] = ctrlReg3Value; - commandBuffer[4] = ctrlReg4Value; - commandBuffer[5] = ctrlReg5Value; + 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; + 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; + std::memset(commandBuffer + 1, 0, 5); + rawPacket = commandBuffer; + rawPacketLen = 6; + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, - DeviceCommandId_t *foundId, size_t *foundLen) { - /* For SPI, the ID will always be the one of the last sent command. */ - *foundId = this->getPendingCommand(); - *foundLen = this->rawPacketLen; + DeviceCommandId_t *foundId, size_t *foundLen) { + /* For SPI, the ID will always be the one of the last sent command. */ + *foundId = this->getPendingCommand(); + *foundLen = this->rawPacketLen; - return HasReturnvaluesIF::RETURN_OK; + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) { - ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - switch(id) { - case(L3GD20H::CONFIGURE_CTRL_REGS): { - commandExecuted = true; - 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::CONFIGURE; - return DeviceHandlerIF::DEVICE_REPLY_INVALID; - } - break; - } - case(L3GD20H::READ_REGS): { - 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; - } - else { - if(internalState == InternalState::CHECK_REGS) { - commandExecuted = true; - } - } + const uint8_t *packet) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + switch(id) { + case(L3GD20H::CONFIGURE_CTRL_REGS): { + commandExecuted = true; + 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::CONFIGURE; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + case(L3GD20H::READ_REGS): { + 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; + } + else { + if(internalState == InternalState::CHECK_REGS) { + commandExecuted = true; + } + } - statusReg = packet[L3GD20H::STATUS_IDX]; + statusReg = packet[L3GD20H::STATUS_IDX]; - int16_t angVelocXRaw = packet[L3GD20H::OUT_X_H] << 8 | packet[L3GD20H::OUT_X_L]; + int16_t angVelocXRaw = packet[L3GD20H::OUT_X_H] << 8 | packet[L3GD20H::OUT_X_L]; int16_t angVelocYRaw = packet[L3GD20H::OUT_Y_H] << 8 | packet[L3GD20H::OUT_Y_L]; int16_t angVelocZRaw = packet[L3GD20H::OUT_Z_H] << 8 | packet[L3GD20H::OUT_Z_L]; - float angVelocX = angVelocXRaw * scaleFactor; - float angVelocY = angVelocYRaw * scaleFactor; - float angVelocZ = angVelocZRaw * scaleFactor; + float angVelocX = angVelocXRaw * sensitivity; + float angVelocY = angVelocYRaw * sensitivity; + float angVelocZ = angVelocZRaw * sensitivity; - int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; - float temperature = 25.0 + temperaturOffset; + int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; + float temperature = 25.0 + temperaturOffset; #if L3GD20_GYRO_DEBUG == 1 if(debugDivider->checkAndIncrement()) { /* Set terminal to utf-8 if there is an issue with micro printout. */ @@ -214,46 +214,46 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, } #endif - PoolReadGuard readSet(&dataset); - if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { - dataset.angVelocX = angVelocX; - dataset.angVelocY = angVelocY; - dataset.angVelocZ = angVelocZ; - dataset.temperature = temperature; - dataset.setValidity(true, true); - } - break; - } - default: - return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; - } - return result; + PoolReadGuard readSet(&dataset); + if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + dataset.angVelocX = angVelocX; + dataset.angVelocY = angVelocY; + dataset.angVelocZ = angVelocZ; + dataset.temperature = temperature; + dataset.setValidity(true, true); + } + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return result; } uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 10000; + return 10000; } ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( - localpool::DataPool &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; + localpool::DataPool &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 GyroHandlerL3GD20H::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); - insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); - insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); + insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); + insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); + insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); } void GyroHandlerL3GD20H::modeChanged() { - internalState = InternalState::NONE; + internalState = InternalState::NONE; } diff --git a/devicehandlers/GyroL3GD20Handler.h b/devicehandlers/GyroL3GD20Handler.h index cc641c0..fb9baac 100644 --- a/devicehandlers/GyroL3GD20Handler.h +++ b/devicehandlers/GyroL3GD20Handler.h @@ -22,56 +22,57 @@ */ class GyroHandlerL3GD20H: public DeviceHandlerBase { public: - GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie); - virtual ~GyroHandlerL3GD20H(); + GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, + CookieIF* comCookie); + virtual ~GyroHandlerL3GD20H(); 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; + /* 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(localpool::DataPool &localDataPoolMap, - LocalDataPoolManager &poolManager) override; + void fillCommandAndReplyMap() override; + void modeChanged() override; + uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; private: - GyroPrimaryDataset dataset; + GyroPrimaryDataset dataset; - enum class InternalState { - NONE, - CONFIGURE, - CHECK_REGS, - NORMAL - }; - InternalState internalState = InternalState::NONE; - bool commandExecuted = false; + enum class InternalState { + NONE, + CONFIGURE, + CHECK_REGS, + NORMAL + }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; - uint8_t statusReg = 0; + 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 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]; + uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; - float scaleFactor = static_cast(L3GD20H::RANGE_DPS_00) / INT16_MAX; + // Set default value + float sensitivity = L3GD20H::SENSITIVITY_00; #if L3GD20_GYRO_DEBUG == 1 PeriodicOperationDivider* debugDivider = nullptr; diff --git a/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h b/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h index 00bb11b..489a45d 100644 --- a/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h +++ b/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h @@ -75,8 +75,11 @@ static constexpr uint8_t CTRL_REG_5_VAL = 0b00000000; /* Possible range values in degrees per second (DPS). */ static constexpr uint16_t RANGE_DPS_00 = 245; +static constexpr float SENSITIVITY_00 = 8.75; static constexpr uint16_t RANGE_DPS_01 = 500; +static constexpr float SENSITIVITY_01 = 17.5; static constexpr uint16_t RANGE_DPS_11 = 2000; +static constexpr float SENSITIVITY_11 = 70.0; static constexpr uint8_t READ_START = CTRL_REG_1; static constexpr size_t READ_LEN = 14; diff --git a/stm32h7/devicetest/GyroL3GD20H.cpp b/stm32h7/devicetest/GyroL3GD20H.cpp index ffcc698..54f69bc 100644 --- a/stm32h7/devicetest/GyroL3GD20H.cpp +++ b/stm32h7/devicetest/GyroL3GD20H.cpp @@ -15,7 +15,7 @@ alignas(32) std::array GyroL3GD20H::rxBuffer; alignas(32) std::array - GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer"))); +GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer"))); TransferStates transferState = TransferStates::IDLE; spi::TransferModes GyroL3GD20H::transferMode = spi::TransferModes::POLLING; @@ -24,7 +24,7 @@ DMA_HandleTypeDef txDmaHandle; DMA_HandleTypeDef rxDmaHandle; GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle, spi::TransferModes transferMode_): - spiHandle(spiHandle) { + spiHandle(spiHandle) { transferMode = transferMode_; if(transferMode == spi::TransferModes::DMA) { spi::setDmaHandles(&txDmaHandle, &rxDmaHandle); @@ -291,7 +291,6 @@ ReturnValue_t GyroL3GD20H::handlePollingTransferInit() { return HasReturnvaluesIF::RETURN_FAILED; } } - return HasReturnvaluesIF::RETURN_OK; sif::printInfo("GyroL3GD20H::initialize: Configuring device\n"); // Configure the 5 configuration registers @@ -317,7 +316,6 @@ ReturnValue_t GyroL3GD20H::handlePollingTransferInit() { return HasReturnvaluesIF::RETURN_FAILED; } } - return HasReturnvaluesIF::RETURN_OK; txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK; std::memset(txBuffer.data() + 1, 0 , 5); @@ -358,6 +356,7 @@ ReturnValue_t GyroL3GD20H::handlePollingSensorRead() { HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET); auto result = HAL_SPI_TransmitReceive(spiHandle, txBuffer.data(), rxBuffer.data(), 15, 1000); HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET); + switch(result) { case(HAL_OK): { handleSensorReadout(); @@ -484,22 +483,32 @@ void GyroL3GD20H::prepareConfigRegs(uint8_t* configRegs) { configRegs[0] = 0b00001111; configRegs[1] = 0b00000000; configRegs[2] = 0b00000000; - configRegs[3] = 0b01000000; // Big endian select + configRegs[3] = 0b01000000; configRegs[4] = 0b00000000; txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK; std::memcpy(txBuffer.data() + 1, configRegs, 5); } +uint8_t GyroL3GD20H::readRegPolling(uint8_t reg) { + uint8_t rxBuf[2] = {}; + uint8_t txBuf[2] = {}; + txBuf[0] = reg | STM_READ_MASK; + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET); + auto result = HAL_SPI_TransmitReceive(spiHandle, txBuf, rxBuf, 2, 1000); + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET); + return rxBuf[1]; +} + void GyroL3GD20H::handleSensorReadout() { uint8_t statusReg = rxBuffer[8]; int16_t gyroXRaw = rxBuffer[9] << 8 | rxBuffer[10]; - float gyroX = static_cast(gyroXRaw) / INT16_MAX * L3G_RANGE; + float gyroX = static_cast(gyroXRaw) * 0.00875; int16_t gyroYRaw = rxBuffer[11] << 8 | rxBuffer[12]; - float gyroY = static_cast(gyroYRaw) / INT16_MAX * L3G_RANGE; + float gyroY = static_cast(gyroYRaw) * 0.00875; int16_t gyroZRaw = rxBuffer[13] << 8 | rxBuffer[14]; - float gyroZ = static_cast(gyroZRaw) / INT16_MAX * L3G_RANGE; + float gyroZ = static_cast(gyroZRaw) * 0.00875; sif::printInfo("Status register: 0b" BYTE_TO_BINARY_PATTERN "\n", BYTE_TO_BINARY(statusReg)); sif::printInfo("Gyro X: %f\n", gyroX); sif::printInfo("Gyro Y: %f\n", gyroY); diff --git a/stm32h7/devicetest/GyroL3GD20H.h b/stm32h7/devicetest/GyroL3GD20H.h index 4ebed1e..641c938 100644 --- a/stm32h7/devicetest/GyroL3GD20H.h +++ b/stm32h7/devicetest/GyroL3GD20H.h @@ -50,9 +50,12 @@ private: ReturnValue_t handlePollingSensorRead(); ReturnValue_t handleInterruptSensorRead(); + uint8_t readRegPolling(uint8_t reg); + static void spiTransferCompleteCallback(SPI_HandleTypeDef *hspi, void* args); static void spiTransferErrorCallback(SPI_HandleTypeDef *hspi, void* args); + void prepareConfigRegs(uint8_t* configRegs); void handleSensorReadout(); };