KSat
/
fsfw
forked from fsfw/fsfw
15
4
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added functions to set x,y,z limits

This commit is contained in:
Robin Müller 2021-09-23 14:56:20 +02:00
parent 29c74283f1
commit 81c33d2dc6
4 changed files with 44 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp
View File

@ -215,7 +215,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
PoolReadGuard readSet(&dataset);
if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
if(std::abs(angVelocX) > 100) {
if(std::abs(angVelocX) < this->absLimitX) {
dataset.angVelocX = angVelocX;
dataset.angVelocX.setValid(true);
}
@ -223,7 +223,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
dataset.angVelocX.setValid(false);
}
if(std::abs(angVelocY) > 100) {
if(std::abs(angVelocY) < this->absLimitY) {
dataset.angVelocY = angVelocY;
dataset.angVelocY.setValid(true);
}
@ -231,7 +231,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
dataset.angVelocY.setValid(false);
}
if(std::abs(angVelocZ) > 100) {
if(std::abs(angVelocZ) < this->absLimitZ) {
dataset.angVelocZ = angVelocZ;
dataset.angVelocZ.setValid(true);
}
@ -277,3 +277,9 @@ void GyroHandlerL3GD20H::fillCommandAndReplyMap() {
void GyroHandlerL3GD20H::modeChanged() {
internalState = InternalState::NONE;
}
void GyroHandlerL3GD20H::setAxisLimits(float limitX, float limitY, float limitZ) {
this->absLimitX = limitX;
this->absLimitY = limitY;
this->absLimitZ = limitZ;
}

20
hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h
View File

@ -22,6 +22,15 @@ public:
CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelayMs = 10000);
virtual ~GyroHandlerL3GD20H();
/**
* Set the absolute limit for the values on the axis in degrees per second.
* The dataset values will be marked as invalid if that limit is exceeded
* @param xLimit
* @param yLimit
* @param zLimit
*/
void setAbsoluteLimits(float limitX, float limitY, float limitZ);
/**
* @brief Configure device handler to go to normal mode immediately
*/
@ -40,13 +49,6 @@ protected:
size_t commandDataLen) override;
ReturnValue_t scanForReply(const uint8_t *start, size_t len,
DeviceCommandId_t *foundId, size_t *foundLen) override;
/**
* This implementation is tailored towards space systems and flags angular velocities
* larger than 100 as invalid
* @param id
* @param packet
* @return
*/
virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
const uint8_t *packet) override;
@ -61,6 +63,10 @@ private:
uint32_t transitionDelayMs = 0;
GyroPrimaryDataset dataset;
float absLimitX = L3GD20H::RANGE_DPS_00;
float absLimitY = L3GD20H::RANGE_DPS_00;
float absLimitZ = L3GD20H::RANGE_DPS_00;
enum class InternalState {
NONE,
CONFIGURE,

26
hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp
View File

@ -291,7 +291,6 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id,
#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 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 << "MGMHandlerLIS3: Magnetic field strength in"
" microtesla:" << std::endl;
@ -308,7 +307,7 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id,
#endif /* OBSW_VERBOSE_LEVEL >= 1 */
PoolReadGuard readHelper(&dataset);
if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
if(std::abs(mgmX) > 100.0) {
if(std::abs(mgmX) < absLimitX) {
dataset.fieldStrengthX = mgmX;
dataset.fieldStrengthX.setValid(true);
}
@ -316,7 +315,7 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id,
dataset.fieldStrengthX.setValid(false);
}
if(std::abs(mgmY) > 100.0) {
if(std::abs(mgmY) < absLimitY) {
dataset.fieldStrengthY = mgmY;
dataset.fieldStrengthY.setValid(true);
}
@ -324,7 +323,7 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id,
dataset.fieldStrengthY.setValid(false);
}
if(std::abs(mgmZ) > 150.0) {
if(std::abs(mgmZ) < absLimitZ) {
dataset.fieldStrengthZ = mgmZ;
dataset.fieldStrengthZ.setValid(true);
}
@ -340,7 +339,6 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id,
float tempValue = 25.0 + ((static_cast<float>(tempValueRaw)) / 8.0);
#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1
if(debugDivider->check()) {
/* Set terminal to utf-8 if there is an issue with micro printout. */
#if FSFW_CPP_OSTREAM_ENABLED == 1
sif::info << "MGMHandlerLIS3: Temperature: " << tempValue << " C" <<
std::endl;
@ -463,16 +461,6 @@ ReturnValue_t MgmLIS3MDLHandler::setOperatingMode(const uint8_t *commandData,
}
void MgmLIS3MDLHandler::fillCommandAndReplyMap() {
/*
* Regarding ArduinoBoard:
* Actually SPI answers directly, but as commanding ArduinoBoard the
* communication could be delayed
* SPI always has to be triggered, so there could be no periodic answer of
* the device, the device has to asked with a command, so periodic is zero.
*
* We dont read single registers, we just expect special
* reply from he Readall_MGM
*/
insertInCommandAndReplyMap(MGMLIS3MDL::READ_CONFIG_AND_DATA, 1, &dataset);
insertInCommandAndReplyMap(MGMLIS3MDL::READ_TEMPERATURE, 1);
insertInCommandAndReplyMap(MGMLIS3MDL::SETUP_MGM, 1);
@ -494,7 +482,7 @@ ReturnValue_t MgmLIS3MDLHandler::prepareCtrlRegisterWrite() {
rawPacket = commandBuffer;
rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1;
/* We dont have to check if this is working because we just did it */
// We dont have to check if this is working because we just did i
return RETURN_OK;
}
@ -522,3 +510,9 @@ ReturnValue_t MgmLIS3MDLHandler::initializeLocalDataPool(
new PoolEntry<float>({0.0}));
return HasReturnvaluesIF::RETURN_OK;
}
void MgmLIS3MDLHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) {
this->absLimitX = xLimit;
this->absLimitY = yLimit;
this->absLimitZ = zLimit;
}

12
hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h
View File

@ -34,6 +34,14 @@ public:
uint8_t switchId, uint32_t transitionDelay = 10000);
virtual ~MgmLIS3MDLHandler();
/**
* Set the absolute limit for the values on the axis in microtesla. The dataset values will
* be marked as invalid if that limit is exceeded
* @param xLimit
* @param yLimit
* @param zLimit
*/
void setAbsoluteLimits(float xLimit, float yLimit, float zLimit);
void setToGoToNormalMode(bool enable);
protected:
@ -77,6 +85,10 @@ private:
//has the size for all adresses of the lis3mdl + the continous write bit
uint8_t commandBuffer[MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1];
float absLimitX = 100;
float absLimitY = 100;
float absLimitZ = 150;
/**
* We want to save the registers we set, so we dont have to read the
* registers when we want to change something.