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

important bugfixes

This commit is contained in:
Robin Müller 2021-06-10 14:17:13 +02:00
parent 92a3f13172
commit 05b64b05a3
No known key found for this signature in database
GPG Key ID: BE6480244DFE612C
5 changed files with 236 additions and 220 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
devicehandlers/GyroL3GD20Handler.cpp
View File

@ -109,13 +109,13 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(
bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0;
if(not fsH and not fsL) {
scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_00) / INT16_MAX;
sensitivity = L3GD20H::SENSITIVITY_00;
}
else if(not fsH and fsL) {
scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_01) / INT16_MAX;
sensitivity = L3GD20H::SENSITIVITY_01;
}
else {
scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_11) / INT16_MAX;
sensitivity = L3GD20H::SENSITIVITY_11;
}
commandBuffer[1] = ctrlReg1Value;
@ -190,9 +190,9 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
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;

3
devicehandlers/GyroL3GD20Handler.h
View File

@ -71,7 +71,8 @@ private:
uint8_t commandBuffer[L3GD20H::READ_LEN + 1];
float scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_00) / INT16_MAX;
// Set default value
float sensitivity = L3GD20H::SENSITIVITY_00;
#if L3GD20_GYRO_DEBUG == 1
PeriodicOperationDivider* debugDivider = nullptr;

3
devicehandlers/devicedefinitions/GyroL3GD20Definitions.h
View File

@ -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;

23
stm32h7/devicetest/GyroL3GD20H.cpp
View File

@ -15,7 +15,7 @@
alignas(32) std::array<uint8_t, GyroL3GD20H::recvBufferSize> GyroL3GD20H::rxBuffer;
alignas(32) std::array<uint8_t, GyroL3GD20H::txBufferSize>
GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer")));
GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer")));
TransferStates transferState = TransferStates::IDLE;
spi::TransferModes GyroL3GD20H::transferMode = spi::TransferModes::POLLING;
@ -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<float>(gyroXRaw) / INT16_MAX * L3G_RANGE;
float gyroX = static_cast<float>(gyroXRaw) * 0.00875;
int16_t gyroYRaw = rxBuffer[11] << 8 | rxBuffer[12];
float gyroY = static_cast<float>(gyroYRaw) / INT16_MAX * L3G_RANGE;
float gyroY = static_cast<float>(gyroYRaw) * 0.00875;
int16_t gyroZRaw = rxBuffer[13] << 8 | rxBuffer[14];
float gyroZ = static_cast<float>(gyroZRaw) / INT16_MAX * L3G_RANGE;
float gyroZ = static_cast<float>(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);

3
stm32h7/devicetest/GyroL3GD20H.h
View File

@ -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();
};