From 81c33d2dc657b853cc7ec63b01123fce04e6e103 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Sep 2021 14:56:20 +0200 Subject: [PATCH] added functions to set x,y,z limits --- .../devicehandlers/GyroL3GD20Handler.cpp | 12 ++++++--- .../devicehandlers/GyroL3GD20Handler.h | 20 +++++++++----- .../devicehandlers/MgmLIS3MDLHandler.cpp | 26 +++++++------------ .../devicehandlers/MgmLIS3MDLHandler.h | 12 +++++++++ 4 files changed, 44 insertions(+), 26 deletions(-) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 06b4548b..4c8495b3 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -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; +} diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index de2884e8..030833be 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -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, diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index 6bc8a19e..4f164d79 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -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(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({0.0})); return HasReturnvaluesIF::RETURN_OK; } + +void MgmLIS3MDLHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) { + this->absLimitX = xLimit; + this->absLimitY = yLimit; + this->absLimitZ = zLimit; +} diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h index b0f249a9..7181d8a0 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h @@ -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.