fsfw-hal/stm32h7/devicetest/GyroL3GD20H.cpp

558 lines
19 KiB
C++
Raw Normal View History

2021-05-31 20:40:16 +02:00
#include "GyroL3GD20H.h"
2021-06-03 21:42:52 +02:00
#include "../spi/mspInit.h"
2021-06-03 14:00:50 +02:00
#include "../spi/spiDefinitions.h"
2021-06-03 14:38:34 +02:00
#include "../spi/spiCore.h"
2021-06-10 19:09:33 +02:00
#include "../spi/spiInterrupts.h"
#include "../spi/stm32h743ziSpi.h"
2021-05-31 20:40:16 +02:00
2021-06-03 14:00:50 +02:00
#include "fsfw/tasks/TaskFactory.h"
2021-05-31 20:40:16 +02:00
#include "fsfw/serviceinterface/ServiceInterface.h"
2021-06-03 14:00:50 +02:00
2021-05-31 20:40:16 +02:00
#include "stm32h7xx_nucleo.h"
#include "stm32h7xx_hal_spi.h"
2021-06-03 21:42:52 +02:00
#include "stm32h7xx_hal_rcc.h"
2021-05-31 20:40:16 +02:00
2021-06-04 14:41:02 +02:00
#include <cstring>
2021-06-04 12:58:30 +02:00
alignas(32) std::array<uint8_t, GyroL3GD20H::recvBufferSize> GyroL3GD20H::rxBuffer;
alignas(32) std::array<uint8_t, GyroL3GD20H::txBufferSize>
2021-06-10 14:17:13 +02:00
GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer")));
2021-06-04 12:58:30 +02:00
2021-06-03 14:00:50 +02:00
TransferStates transferState = TransferStates::IDLE;
2021-06-04 15:50:02 +02:00
spi::TransferModes GyroL3GD20H::transferMode = spi::TransferModes::POLLING;
2021-05-31 20:40:16 +02:00
2021-06-04 15:50:02 +02:00
GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle, spi::TransferModes transferMode_):
2021-06-10 19:09:33 +02:00
spiHandle(spiHandle) {
txDmaHandle = new DMA_HandleTypeDef();
rxDmaHandle = new DMA_HandleTypeDef();
spi::setSpiHandle(spiHandle);
2021-06-04 15:50:02 +02:00
transferMode = transferMode_;
2021-06-03 22:07:59 +02:00
if(transferMode == spi::TransferModes::DMA) {
2021-06-10 19:09:33 +02:00
mspCfg = new spi::MspDmaConfigStruct();
auto typedCfg = dynamic_cast<spi::MspDmaConfigStruct*>(mspCfg);
spi::setDmaHandles(txDmaHandle, rxDmaHandle);
spi::h743zi::standardDmaCfg(*typedCfg, IrqPriorities::HIGHEST_FREERTOS,
IrqPriorities::HIGHEST_FREERTOS, IrqPriorities::HIGHEST_FREERTOS);
spi::setSpiDmaMspFunctions(typedCfg);
2021-06-03 22:07:59 +02:00
}
2021-06-04 15:50:02 +02:00
else if(transferMode == spi::TransferModes::INTERRUPT) {
2021-06-10 19:09:33 +02:00
mspCfg = new spi::MspIrqConfigStruct();
auto typedCfg = dynamic_cast<spi::MspIrqConfigStruct*>(mspCfg);
spi::h743zi::standardInterruptCfg(*typedCfg, IrqPriorities::HIGHEST_FREERTOS);
spi::setSpiIrqMspFunctions(typedCfg);
2021-06-04 15:50:02 +02:00
}
2021-06-03 22:07:59 +02:00
else if(transferMode == spi::TransferModes::POLLING) {
2021-06-10 19:09:33 +02:00
mspCfg = new spi::MspPollingConfigStruct();
auto typedCfg = dynamic_cast<spi::MspPollingConfigStruct*>(mspCfg);
spi::h743zi::standardPollingCfg(*typedCfg);
spi::setSpiPollingMspFunctions(typedCfg);
2021-06-03 22:07:59 +02:00
}
2021-06-04 15:50:02 +02:00
2021-06-05 13:29:43 +02:00
spi::assignTransferRxTxCompleteCallback(&spiTransferCompleteCallback, nullptr);
spi::assignTransferErrorCallback(&spiTransferErrorCallback, nullptr);
2021-06-03 16:44:02 +02:00
GPIO_InitTypeDef chipSelect = {};
2021-06-03 21:42:52 +02:00
__HAL_RCC_GPIOD_CLK_ENABLE();
2021-06-03 16:44:02 +02:00
chipSelect.Pin = GPIO_PIN_14;
chipSelect.Mode = GPIO_MODE_OUTPUT_PP;
HAL_GPIO_Init(GPIOD, &chipSelect);
2021-06-03 21:42:52 +02:00
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
2021-05-31 20:40:16 +02:00
}
2021-06-10 19:09:33 +02:00
GyroL3GD20H::~GyroL3GD20H() {
delete txDmaHandle;
delete rxDmaHandle;
2021-06-10 21:31:21 +02:00
if(mspCfg != nullptr) {
delete mspCfg;
}
2021-06-10 19:09:33 +02:00
}
2021-06-03 14:00:50 +02:00
ReturnValue_t GyroL3GD20H::initialize() {
// Configure the SPI peripheral
2021-05-31 20:40:16 +02:00
spiHandle->Instance = SPI1;
2021-06-03 14:03:38 +02:00
spiHandle->Init.BaudRatePrescaler = spi::getPrescaler(HAL_RCC_GetHCLKFreq(), 3900000);
2021-05-31 20:40:16 +02:00
spiHandle->Init.Direction = SPI_DIRECTION_2LINES;
2021-06-10 12:05:49 +02:00
spi::assignSpiMode(spi::SpiModes::MODE_3, *spiHandle);
2021-05-31 20:40:16 +02:00
spiHandle->Init.DataSize = SPI_DATASIZE_8BIT;
spiHandle->Init.FirstBit = SPI_FIRSTBIT_MSB;
spiHandle->Init.TIMode = SPI_TIMODE_DISABLE;
spiHandle->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
spiHandle->Init.CRCPolynomial = 7;
spiHandle->Init.CRCLength = SPI_CRC_LENGTH_8BIT;
spiHandle->Init.NSS = SPI_NSS_SOFT;
spiHandle->Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
2021-06-03 14:00:50 +02:00
// Recommended setting to avoid glitches
2021-05-31 20:40:16 +02:00
spiHandle->Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_ENABLE;
spiHandle->Init.Mode = SPI_MODE_MASTER;
if(HAL_SPI_Init(spiHandle) != HAL_OK) {
sif::printWarning("Error initializing SPI\n");
2021-06-03 14:00:50 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-06-10 21:31:21 +02:00
delete mspCfg;
2021-06-03 14:00:50 +02:00
transferState = TransferStates::WAIT;
2021-06-03 16:44:02 +02:00
sif::printInfo("GyroL3GD20H::performOperation: Reading WHO AM I register\n");
txBuffer[0] = WHO_AM_I_REG | STM_READ_MASK;
txBuffer[1] = 0;
2021-06-03 22:07:59 +02:00
switch(transferMode) {
case(spi::TransferModes::DMA): {
2021-06-04 14:41:02 +02:00
return handleDmaTransferInit();
2021-06-03 22:07:59 +02:00
}
2021-06-04 15:50:02 +02:00
case(spi::TransferModes::INTERRUPT): {
return handleInterruptTransferInit();
}
2021-06-03 22:07:59 +02:00
case(spi::TransferModes::POLLING): {
2021-06-04 15:50:02 +02:00
return handlePollingTransferInit();
2021-06-03 22:07:59 +02:00
}
default: {
2021-06-04 14:41:02 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
}
2021-06-03 22:07:59 +02:00
2021-06-04 14:41:02 +02:00
return HasReturnvaluesIF::RETURN_OK;
}
ReturnValue_t GyroL3GD20H::performOperation() {
switch(transferMode) {
case(spi::TransferModes::DMA): {
return handleDmaSensorRead();
}
2021-06-04 15:50:02 +02:00
case(spi::TransferModes::POLLING): {
return handlePollingSensorRead();
}
case(spi::TransferModes::INTERRUPT): {
return handleInterruptSensorRead();
}
2021-06-04 14:41:02 +02:00
default: {
return HasReturnvaluesIF::RETURN_FAILED;
2021-06-03 22:07:59 +02:00
}
}
return HasReturnvaluesIF::RETURN_OK;
}
2021-06-04 14:41:02 +02:00
ReturnValue_t GyroL3GD20H::handleDmaTransferInit() {
2021-06-04 12:58:30 +02:00
/* 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
2021-06-04 14:41:02 +02:00
HAL_StatusTypeDef result = performDmaTransfer(2);
if(result != HAL_OK) {
2021-06-03 14:00:50 +02:00
// Transfer error in transmission process
2021-06-04 14:41:02 +02:00
sif::printWarning("GyroL3GD20H::initialize: Error transmitting SPI with DMA\n");
2021-06-03 14:00:50 +02:00
}
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
2021-06-04 12:58:30 +02:00
TaskFactory::delayTask(1);
2021-06-03 14:00:50 +02:00
}
switch(transferState) {
case(TransferStates::SUCCESS): {
2021-06-03 16:44:02 +02:00
uint8_t whoAmIVal = rxBuffer[1];
if(whoAmIVal != EXPECTED_WHO_AM_I_VAL) {
2021-06-04 14:41:02 +02:00
sif::printDebug("GyroL3GD20H::initialize: "
2021-06-03 21:42:52 +02:00
"Read WHO AM I value %d not equal to expected value!\n", whoAmIVal);
2021-06-03 16:44:02 +02:00
}
2021-06-03 14:00:50 +02:00
transferState = TransferStates::IDLE;
break;
}
case(TransferStates::FAILURE): {
sif::printWarning("Transfer failure\n");
transferState = TransferStates::FAILURE;
2021-06-04 14:41:02 +02:00
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];
2021-06-04 15:50:02 +02:00
prepareConfigRegs(configRegs);
2021-06-04 14:41:02 +02:00
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("GyroL3GD20H::initialize: Configuration transfer success\n");
transferState = TransferStates::IDLE;
2021-06-03 14:00:50 +02:00
break;
}
2021-06-04 14:41:02 +02:00
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): {
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("GyroL3GD20H::initialize: Configuration transfer failure\n");
transferState = TransferStates::FAILURE;
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-06-03 14:00:50 +02:00
default: {
2021-06-03 22:07:59 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
2021-06-03 14:00:50 +02:00
}
2021-05-31 20:40:16 +02:00
}
return HasReturnvaluesIF::RETURN_OK;
}
2021-06-03 14:00:50 +02:00
2021-06-04 14:41:02 +02:00
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
2021-06-04 15:50:02 +02:00
sif::printDebug("GyroL3GD20H::handleDmaSensorRead: Error transmitting SPI with DMA\n");
2021-06-04 14:41:02 +02:00
}
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
TaskFactory::delayTask(1);
}
switch(transferState) {
case(TransferStates::SUCCESS): {
2021-06-04 15:50:02 +02:00
handleSensorReadout();
2021-06-04 14:41:02 +02:00
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);
}
2021-06-04 15:50:02 +02:00
ReturnValue_t GyroL3GD20H::handlePollingTransferInit() {
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
auto result = HAL_SPI_TransmitReceive(spiHandle, txBuffer.data(), rxBuffer.data(), 2, 1000);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
switch(result) {
case(HAL_OK): {
sif::printInfo("GyroL3GD20H::initialize: Polling 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);
}
break;
}
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer timeout\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
case(HAL_ERROR): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer failure\n");
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];
prepareConfigRegs(configRegs);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
result = HAL_SPI_TransmitReceive(spiHandle, txBuffer.data(), rxBuffer.data(), 6, 1000);
2021-06-03 21:42:52 +02:00
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
2021-06-04 15:50:02 +02:00
switch(result) {
case(HAL_OK): {
break;
}
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer timeout\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
case(HAL_ERROR): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer failure\n");
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);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
result = HAL_SPI_TransmitReceive(spiHandle, txBuffer.data(), rxBuffer.data(), 6, 1000);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
switch(result) {
case(HAL_OK): {
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");
}
break;
}
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer timeout\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
case(HAL_ERROR): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer failure\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
default: {
return HasReturnvaluesIF::RETURN_FAILED;
}
}
return HasReturnvaluesIF::RETURN_OK;
2021-06-03 14:00:50 +02:00
}
2021-06-04 15:50:02 +02:00
ReturnValue_t GyroL3GD20H::handlePollingSensorRead() {
txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK;
std::memset(txBuffer.data() + 1, 0 , 14);
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);
2021-06-10 14:17:13 +02:00
2021-06-04 15:50:02 +02:00
switch(result) {
case(HAL_OK): {
handleSensorReadout();
break;
}
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer timeout\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
case(HAL_ERROR): {
sif::printDebug("GyroL3GD20H::initialize: Polling transfer failure\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
default: {
return HasReturnvaluesIF::RETURN_FAILED;
}
}
return HasReturnvaluesIF::RETURN_OK;
2021-06-03 14:00:50 +02:00
}
2021-06-03 22:07:59 +02:00
2021-06-04 15:50:02 +02:00
ReturnValue_t GyroL3GD20H::handleInterruptTransferInit() {
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
switch(HAL_SPI_TransmitReceive_IT(spiHandle, txBuffer.data(), rxBuffer.data(), 2)) {
2021-06-03 22:07:59 +02:00
case(HAL_OK): {
2021-06-04 15:50:02 +02:00
sif::printInfo("GyroL3GD20H::initialize: Interrupt transfer success\n");
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
TaskFactory::delayTask(1);
}
2021-06-03 22:07:59 +02:00
uint8_t whoAmIVal = rxBuffer[1];
if(whoAmIVal != EXPECTED_WHO_AM_I_VAL) {
2021-06-04 15:50:02 +02:00
sif::printDebug("GyroL3GD20H::initialize: "
2021-06-03 22:07:59 +02:00
"Read WHO AM I value %d not equal to expected value!\n", whoAmIVal);
}
break;
}
2021-06-04 15:50:02 +02:00
case(HAL_BUSY):
case(HAL_ERROR):
2021-06-03 22:07:59 +02:00
case(HAL_TIMEOUT): {
2021-06-04 15:50:02 +02:00
sif::printDebug("GyroL3GD20H::initialize: Initialization failure using interrupts\n");
2021-06-03 22:07:59 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-06-04 15:50:02 +02:00
}
sif::printInfo("GyroL3GD20H::initialize: Configuring device\n");
transferState = TransferStates::WAIT;
// Configure the 5 configuration registers
uint8_t configRegs[5];
prepareConfigRegs(configRegs);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
switch(HAL_SPI_TransmitReceive_IT(spiHandle, txBuffer.data(), rxBuffer.data(), 6)) {
case(HAL_OK): {
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
TaskFactory::delayTask(1);
}
break;
}
case(HAL_BUSY):
case(HAL_ERROR):
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Initialization failure using interrupts\n");
2021-06-03 22:07:59 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
2021-06-04 15:50:02 +02:00
}
txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK;
std::memset(txBuffer.data() + 1, 0 , 5);
transferState = TransferStates::WAIT;
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
switch(HAL_SPI_TransmitReceive_IT(spiHandle, txBuffer.data(), rxBuffer.data(), 6)) {
case(HAL_OK): {
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
TaskFactory::delayTask(1);
}
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");
}
break;
}
case(HAL_BUSY):
case(HAL_ERROR):
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Initialization failure using interrupts\n");
2021-06-03 22:07:59 +02:00
return HasReturnvaluesIF::RETURN_FAILED;
}
}
return HasReturnvaluesIF::RETURN_OK;
}
2021-06-04 15:50:02 +02:00
ReturnValue_t GyroL3GD20H::handleInterruptSensorRead() {
transferState = TransferStates::WAIT;
txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK;
std::memset(txBuffer.data() + 1, 0 , 14);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
switch(HAL_SPI_TransmitReceive_IT(spiHandle, txBuffer.data(), rxBuffer.data(), 15)) {
case(HAL_OK): {
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
TaskFactory::delayTask(1);
}
handleSensorReadout();
break;
}
case(HAL_BUSY):
case(HAL_ERROR):
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Sensor read failure using interrupts\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
}
return HasReturnvaluesIF::RETURN_OK;
}
void GyroL3GD20H::prepareConfigRegs(uint8_t* configRegs) {
// Enable sensor
configRegs[0] = 0b00001111;
configRegs[1] = 0b00000000;
configRegs[2] = 0b00000000;
// Big endian select
2021-06-10 14:17:13 +02:00
configRegs[3] = 0b01000000;
2021-06-04 15:50:02 +02:00
configRegs[4] = 0b00000000;
txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK;
std::memcpy(txBuffer.data() + 1, configRegs, 5);
}
2021-06-10 14:17:13 +02:00
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];
}
2021-06-04 15:50:02 +02:00
void GyroL3GD20H::handleSensorReadout() {
uint8_t statusReg = rxBuffer[8];
int16_t gyroXRaw = rxBuffer[9] << 8 | rxBuffer[10];
2021-06-10 14:17:13 +02:00
float gyroX = static_cast<float>(gyroXRaw) * 0.00875;
2021-06-04 15:50:02 +02:00
int16_t gyroYRaw = rxBuffer[11] << 8 | rxBuffer[12];
2021-06-10 14:17:13 +02:00
float gyroY = static_cast<float>(gyroYRaw) * 0.00875;
2021-06-04 15:50:02 +02:00
int16_t gyroZRaw = rxBuffer[13] << 8 | rxBuffer[14];
2021-06-10 14:17:13 +02:00
float gyroZ = static_cast<float>(gyroZRaw) * 0.00875;
2021-06-04 15:50:02 +02:00
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);
}
2021-06-05 13:29:43 +02:00
void GyroL3GD20H::spiTransferCompleteCallback(SPI_HandleTypeDef *hspi, void* args) {
2021-06-04 15:50:02 +02:00
transferState = TransferStates::SUCCESS;
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
if(GyroL3GD20H::transferMode == spi::TransferModes::DMA) {
// Invalidate cache prior to access by CPU
SCB_InvalidateDCache_by_Addr ((uint32_t *)GyroL3GD20H::rxBuffer.data(),
GyroL3GD20H::recvBufferSize);
}
}
/**
* @brief SPI error callbacks.
* @param hspi: SPI handle
* @note This example shows a simple way to report transfer error, and you can
* add your own implementation.
* @retval None
*/
2021-06-05 13:29:43 +02:00
void GyroL3GD20H::spiTransferErrorCallback(SPI_HandleTypeDef *hspi, void* args) {
2021-06-04 15:50:02 +02:00
transferState = TransferStates::FAILURE;
}