eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 121 Wiki Activity

L3DG20H device handler tested

Browse Source
This commit is contained in:
Robin Müller
2021-03-07 14:06:29 +01:00
committed by Robin.Mueller
parent ab13fe15e4
commit 0db53f44c3
24 changed files with 260 additions and 116 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
mission/devices/GyroL3GD20Handler.cpp
View File

@ -1,25 +1,42 @@
#include <mission/devices/GyroL3GD20Handler.h>
#include "GyroL3GD20Handler.h"
#include <OBSWConfig.h>
#include <fsfw/datapool/PoolReadGuard.h>
GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication,
CookieIF *comCookie):
DeviceHandlerBase(objectId, deviceCommunication, comCookie),
dataset(this) {
#if OBSW_VERBOSE_LEVEL >= 1
debugDivider = new PeriodicOperationDivider(5);
#endif
}
GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {}
void GyroHandlerL3GD20H::doStartUp() {
if(internalState == InternalState::STATE_NONE) {
internalState = InternalState::STATE_CONFIGURE;
if(internalState == InternalState::NONE) {
internalState = InternalState::CONFIGURE;
}
if(internalState == InternalState::STATE_CONFIGURE) {
if(internalState == InternalState::CONFIGURE) {
if(commandExecuted) {
internalState = InternalState::STATE_NORMAL;
internalState = InternalState::CHECK_REGS;
commandExecuted = false;
}
}
if(internalState == InternalState::CHECK_REGS) {
if(commandExecuted) {
internalState = InternalState::NORMAL;
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL);
#else
setMode(_MODE_TO_ON);
#endif
commandExecuted = false;
}
}
}
void GyroHandlerL3GD20H::doShutDown() {
@ -28,11 +45,11 @@ void GyroHandlerL3GD20H::doShutDown() {
ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
switch(internalState) {
case(InternalState::STATE_NONE):
case(InternalState::STATE_NORMAL): {
case(InternalState::NONE):
case(InternalState::NORMAL): {
return HasReturnvaluesIF::RETURN_OK;
}
case(InternalState::STATE_CONFIGURE): {
case(InternalState::CONFIGURE): {
*id = L3GD20H::CONFIGURE_CTRL_REGS;
uint8_t command [5];
command[0] = L3GD20H::CTRL_REG_1_VAL;
@ -41,10 +58,13 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t
command[3] = L3GD20H::CTRL_REG_4_VAL;
command[4] = L3GD20H::CTRL_REG_5_VAL;
return buildCommandFromCommand(*id, command, 5);
break;
}
case(InternalState::CHECK_REGS): {
*id = L3GD20H::READ_REGS;
return buildCommandFromCommand(*id, nullptr, 0);
}
default:
// might be a configuration error.
/* Might be a configuration error. */
sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown "
<< "internal state!" << std::endl;
return HasReturnvaluesIF::RETURN_OK;
@ -122,11 +142,11 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(
ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) {
// SPI, 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();
*foundLen = this->rawPacketLen;
// Data with SPI Interface has always this answer
/* Data with SPI Interface has always this answer */
if (start[0] == 0b11111111) {
return HasReturnvaluesIF::RETURN_OK;
}
@ -138,6 +158,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
switch(id) {
case(L3GD20H::CONFIGURE_CTRL_REGS): {
commandExecuted = true;
break;
}
case(L3GD20H::READ_CTRL_REGS): {
@ -147,18 +168,23 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
commandExecuted = true;
}
else {
// Attempt reconfiguration.
internalState = InternalState::STATE_CONFIGURE;
/* Attempt reconfiguration. */
internalState = InternalState::CONFIGURE;
return DeviceHandlerIF::DEVICE_REPLY_INVALID;
}
break;
}
case(L3GD20H::READ_START): {
case(L3GD20H::READ_REGS): {
if(packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and
packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and
packet[5] != ctrlReg5Value) {
return DeviceHandlerIF::DEVICE_REPLY_INVALID;
}
else {
if(internalState == InternalState::CHECK_REGS) {
commandExecuted = true;
}
}
statusReg = packet[L3GD20H::STATUS_IDX];
@ -171,6 +197,23 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX];
float temperature = 25.0 + temperaturOffset;
#if OBSW_VERBOSE_LEVEL >= 1
if(debugDivider->checkAndIncrement()) {
/* Set terminal to utf-8 if there is an issue with micro printout. */
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::info << "GyroHandlerL3GD20H: Angular velocities in degrees per second:" <<
std::endl;
sif::info << "X: " << angVelocX << " \xC2\xB0" << std::endl;
sif::info << "Y: " << angVelocY << " \xC2\xB0" << std::endl;
sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl;
#else
sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n");
sif::printInfo("X: %f " "\xC2\xB0" "T\n", angVelocX);
sif::printInfo("Y: %f " "\xC2\xB0" "T\n", angVelocY);
sif::printInfo("Z: %f " "\xC2\xB0" "T\n", angVelocZ);
#endif
}
#endif
PoolReadGuard readSet(&dataset);
if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
@ -190,7 +233,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) {
return 20000;
return 25000;
}
ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool(
@ -213,5 +256,5 @@ void GyroHandlerL3GD20H::fillCommandAndReplyMap() {
}
void GyroHandlerL3GD20H::modeChanged() {
internalState = InternalState::STATE_NONE;
internalState = InternalState::NONE;
}