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

346
devicehandlers/GyroL3GD20Handler.cpp
View File

@ -3,9 +3,9 @@
#include <fsfw/datapool/PoolReadGuard.h> #include <fsfw/datapool/PoolReadGuard.h>
GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie): CookieIF *comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie), DeviceHandlerBase(objectId, deviceCommunication, comCookie),
dataset(this) { dataset(this) {
#if L3GD20_GYRO_DEBUG == 1 #if L3GD20_GYRO_DEBUG == 1
debugDivider = new PeriodicOperationDivider(5); debugDivider = new PeriodicOperationDivider(5);
#endif #endif
@ -14,188 +14,188 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC
GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {}
void GyroHandlerL3GD20H::doStartUp() { void GyroHandlerL3GD20H::doStartUp() {
if(internalState == InternalState::NONE) { if(internalState == InternalState::NONE) {
internalState = InternalState::CONFIGURE; internalState = InternalState::CONFIGURE;
} }
if(internalState == InternalState::CONFIGURE) { if(internalState == InternalState::CONFIGURE) {
if(commandExecuted) { if(commandExecuted) {
internalState = InternalState::CHECK_REGS; internalState = InternalState::CHECK_REGS;
commandExecuted = false; commandExecuted = false;
} }
} }
if(internalState == InternalState::CHECK_REGS) { if(internalState == InternalState::CHECK_REGS) {
if(commandExecuted) { if(commandExecuted) {
internalState = InternalState::NORMAL; internalState = InternalState::NORMAL;
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1 #if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL); setMode(MODE_NORMAL);
#else #else
setMode(_MODE_TO_ON); setMode(_MODE_TO_ON);
#endif #endif
commandExecuted = false; commandExecuted = false;
} }
} }
} }
void GyroHandlerL3GD20H::doShutDown() { void GyroHandlerL3GD20H::doShutDown() {
setMode(_MODE_POWER_DOWN); setMode(_MODE_POWER_DOWN);
} }
ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch(internalState) { switch(internalState) {
case(InternalState::NONE): case(InternalState::NONE):
case(InternalState::NORMAL): { case(InternalState::NORMAL): {
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
case(InternalState::CONFIGURE): { case(InternalState::CONFIGURE): {
*id = L3GD20H::CONFIGURE_CTRL_REGS; *id = L3GD20H::CONFIGURE_CTRL_REGS;
uint8_t command [5]; uint8_t command [5];
command[0] = L3GD20H::CTRL_REG_1_VAL; command[0] = L3GD20H::CTRL_REG_1_VAL;
command[1] = L3GD20H::CTRL_REG_2_VAL; command[1] = L3GD20H::CTRL_REG_2_VAL;
command[2] = L3GD20H::CTRL_REG_3_VAL; command[2] = L3GD20H::CTRL_REG_3_VAL;
command[3] = L3GD20H::CTRL_REG_4_VAL; command[3] = L3GD20H::CTRL_REG_4_VAL;
command[4] = L3GD20H::CTRL_REG_5_VAL; command[4] = L3GD20H::CTRL_REG_5_VAL;
return buildCommandFromCommand(*id, command, 5); return buildCommandFromCommand(*id, command, 5);
} }
case(InternalState::CHECK_REGS): { case(InternalState::CHECK_REGS): {
*id = L3GD20H::READ_REGS; *id = L3GD20H::READ_REGS;
return buildCommandFromCommand(*id, nullptr, 0); return buildCommandFromCommand(*id, nullptr, 0);
} }
default: default:
#if FSFW_CPP_OSTREAM_ENABLED == 1 #if FSFW_CPP_OSTREAM_ENABLED == 1
/* Might be a configuration error. */ /* Might be a configuration error. */
sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
std::endl; std::endl;
#else #else
sif::printDebug("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n"); sif::printDebug("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n");
#endif #endif
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) { ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) {
*id = L3GD20H::READ_REGS; *id = L3GD20H::READ_REGS;
return buildCommandFromCommand(*id, nullptr, 0); return buildCommandFromCommand(*id, nullptr, 0);
} }
ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t *commandData, DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) { size_t commandDataLen) {
switch(deviceCommand) { switch(deviceCommand) {
case(L3GD20H::READ_REGS): { case(L3GD20H::READ_REGS): {
commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK |
L3GD20H::READ_MASK; L3GD20H::READ_MASK;
std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN); std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN);
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = L3GD20H::READ_LEN + 1; rawPacketLen = L3GD20H::READ_LEN + 1;
break; break;
} }
case(L3GD20H::CONFIGURE_CTRL_REGS): { case(L3GD20H::CONFIGURE_CTRL_REGS): {
commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK; commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK;
if(commandData == nullptr or commandDataLen != 5) { if(commandData == nullptr or commandDataLen != 5) {
return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; return DeviceHandlerIF::INVALID_COMMAND_PARAMETER;
} }
ctrlReg1Value = commandData[0]; ctrlReg1Value = commandData[0];
ctrlReg2Value = commandData[1]; ctrlReg2Value = commandData[1];
ctrlReg3Value = commandData[2]; ctrlReg3Value = commandData[2];
ctrlReg4Value = commandData[3]; ctrlReg4Value = commandData[3];
ctrlReg5Value = commandData[4]; ctrlReg5Value = commandData[4];
bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1; bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1;
bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0; bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0;
if(not fsH and not fsL) { 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) { else if(not fsH and fsL) {
scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_01) / INT16_MAX; sensitivity = L3GD20H::SENSITIVITY_01;
} }
else { else {
scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_11) / INT16_MAX; sensitivity = L3GD20H::SENSITIVITY_11;
} }
commandBuffer[1] = ctrlReg1Value; commandBuffer[1] = ctrlReg1Value;
commandBuffer[2] = ctrlReg2Value; commandBuffer[2] = ctrlReg2Value;
commandBuffer[3] = ctrlReg3Value; commandBuffer[3] = ctrlReg3Value;
commandBuffer[4] = ctrlReg4Value; commandBuffer[4] = ctrlReg4Value;
commandBuffer[5] = ctrlReg5Value; commandBuffer[5] = ctrlReg5Value;
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = 6; rawPacketLen = 6;
break; break;
} }
case(L3GD20H::READ_CTRL_REGS): { case(L3GD20H::READ_CTRL_REGS): {
commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK |
L3GD20H::READ_MASK; L3GD20H::READ_MASK;
std::memset(commandBuffer + 1, 0, 5); std::memset(commandBuffer + 1, 0, 5);
rawPacket = commandBuffer; rawPacket = commandBuffer;
rawPacketLen = 6; rawPacketLen = 6;
break; break;
} }
default: default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
} }
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) { DeviceCommandId_t *foundId, size_t *foundLen) {
/* For SPI, the ID will always be the one of the last sent command. */ /* For SPI, the ID will always be the one of the last sent command. */
*foundId = this->getPendingCommand(); *foundId = this->getPendingCommand();
*foundLen = this->rawPacketLen; *foundLen = this->rawPacketLen;
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) { const uint8_t *packet) {
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
switch(id) { switch(id) {
case(L3GD20H::CONFIGURE_CTRL_REGS): { case(L3GD20H::CONFIGURE_CTRL_REGS): {
commandExecuted = true; commandExecuted = true;
break; break;
} }
case(L3GD20H::READ_CTRL_REGS): { case(L3GD20H::READ_CTRL_REGS): {
if(packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and if(packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and
packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and
packet[5] == ctrlReg5Value) { packet[5] == ctrlReg5Value) {
commandExecuted = true; commandExecuted = true;
} }
else { else {
/* Attempt reconfiguration. */ /* Attempt reconfiguration. */
internalState = InternalState::CONFIGURE; internalState = InternalState::CONFIGURE;
return DeviceHandlerIF::DEVICE_REPLY_INVALID; return DeviceHandlerIF::DEVICE_REPLY_INVALID;
} }
break; break;
} }
case(L3GD20H::READ_REGS): { case(L3GD20H::READ_REGS): {
if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and
packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and
packet[5] != ctrlReg5Value) { packet[5] != ctrlReg5Value) {
return DeviceHandlerIF::DEVICE_REPLY_INVALID; return DeviceHandlerIF::DEVICE_REPLY_INVALID;
} }
else { else {
if(internalState == InternalState::CHECK_REGS) { if(internalState == InternalState::CHECK_REGS) {
commandExecuted = true; 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 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]; int16_t angVelocZRaw = packet[L3GD20H::OUT_Z_H] << 8 | packet[L3GD20H::OUT_Z_L];
float angVelocX = angVelocXRaw * scaleFactor; float angVelocX = angVelocXRaw * sensitivity;
float angVelocY = angVelocYRaw * scaleFactor; float angVelocY = angVelocYRaw * sensitivity;
float angVelocZ = angVelocZRaw * scaleFactor; float angVelocZ = angVelocZRaw * sensitivity;
int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX];
float temperature = 25.0 + temperaturOffset; float temperature = 25.0 + temperaturOffset;
#if L3GD20_GYRO_DEBUG == 1 #if L3GD20_GYRO_DEBUG == 1
if(debugDivider->checkAndIncrement()) { if(debugDivider->checkAndIncrement()) {
/* Set terminal to utf-8 if there is an issue with micro printout. */ /* 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 #endif
PoolReadGuard readSet(&dataset); PoolReadGuard readSet(&dataset);
if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
dataset.angVelocX = angVelocX; dataset.angVelocX = angVelocX;
dataset.angVelocY = angVelocY; dataset.angVelocY = angVelocY;
dataset.angVelocZ = angVelocZ; dataset.angVelocZ = angVelocZ;
dataset.temperature = temperature; dataset.temperature = temperature;
dataset.setValidity(true, true); dataset.setValidity(true, true);
} }
break; break;
} }
default: default:
return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
} }
return result; return result;
} }
uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) {
return 10000; return 10000;
} }
ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool(
localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X,
new PoolEntry<float>({0.0})); new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y,
new PoolEntry<float>({0.0})); new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z,
new PoolEntry<float>({0.0})); new PoolEntry<float>({0.0}));
localDataPoolMap.emplace(L3GD20H::TEMPERATURE, localDataPoolMap.emplace(L3GD20H::TEMPERATURE,
new PoolEntry<float>({0.0})); new PoolEntry<float>({0.0}));
return HasReturnvaluesIF::RETURN_OK; return HasReturnvaluesIF::RETURN_OK;
} }
void GyroHandlerL3GD20H::fillCommandAndReplyMap() { void GyroHandlerL3GD20H::fillCommandAndReplyMap() {
insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset);
insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1);
insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1);
} }
void GyroHandlerL3GD20H::modeChanged() { void GyroHandlerL3GD20H::modeChanged() {
internalState = InternalState::NONE; internalState = InternalState::NONE;
} }

79
devicehandlers/GyroL3GD20Handler.h
View File

@ -22,56 +22,57 @@
*/ */
class GyroHandlerL3GD20H: public DeviceHandlerBase { class GyroHandlerL3GD20H: public DeviceHandlerBase {
public: public:
GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication,
CookieIF* comCookie); CookieIF* comCookie);
virtual ~GyroHandlerL3GD20H(); virtual ~GyroHandlerL3GD20H();
protected: protected:
/* DeviceHandlerBase overrides */ /* DeviceHandlerBase overrides */
ReturnValue_t buildTransitionDeviceCommand( ReturnValue_t buildTransitionDeviceCommand(
DeviceCommandId_t *id) override; DeviceCommandId_t *id) override;
void doStartUp() override; void doStartUp() override;
void doShutDown() override; void doShutDown() override;
ReturnValue_t buildNormalDeviceCommand( ReturnValue_t buildNormalDeviceCommand(
DeviceCommandId_t *id) override; DeviceCommandId_t *id) override;
ReturnValue_t buildCommandFromCommand( ReturnValue_t buildCommandFromCommand(
DeviceCommandId_t deviceCommand, const uint8_t *commandData, DeviceCommandId_t deviceCommand, const uint8_t *commandData,
size_t commandDataLen) override; size_t commandDataLen) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t len, ReturnValue_t scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) override; DeviceCommandId_t *foundId, size_t *foundLen) override;
ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) override; const uint8_t *packet) override;
void fillCommandAndReplyMap() override; void fillCommandAndReplyMap() override;
void modeChanged() override; void modeChanged() override;
uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override;
ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
LocalDataPoolManager &poolManager) override; LocalDataPoolManager &poolManager) override;
private: private:
GyroPrimaryDataset dataset; GyroPrimaryDataset dataset;
enum class InternalState { enum class InternalState {
NONE, NONE,
CONFIGURE, CONFIGURE,
CHECK_REGS, CHECK_REGS,
NORMAL NORMAL
}; };
InternalState internalState = InternalState::NONE; InternalState internalState = InternalState::NONE;
bool commandExecuted = false; bool commandExecuted = false;
uint8_t statusReg = 0; uint8_t statusReg = 0;
uint8_t ctrlReg1Value = L3GD20H::CTRL_REG_1_VAL; uint8_t ctrlReg1Value = L3GD20H::CTRL_REG_1_VAL;
uint8_t ctrlReg2Value = L3GD20H::CTRL_REG_2_VAL; uint8_t ctrlReg2Value = L3GD20H::CTRL_REG_2_VAL;
uint8_t ctrlReg3Value = L3GD20H::CTRL_REG_3_VAL; uint8_t ctrlReg3Value = L3GD20H::CTRL_REG_3_VAL;
uint8_t ctrlReg4Value = L3GD20H::CTRL_REG_4_VAL; uint8_t ctrlReg4Value = L3GD20H::CTRL_REG_4_VAL;
uint8_t ctrlReg5Value = L3GD20H::CTRL_REG_5_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<float>(L3GD20H::RANGE_DPS_00) / INT16_MAX; // Set default value
float sensitivity = L3GD20H::SENSITIVITY_00;
#if L3GD20_GYRO_DEBUG == 1 #if L3GD20_GYRO_DEBUG == 1
PeriodicOperationDivider* debugDivider = nullptr; 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). */ /* Possible range values in degrees per second (DPS). */
static constexpr uint16_t RANGE_DPS_00 = 245; 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 uint16_t RANGE_DPS_01 = 500;
static constexpr float SENSITIVITY_01 = 17.5;
static constexpr uint16_t RANGE_DPS_11 = 2000; 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 uint8_t READ_START = CTRL_REG_1;
static constexpr size_t READ_LEN = 14; static constexpr size_t READ_LEN = 14;

25
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::recvBufferSize> GyroL3GD20H::rxBuffer;
alignas(32) std::array<uint8_t, GyroL3GD20H::txBufferSize> alignas(32) std::array<uint8_t, GyroL3GD20H::txBufferSize>
GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer"))); GyroL3GD20H::txBuffer __attribute__((section(".dma_buffer")));
TransferStates transferState = TransferStates::IDLE; TransferStates transferState = TransferStates::IDLE;
spi::TransferModes GyroL3GD20H::transferMode = spi::TransferModes::POLLING; spi::TransferModes GyroL3GD20H::transferMode = spi::TransferModes::POLLING;
@ -24,7 +24,7 @@ DMA_HandleTypeDef txDmaHandle;
DMA_HandleTypeDef rxDmaHandle; DMA_HandleTypeDef rxDmaHandle;
GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle, spi::TransferModes transferMode_): GyroL3GD20H::GyroL3GD20H(SPI_HandleTypeDef *spiHandle, spi::TransferModes transferMode_):
spiHandle(spiHandle) { spiHandle(spiHandle) {
transferMode = transferMode_; transferMode = transferMode_;
if(transferMode == spi::TransferModes::DMA) { if(transferMode == spi::TransferModes::DMA) {
spi::setDmaHandles(&txDmaHandle, &rxDmaHandle); spi::setDmaHandles(&txDmaHandle, &rxDmaHandle);
@ -291,7 +291,6 @@ ReturnValue_t GyroL3GD20H::handlePollingTransferInit() {
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
} }
} }
return HasReturnvaluesIF::RETURN_OK;
sif::printInfo("GyroL3GD20H::initialize: Configuring device\n"); sif::printInfo("GyroL3GD20H::initialize: Configuring device\n");
// Configure the 5 configuration registers // Configure the 5 configuration registers
@ -317,7 +316,6 @@ ReturnValue_t GyroL3GD20H::handlePollingTransferInit() {
return HasReturnvaluesIF::RETURN_FAILED; return HasReturnvaluesIF::RETURN_FAILED;
} }
} }
return HasReturnvaluesIF::RETURN_OK;
txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK; txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK | STM_READ_MASK;
std::memset(txBuffer.data() + 1, 0 , 5); 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); HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_RESET);
auto result = HAL_SPI_TransmitReceive(spiHandle, txBuffer.data(), rxBuffer.data(), 15, 1000); auto result = HAL_SPI_TransmitReceive(spiHandle, txBuffer.data(), rxBuffer.data(), 15, 1000);
HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOD, GPIO_PIN_14, GPIO_PIN_SET);
switch(result) { switch(result) {
case(HAL_OK): { case(HAL_OK): {
handleSensorReadout(); handleSensorReadout();
@ -484,22 +483,32 @@ void GyroL3GD20H::prepareConfigRegs(uint8_t* configRegs) {
configRegs[0] = 0b00001111; configRegs[0] = 0b00001111;
configRegs[1] = 0b00000000; configRegs[1] = 0b00000000;
configRegs[2] = 0b00000000; configRegs[2] = 0b00000000;
configRegs[3] = 0b01000000;
// Big endian select // Big endian select
configRegs[3] = 0b01000000;
configRegs[4] = 0b00000000; configRegs[4] = 0b00000000;
txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK; txBuffer[0] = CTRL_REG_1 | STM_AUTO_INCREMENT_MASK;
std::memcpy(txBuffer.data() + 1, configRegs, 5); 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() { void GyroL3GD20H::handleSensorReadout() {
uint8_t statusReg = rxBuffer[8]; uint8_t statusReg = rxBuffer[8];
int16_t gyroXRaw = rxBuffer[9] << 8 | rxBuffer[10]; 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]; 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]; 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("Status register: 0b" BYTE_TO_BINARY_PATTERN "\n", BYTE_TO_BINARY(statusReg));
sif::printInfo("Gyro X: %f\n", gyroX); sif::printInfo("Gyro X: %f\n", gyroX);
sif::printInfo("Gyro Y: %f\n", gyroY); sif::printInfo("Gyro Y: %f\n", gyroY);

3
stm32h7/devicetest/GyroL3GD20H.h
View File

@ -50,9 +50,12 @@ private:
ReturnValue_t handlePollingSensorRead(); ReturnValue_t handlePollingSensorRead();
ReturnValue_t handleInterruptSensorRead(); ReturnValue_t handleInterruptSensorRead();
uint8_t readRegPolling(uint8_t reg);
static void spiTransferCompleteCallback(SPI_HandleTypeDef *hspi, void* args); static void spiTransferCompleteCallback(SPI_HandleTypeDef *hspi, void* args);
static void spiTransferErrorCallback(SPI_HandleTypeDef *hspi, void* args); static void spiTransferErrorCallback(SPI_HandleTypeDef *hspi, void* args);
void prepareConfigRegs(uint8_t* configRegs); void prepareConfigRegs(uint8_t* configRegs);
void handleSensorReadout(); void handleSensorReadout();
}; };