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

21
mission/devices/GyroL3GD20Handler.h
View File

@ -1,8 +1,12 @@
#ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_
#define MISSION_DEVICES_GYROL3GD20HANDLER_H_
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include "devicedefinitions/GyroL3GD20Definitions.h"
#include <OBSWConfig.h>
#include <fsfw/devicehandlers/DeviceHandlerBase.h>
#include <fsfw/globalfunctions/PeriodicOperationDivider.h>
/**
* @brief Device Handler for the L3GD20H gyroscope sensor
@ -43,14 +47,15 @@ protected:
LocalDataPoolManager &poolManager) override;
private:
L3GD20H::GyroPrimaryDataset dataset;
GyroPrimaryDataset dataset;
enum class InternalState {
STATE_NONE,
STATE_CONFIGURE,
STATE_NORMAL
NONE,
CONFIGURE,
CHECK_REGS,
NORMAL
};
InternalState internalState = InternalState::STATE_NONE;
InternalState internalState = InternalState::NONE;
bool commandExecuted = false;
uint8_t statusReg = 0;
@ -64,6 +69,10 @@ private:
uint8_t commandBuffer[L3GD20H::READ_LEN + 1];
float scaleFactor = static_cast<float>(L3GD20H::RANGE_DPS_00) / INT16_MAX;
#if OBSW_VERBOSE_LEVEL >= 1
PeriodicOperationDivider* debugDivider = nullptr;
#endif
};

7
mission/devices/MGMHandlerLIS3MDL.cpp
View File

@ -7,7 +7,7 @@ MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId,
DeviceHandlerBase(objectId, deviceCommunication, comCookie),
dataset(this) {
#if OBSW_VERBOSE_LEVEL >= 1
debugDivider = new PeriodicOperationDivider(10);
debugDivider = new PeriodicOperationDivider(5);
#endif
/* Set to default values right away. */
registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
@ -44,8 +44,11 @@ void MGMHandlerLIS3MDL::doStartUp() {
/* Set up cached registers which will be used to configure the MGM. */
if(commandExecuted) {
commandExecuted = false;
/* Replace _MODE_TO_ON with MODE_NORMAL to jump to normal mode quickly */
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL);
#else
setMode(_MODE_TO_ON);
#endif
}
break;
}

47
mission/devices/MGMHandlerRM3100.cpp
View File

@ -12,7 +12,7 @@ MGMHandlerRM3100::MGMHandlerRM3100(object_id_t objectId,
DeviceHandlerBase(objectId, deviceCommunication, comCookie),
primaryDataset(this) {
#if OBSW_VERBOSE_LEVEL >= 1
debugDivider = new PeriodicOperationDivider(10);
debugDivider = new PeriodicOperationDivider(5);
#endif
}
@ -20,28 +20,34 @@ MGMHandlerRM3100::~MGMHandlerRM3100() {}
void MGMHandlerRM3100::doStartUp() {
switch(internalState) {
case(InternalState::STATE_NONE): {
internalState = InternalState::STATE_CONFIGURE_CMM;
case(InternalState::NONE): {
internalState = InternalState::CONFIGURE_CMM;
break;
}
case(InternalState::STATE_CONFIGURE_CMM): {
internalState = InternalState::STATE_READ_CMM;
case(InternalState::CONFIGURE_CMM): {
internalState = InternalState::READ_CMM;
break;
}
case(InternalState::STATE_READ_CMM): {
case(InternalState::READ_CMM): {
if(commandExecuted) {
internalState = InternalState::STATE_CONFIGURE_TMRC;
}
break;
}
case(InternalState::STATE_CONFIGURE_TMRC): {
internalState = InternalState::STATE_READ_TMRC;
if(commandExecuted) {
internalState = InternalState::STATE_READ_TMRC;
}
break;
}
case(InternalState::STATE_READ_TMRC): {
if(commandExecuted) {
internalState = InternalState::STATE_NORMAL;
internalState = InternalState::NORMAL;
#if OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP == 1
setMode(MODE_NORMAL);
#else
setMode(_MODE_TO_ON);
#endif
}
break;
}
@ -58,15 +64,15 @@ void MGMHandlerRM3100::doShutDown() {
ReturnValue_t MGMHandlerRM3100::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_CMM): {
case(InternalState::CONFIGURE_CMM): {
*id = RM3100::CONFIGURE_CMM;
break;
}
case(InternalState::STATE_READ_CMM): {
case(InternalState::READ_CMM): {
*id = RM3100::READ_CMM;
break;
}
@ -79,9 +85,9 @@ ReturnValue_t MGMHandlerRM3100::buildTransitionDeviceCommand(
break;
}
default:
// might be a configuration error.
sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown "
<< "internal state!" << std::endl;
/* Might be a configuration error. */
sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
std::endl;
return HasReturnvaluesIF::RETURN_OK;
}
@ -164,6 +170,9 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
case(RM3100::CONFIGURE_CYCLE_COUNT):
case(RM3100::CONFIGURE_TMRC): {
/* We can only check whether write was successful with read operation. */
if(mode == _MODE_START_UP) {
commandExecuted = true;
}
break;
}
case(RM3100::READ_CMM): {
@ -171,12 +180,12 @@ ReturnValue_t MGMHandlerRM3100::interpretDeviceReply(
/* We clear the seventh bit in any case
* because this one is zero sometimes for some reason */
bitutil::bitClear(&cmmValue, 6);
if(cmmValue == cmmRegValue) {
if(cmmValue == cmmRegValue and internalState == InternalState::READ_CMM) {
commandExecuted = true;
}
else {
/* Attempt reconfiguration. */
internalState = InternalState::STATE_CONFIGURE_CMM;
internalState = InternalState::CONFIGURE_CMM;
return DeviceHandlerIF::DEVICE_REPLY_INVALID;
}
break;
@ -304,7 +313,7 @@ void MGMHandlerRM3100::fillCommandAndReplyMap() {
}
void MGMHandlerRM3100::modeChanged(void) {
internalState = InternalState::STATE_NONE;
internalState = InternalState::NONE;
}
ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool(
@ -316,7 +325,7 @@ ReturnValue_t MGMHandlerRM3100::initializeLocalDataPool(
}
uint32_t MGMHandlerRM3100::getTransitionDelayMs(Mode_t from, Mode_t to) {
return 10000;
return 15000;
}
ReturnValue_t MGMHandlerRM3100::handleDataReadout(const uint8_t *packet) {

10
mission/devices/MGMHandlerRM3100.h
View File

@ -61,18 +61,18 @@ protected:
private:
enum class InternalState {
STATE_NONE,
STATE_CONFIGURE_CMM,
STATE_READ_CMM,
NONE,
CONFIGURE_CMM,
READ_CMM,
// The cycle count states are propably not going to be used because
// the default cycle count will be used.
STATE_CONFIGURE_CYCLE_COUNT,
STATE_READ_CYCLE_COUNT,
STATE_CONFIGURE_TMRC,
STATE_READ_TMRC,
STATE_NORMAL
NORMAL
};
InternalState internalState = InternalState::STATE_NONE;
InternalState internalState = InternalState::NONE;
bool commandExecuted = false;
RM3100::Rm3100PrimaryDataset primaryDataset;

48
mission/devices/devicedefinitions/GyroL3GD20Definitions.h
View File

@ -1,11 +1,15 @@
#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_
#define MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_
#include <fsfw/datapoollocal/StaticLocalDataSet.h>
#include <fsfw/devicehandlers/DeviceHandlerIF.h>
#include <cstdint>
namespace L3GD20H {
/* Actual size is 15 but we round up a bit */
static constexpr size_t MAX_BUFFER_SIZE = 16;
static constexpr uint8_t READ_MASK = 0b1000'0000;
static constexpr uint8_t AUTO_INCREMENT_MASK = 0b0100'0000;
@ -104,27 +108,33 @@ enum GyroPoolIds: lp_id_t {
TEMPERATURE
};
class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
public:
GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
StaticLocalDataSet(hkOwner, GYRO_DATASET_ID) {}
GyroPrimaryDataset(object_id_t mgmId):
StaticLocalDataSet(sid_t(mgmId, GYRO_DATASET_ID)) {}
// Angular velocities in degrees per second (DPS)
lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId,
ANG_VELOC_X, this);
lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId,
ANG_VELOC_Y, this);
lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId,
ANG_VELOC_Z, this);
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
TEMPERATURE, this);
};
}
class GyroPrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> {
public:
/** Constructor for data users like controllers */
GyroPrimaryDataset(object_id_t mgmId):
StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) {
setAllVariablesReadOnly();
}
/* Angular velocities in degrees per second (DPS) */
lp_var_t<float> angVelocX = lp_var_t<float>(sid.objectId,
L3GD20H::ANG_VELOC_X, this);
lp_var_t<float> angVelocY = lp_var_t<float>(sid.objectId,
L3GD20H::ANG_VELOC_Y, this);
lp_var_t<float> angVelocZ = lp_var_t<float>(sid.objectId,
L3GD20H::ANG_VELOC_Z, this);
lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
L3GD20H::TEMPERATURE, this);
private:
friend class GyroHandlerL3GD20H;
/** Constructor for the data creator */
GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner):
StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {}
};
#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */

3
mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h
View File

@ -15,6 +15,9 @@ enum opMode {
LOW, MEDIUM, HIGH, ULTRA
};
/* Actually 15, we just round up a bit */
static constexpr size_t MAX_BUFFER_SIZE = 16;
static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100;
static const DeviceCommandId_t SETUP_MGM = 0x00;

3
mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h
View File

@ -9,6 +9,9 @@
namespace RM3100 {
/* Actually 10, we round up a little bit */
static constexpr size_t MAX_BUFFER_SIZE = 12;
static constexpr uint8_t READ_MASK = 0b1000'0000;
/*----------------------------------------------------------------------------*/