From cf1d6bad195cf3df079c3d674f1f13dd48f82e13 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Jun 2021 14:41:02 +0200 Subject: [PATCH] performed sensor readout --- stm32h7/devicetest/GyroL3GD20H.cpp | 177 +++++++++++++++++++++++++---- stm32h7/devicetest/GyroL3GD20H.h | 11 +- 2 files changed, 165 insertions(+), 23 deletions(-) diff --git a/stm32h7/devicetest/GyroL3GD20H.cpp b/stm32h7/devicetest/GyroL3GD20H.cpp index bf11081..6b8fcc4 100644 --- a/stm32h7/devicetest/GyroL3GD20H.cpp +++ b/stm32h7/devicetest/GyroL3GD20H.cpp @@ -11,6 +11,8 @@ #include "stm32h7xx_hal_spi.h" #include "stm32h7xx_hal_rcc.h" +#include + alignas(32) std::array GyroL3GD20H::rxBuffer; alignas(32) std::array GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer"))); @@ -58,10 +60,7 @@ ReturnValue_t GyroL3GD20H::initialize() { sif::printWarning("Error initializing SPI\n"); return HasReturnvaluesIF::RETURN_FAILED; } - return HasReturnvaluesIF::RETURN_OK; -} -ReturnValue_t GyroL3GD20H::performOperation() { transferState = TransferStates::WAIT; sif::printInfo("GyroL3GD20H::performOperation: Reading WHO AM I register\n"); @@ -69,33 +68,85 @@ ReturnValue_t GyroL3GD20H::performOperation() { txBuffer[0] = WHO_AM_I_REG | STM_READ_MASK; txBuffer[1] = 0; - // Start SPI transfer via DMA - HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET); switch(transferMode) { case(spi::TransferModes::DMA): { - return handleDmaTransfer(); + return handleDmaTransferInit(); } case(spi::TransferModes::POLLING): { return handlePollingTransfer(); } default: { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t GyroL3GD20H::performOperation() { + switch(transferMode) { + case(spi::TransferModes::DMA): { + return handleDmaSensorRead(); + } + default: { + return HasReturnvaluesIF::RETURN_FAILED; } } return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t GyroL3GD20H::handleDmaTransfer() { +ReturnValue_t GyroL3GD20H::handleDmaTransferInit() { /* Clean D-cache */ /* Make sure the address is 32-byte aligned and add 32-bytes to length, in case it overlaps cacheline */ // See https://community.st.com/s/article/FAQ-DMA-is-not-working-on-STM32H7-devices -#if STM_USE_PERIPHERAL_TX_BUFFER_MPU_PROTECTION == 0 - SCB_CleanDCache_by_Addr((uint32_t*)(((uint32_t)txBuffer.data()) & ~(uint32_t)0x1F), - txBuffer.size()+32); -#endif + HAL_StatusTypeDef result = performDmaTransfer(2); + if(result != HAL_OK) { + // Transfer error in transmission process + sif::printWarning("GyroL3GD20H::initialize: Error transmitting SPI with DMA\n"); + } - if(HAL_SPI_TransmitReceive_DMA(spiHandle, txBuffer.data(), rxBuffer.data(), 2) != HAL_OK) { + // Wait for the transfer to complete + while (transferState == TransferStates::WAIT) { + TaskFactory::delayTask(1); + } + + switch(transferState) { + case(TransferStates::SUCCESS): { + uint8_t whoAmIVal = rxBuffer[1]; + if(whoAmIVal != EXPECTED_WHO_AM_I_VAL) { + sif::printDebug("GyroL3GD20H::initialize: " + "Read WHO AM I value %d not equal to expected value!\n", whoAmIVal); + } + transferState = TransferStates::IDLE; + break; + } + case(TransferStates::FAILURE): { + sif::printWarning("Transfer failure\n"); + transferState = TransferStates::FAILURE; + return HasReturnvaluesIF::RETURN_FAILED; + } + default: { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + + sif::printInfo("GyroL3GD20H::initialize: Configuring device\n"); + + // Configure the 5 configuration registers + uint8_t configRegs[5]; + // Enable sensor + configRegs[0] = 0b00001111; + configRegs[1] = 0b00000000; + configRegs[2] = 0b00000000; + configRegs[3] = 0b01000000; + // Big endian select + configRegs[4] = 0b00000000; + + txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK; + std::memcpy(txBuffer.data() + 1, configRegs, 5); + result = performDmaTransfer(6); + if(result != HAL_OK) { // Transfer error in transmission process sif::printWarning("Error transmitting SPI with DMA\n"); } @@ -105,24 +156,52 @@ ReturnValue_t GyroL3GD20H::handleDmaTransfer() { TaskFactory::delayTask(1); } - // Invalidate cache prior to access by CPU - SCB_InvalidateDCache_by_Addr ((uint32_t *)rxBuffer.data(), recvBufferSize); + switch(transferState) { + case(TransferStates::SUCCESS): { + sif::printInfo("GyroL3GD20H::initialize: Configuration transfer success\n"); + transferState = TransferStates::IDLE; + break; + } + case(TransferStates::FAILURE): { + sif::printWarning("GyroL3GD20H::initialize: Configuration transfer failure\n"); + transferState = TransferStates::FAILURE; + return HasReturnvaluesIF::RETURN_FAILED; + } + default: { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + + + txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK; + std::memset(txBuffer.data() + 1, 0 , 5); + result = performDmaTransfer(6); + if(result != HAL_OK) { + // Transfer error in transmission process + sif::printWarning("Error transmitting SPI with DMA\n"); + } + // Wait for the transfer to complete + while (transferState == TransferStates::WAIT) { + TaskFactory::delayTask(1); + } switch(transferState) { case(TransferStates::SUCCESS): { - sif::printInfo("DMA transfer success\n"); - uint8_t whoAmIVal = rxBuffer[1]; - if(whoAmIVal != EXPECTED_WHO_AM_I_VAL) { - sif::printDebug("GyroL3GD20H::performOperation: " - "Read WHO AM I value %d not equal to expected value!\n", whoAmIVal); + if(rxBuffer[1] != configRegs[0] or rxBuffer[2] != configRegs[1] or + rxBuffer[3] != configRegs[2] or rxBuffer[4] != configRegs[3] or + rxBuffer[5] != configRegs[4]) { + sif::printWarning("GyroL3GD20H::initialize: Configuration failure\n"); + } + else { + sif::printInfo("GyroL3GD20H::initialize: Configuration success\n"); } transferState = TransferStates::IDLE; break; } case(TransferStates::FAILURE): { - sif::printWarning("Transfer failure\n"); + sif::printWarning("GyroL3GD20H::initialize: Configuration transfer failure\n"); transferState = TransferStates::FAILURE; - break; + return HasReturnvaluesIF::RETURN_FAILED; } default: { return HasReturnvaluesIF::RETURN_FAILED; @@ -131,6 +210,59 @@ ReturnValue_t GyroL3GD20H::handleDmaTransfer() { return HasReturnvaluesIF::RETURN_OK; } +ReturnValue_t GyroL3GD20H::handleDmaSensorRead() { + txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK; + std::memset(txBuffer.data() + 1, 0 , 14); + + HAL_StatusTypeDef result = performDmaTransfer(15); + if(result != HAL_OK) { + // Transfer error in transmission process + sif::printWarning("Error transmitting SPI with DMA\n"); + } + // Wait for the transfer to complete + while (transferState == TransferStates::WAIT) { + TaskFactory::delayTask(1); + } + + switch(transferState) { + case(TransferStates::SUCCESS): { + uint8_t statusReg = rxBuffer[8]; + int16_t gyroXRaw = rxBuffer[9] << 8 | rxBuffer[10]; + float gyroX = static_cast(gyroXRaw) / INT16_MAX * L3G_RANGE; + int16_t gyroYRaw = rxBuffer[11] << 8 | rxBuffer[12]; + float gyroY = static_cast(gyroYRaw) / INT16_MAX * L3G_RANGE; + int16_t gyroZRaw = rxBuffer[13] << 8 | rxBuffer[14]; + float gyroZ = static_cast(gyroZRaw) / INT16_MAX * L3G_RANGE; + 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); + sif::printInfo("Gyro Z: %f\n", gyroZ); + break; + } + case(TransferStates::FAILURE): { + sif::printWarning("GyroL3GD20H::handleDmaSensorRead: Sensor read failure\n"); + transferState = TransferStates::FAILURE; + return HasReturnvaluesIF::RETURN_FAILED; + } + default: { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} + +HAL_StatusTypeDef GyroL3GD20H::performDmaTransfer(size_t sendSize) { + transferState = TransferStates::WAIT; +#if STM_USE_PERIPHERAL_TX_BUFFER_MPU_PROTECTION == 0 + SCB_CleanDCache_by_Addr((uint32_t*)(((uint32_t)txBuffer.data()) & ~(uint32_t)0x1F), + txBuffer.size()+32); +#endif + + // Start SPI transfer via DMA + HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET); + return HAL_SPI_TransmitReceive_DMA(spiHandle, txBuffer.data(), rxBuffer.data(), sendSize); +} + /** * @brief TxRx Transfer completed callback. * @param hspi: SPI handle @@ -141,6 +273,9 @@ ReturnValue_t GyroL3GD20H::handleDmaTransfer() { void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) { transferState = TransferStates::SUCCESS; HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET); + // Invalidate cache prior to access by CPU + SCB_InvalidateDCache_by_Addr ((uint32_t *)GyroL3GD20H::rxBuffer.data(), + GyroL3GD20H::recvBufferSize); } /** diff --git a/stm32h7/devicetest/GyroL3GD20H.h b/stm32h7/devicetest/GyroL3GD20H.h index 59c39b9..917d3ea 100644 --- a/stm32h7/devicetest/GyroL3GD20H.h +++ b/stm32h7/devicetest/GyroL3GD20H.h @@ -18,6 +18,7 @@ enum class TransferStates { }; class GyroL3GD20H { + friend void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi); public: GyroL3GD20H(SPI_HandleTypeDef* spiHandle, spi::TransferModes transferMode); @@ -28,17 +29,23 @@ private: const uint8_t WHO_AM_I_REG = 0b00001111; const uint8_t STM_READ_MASK = 0b10000000; + const uint8_t STM_AUTO_INCREMENT_MASK = 0b01000000; const uint8_t EXPECTED_WHO_AM_I_VAL = 0b11010111; + const uint8_t CTRL_REG_1 = 0b00100000; + const uint32_t L3G_RANGE = 245; SPI_HandleTypeDef* spiHandle; spi::TransferModes transferMode; - static constexpr size_t recvBufferSize = 32 * 10; + static constexpr size_t recvBufferSize = 32 * 10; static std::array rxBuffer; static constexpr size_t txBufferSize = 32; static std::array txBuffer; - ReturnValue_t handleDmaTransfer(); + ReturnValue_t handleDmaTransferInit(); + ReturnValue_t handleDmaSensorRead(); + HAL_StatusTypeDef performDmaTransfer(size_t sendSize); + ReturnValue_t handlePollingTransfer(); };