fsfw/fsfw-hal
13
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

IRQ and POlling now working as well

Browse Source
This commit is contained in:
Robin Müller
2021-06-04 15:50:02 +02:00
parent cf1d6bad19
commit 895ba1b455
7 changed files with 337 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

323
stm32h7/devicetest/GyroL3GD20H.cpp
View File

@ -18,19 +18,27 @@ alignas(32) std::array<uint8_t, GyroL3GD20H::txBufferSize>
GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer")));
TransferStates transferState = TransferStates::IDLE;
spi::TransferModes GyroL3GD20H::transferMode = spi::TransferModes::POLLING;
DMA_HandleTypeDef txDmaHandle;
DMA_HandleTypeDef rxDmaHandle;
GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle, spi::TransferModes transferMode):
spiHandle(spiHandle), transferMode(transferMode) {
GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle, spi::TransferModes transferMode_):
spiHandle(spiHandle) {
transferMode = transferMode_;
if(transferMode == spi::TransferModes::DMA) {
set_dma_handles(&txDmaHandle, &rxDmaHandle);
set_spi_msp_functions(&hal_spi_msp_init_dma, spiHandle, &hal_spi_msp_deinit_dma, spiHandle);
}
else if(transferMode == spi::TransferModes::INTERRUPT) {
set_spi_msp_functions(&hal_spi_msp_init_interrupt, spiHandle,
&hal_spi_msp_deinit_interrupt, spiHandle);
}
else if(transferMode == spi::TransferModes::POLLING) {
set_spi_msp_functions(&hal_spi_msp_init_polling, spiHandle,
&hal_spi_msp_deinit_polling, spiHandle);
}
GPIO_InitTypeDef chipSelect = {};
__HAL_RCC_GPIOD_CLK_ENABLE();
chipSelect.Pin = GPIO_PIN_14;
@ -72,8 +80,11 @@ ReturnValue_t GyroL3GD20H::initialize() {
case(spi::TransferModes::DMA): {
return handleDmaTransferInit();
}
case(spi::TransferModes::INTERRUPT): {
return handleInterruptTransferInit();
}
case(spi::TransferModes::POLLING): {
return handlePollingTransfer();
return handlePollingTransferInit();
}
default: {
return HasReturnvaluesIF::RETURN_FAILED;
@ -88,6 +99,12 @@ ReturnValue_t GyroL3GD20H::performOperation() {
case(spi::TransferModes::DMA): {
return handleDmaSensorRead();
}
case(spi::TransferModes::POLLING): {
return handlePollingSensorRead();
}
case(spi::TransferModes::INTERRUPT): {
return handleInterruptSensorRead();
}
default: {
return HasReturnvaluesIF::RETURN_FAILED;
}
@ -132,19 +149,10 @@ ReturnValue_t GyroL3GD20H::handleDmaTransferInit() {
}
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;
prepareConfigRegs(configRegs);
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
@ -217,7 +225,7 @@ ReturnValue_t GyroL3GD20H::handleDmaSensorRead() {
HAL_StatusTypeDef result = performDmaTransfer(15);
if(result != HAL_OK) {
// Transfer error in transmission process
sif::printWarning("Error transmitting SPI with DMA\n");
sif::printDebug("GyroL3GD20H::handleDmaSensorRead: Error transmitting SPI with DMA\n");
}
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
@ -226,17 +234,7 @@ ReturnValue_t GyroL3GD20H::handleDmaSensorRead() {
switch(transferState) {
case(TransferStates::SUCCESS): {
uint8_t statusReg = rxBuffer[8];
int16_t gyroXRaw = rxBuffer[9] << 8 | rxBuffer[10];
float gyroX = static_cast<float>(gyroXRaw) / INT16_MAX * L3G_RANGE;
int16_t gyroYRaw = rxBuffer[11] << 8 | rxBuffer[12];
float gyroY = static_cast<float>(gyroYRaw) / INT16_MAX * L3G_RANGE;
int16_t gyroZRaw = rxBuffer[13] << 8 | rxBuffer[14];
float gyroZ = static_cast<float>(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);
handleSensorReadout();
break;
}
case(TransferStates::FAILURE): {
@ -263,6 +261,247 @@ HAL_StatusTypeDef GyroL3GD20H::performDmaTransfer(size_t sendSize) {
return HAL_SPI_TransmitReceive_DMA(spiHandle, txBuffer.data(), rxBuffer.data(), sendSize);
}
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;
}
}
return HasReturnvaluesIF::RETURN_OK;
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);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
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;
}
}
return HasReturnvaluesIF::RETURN_OK;
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;
}
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);
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;
}
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)) {
case(HAL_OK): {
sif::printInfo("GyroL3GD20H::initialize: Interrupt transfer success\n");
// Wait for the transfer to complete
while (transferState == TransferStates::WAIT) {
TaskFactory::delayTask(1);
}
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);
}
break;
}
case(HAL_BUSY):
case(HAL_ERROR):
case(HAL_TIMEOUT): {
sif::printDebug("GyroL3GD20H::initialize: Initialization failure using interrupts\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
}
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");
return HasReturnvaluesIF::RETURN_FAILED;
}
}
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");
return HasReturnvaluesIF::RETURN_FAILED;
}
}
return HasReturnvaluesIF::RETURN_OK;
}
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;
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);
}
void GyroL3GD20H::handleSensorReadout() {
uint8_t statusReg = rxBuffer[8];
int16_t gyroXRaw = rxBuffer[9] << 8 | rxBuffer[10];
float gyroX = static_cast<float>(gyroXRaw) / INT16_MAX * L3G_RANGE;
int16_t gyroYRaw = rxBuffer[11] << 8 | rxBuffer[12];
float gyroY = static_cast<float>(gyroYRaw) / INT16_MAX * L3G_RANGE;
int16_t gyroZRaw = rxBuffer[13] << 8 | rxBuffer[14];
float gyroZ = static_cast<float>(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);
}
/**
* @brief TxRx Transfer completed callback.
* @param hspi: SPI handle
@ -273,9 +512,11 @@ HAL_StatusTypeDef GyroL3GD20H::performDmaTransfer(size_t sendSize) {
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);
if(GyroL3GD20H::transferMode == spi::TransferModes::DMA) {
// Invalidate cache prior to access by CPU
SCB_InvalidateDCache_by_Addr ((uint32_t *)GyroL3GD20H::rxBuffer.data(),
GyroL3GD20H::recvBufferSize);
}
}
/**
@ -288,29 +529,3 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
transferState = TransferStates::FAILURE;
}
ReturnValue_t GyroL3GD20H::handlePollingTransfer() {
switch(HAL_SPI_TransmitReceive(spiHandle, txBuffer.data(), rxBuffer.data(), 2, 1000)) {
case(HAL_OK): {
sif::printInfo("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("Polling transfer timeout\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
case(HAL_ERROR): {
sif::printDebug("Polling transfer failure\n");
return HasReturnvaluesIF::RETURN_FAILED;
}
default: {
return HasReturnvaluesIF::RETURN_FAILED;
}
}
return HasReturnvaluesIF::RETURN_OK;
}