diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 4a492e5d..70ba49eb 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -3,11 +3,11 @@ #include "fsfw/datapool/PoolReadGuard.h" GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie, uint8_t switchId, uint32_t transitionDelayMs): + CookieIF *comCookie, uint32_t transitionDelayMs): DeviceHandlerBase(objectId, deviceCommunication, comCookie), - switchId(switchId), transitionDelayMs(transitionDelayMs), dataset(this) { + transitionDelayMs(transitionDelayMs), dataset(this) { #if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(5); + debugDivider = new PeriodicOperationDivider(3); #endif } @@ -215,11 +215,32 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, PoolReadGuard readSet(&dataset); if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { - dataset.angVelocX = angVelocX; - dataset.angVelocY = angVelocY; - dataset.angVelocZ = angVelocZ; + if(std::abs(angVelocX) < this->absLimitX) { + dataset.angVelocX = angVelocX; + dataset.angVelocX.setValid(true); + } + else { + dataset.angVelocX.setValid(false); + } + + if(std::abs(angVelocY) < this->absLimitY) { + dataset.angVelocY = angVelocY; + dataset.angVelocY.setValid(true); + } + else { + dataset.angVelocY.setValid(false); + } + + if(std::abs(angVelocZ) < this->absLimitZ) { + dataset.angVelocZ = angVelocZ; + dataset.angVelocZ.setValid(true); + } + else { + dataset.angVelocZ.setValid(false); + } + dataset.temperature = temperature; - dataset.setValidity(true, true); + dataset.temperature.setValid(true); } break; } @@ -234,7 +255,7 @@ uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { return this->transitionDelayMs; } -void GyroHandlerL3GD20H::setGoNormalModeAtStartup() { +void GyroHandlerL3GD20H::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; } @@ -256,3 +277,9 @@ void GyroHandlerL3GD20H::fillCommandAndReplyMap() { void GyroHandlerL3GD20H::modeChanged() { internalState = InternalState::NONE; } + +void GyroHandlerL3GD20H::setAbsoluteLimits(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 bc1d9c1c..870f551d 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -19,13 +19,22 @@ class GyroHandlerL3GD20H: public DeviceHandlerBase { public: GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelayMs = 10000); + CookieIF* comCookie, uint32_t transitionDelayMs); 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 */ - void setGoNormalModeAtStartup(); + void setToGoToNormalMode(bool enable); protected: /* DeviceHandlerBase overrides */ @@ -40,12 +49,12 @@ protected: size_t commandDataLen) override; ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; void fillCommandAndReplyMap() override; void modeChanged() override; - uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; @@ -54,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 63dabfc9..891d6fdc 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -6,13 +6,13 @@ #endif MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelay): + CookieIF* comCookie, uint32_t transitionDelay): DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this), transitionDelay(transitionDelay) { #if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(5); + debugDivider = new PeriodicOperationDivider(3); #endif - /* Set to default values right away. */ + // Set to default values right away registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT; @@ -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,10 +307,29 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, #endif /* OBSW_VERBOSE_LEVEL >= 1 */ PoolReadGuard readHelper(&dataset); if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) { - dataset.fieldStrengthX = mgmX; - dataset.fieldStrengthY = mgmY; - dataset.fieldStrengthZ = mgmZ; - dataset.setValidity(true, true); + if(std::abs(mgmX) < absLimitX) { + dataset.fieldStrengthX = mgmX; + dataset.fieldStrengthX.setValid(true); + } + else { + dataset.fieldStrengthX.setValid(false); + } + + if(std::abs(mgmY) < absLimitY) { + dataset.fieldStrengthY = mgmY; + dataset.fieldStrengthY.setValid(true); + } + else { + dataset.fieldStrengthY.setValid(false); + } + + if(std::abs(mgmZ) < absLimitZ) { + dataset.fieldStrengthZ = mgmZ; + dataset.fieldStrengthZ.setValid(true); + } + else { + dataset.fieldStrengthZ.setValid(false); + } } break; } @@ -321,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; @@ -444,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); @@ -475,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; } @@ -503,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 9fb41156..6bf89a49 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h @@ -31,9 +31,17 @@ public: static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW); MgmLIS3MDLHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF* comCookie, - uint8_t switchId, uint32_t transitionDelay = 10000); + uint32_t transitionDelay); 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: @@ -42,7 +50,7 @@ protected: void doShutDown() override; void doStartUp() override; void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override; - uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; ReturnValue_t buildCommandFromCommand( DeviceCommandId_t deviceCommand, const uint8_t *commandData, size_t commandDataLen) override; @@ -52,7 +60,14 @@ protected: DeviceCommandId_t *id) override; ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + /** + * This implementation is tailored towards space applications and will flag values larger + * than 100 microtesla on X,Y and 150 microtesla on Z as invalid + * @param id + * @param packet + * @return + */ + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; void fillCommandAndReplyMap() override; void modeChanged(void) override; @@ -61,15 +76,19 @@ protected: private: MGMLIS3MDL::MgmPrimaryDataset dataset; - //Length a sindgle command SPI answer + //Length a single command SPI answer static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2; uint32_t transitionDelay; - //Single SPIcommand has 2 bytes, first for adress, second for content + // Single SPI command has 2 bytes, first for adress, second for content size_t singleComandSize = 2; - //has the size for all adresses of the lis3mdl + the continous write bit + // 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. diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index 467141de..20cf95d2 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -8,10 +8,9 @@ MgmRM3100Handler::MgmRM3100Handler(object_id_t objectId, - object_id_t deviceCommunication, CookieIF* comCookie, uint8_t switchId, - uint32_t transitionDelay): + object_id_t deviceCommunication, CookieIF* comCookie, uint32_t transitionDelay): DeviceHandlerBase(objectId, deviceCommunication, comCookie), - primaryDataset(this), switchId(switchId), transitionDelay(transitionDelay) { + primaryDataset(this), transitionDelay(transitionDelay) { #if FSFW_HAL_RM3100_MGM_DEBUG == 1 debugDivider = new PeriodicOperationDivider(3); #endif @@ -322,13 +321,7 @@ ReturnValue_t MgmRM3100Handler::initializeLocalDataPool( } uint32_t MgmRM3100Handler::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 25000; -} - -ReturnValue_t MgmRM3100Handler::getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) { - *switches = &switchId; - *numberOfSwitches = 1; - return HasReturnvaluesIF::RETURN_OK; + return this->transitionDelay; } void MgmRM3100Handler::setToGoToNormalMode(bool enable) { diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h index eb6e62bc..1ba680cb 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h @@ -32,7 +32,7 @@ public: SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO); MgmRM3100Handler(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelay = 10000); + CookieIF* comCookie, uint32_t transitionDelay); virtual ~MgmRM3100Handler(); /** @@ -48,21 +48,16 @@ protected: DeviceCommandId_t *id) override; void doStartUp() override; void doShutDown() override; - ReturnValue_t buildNormalDeviceCommand( - DeviceCommandId_t *id) override; - ReturnValue_t buildCommandFromCommand( - DeviceCommandId_t deviceCommand, const uint8_t *commandData, - size_t commandDataLen) override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) override; ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, - const uint8_t *packet) override; - ReturnValue_t getSwitches(const uint8_t **switches, - uint8_t *numberOfSwitches) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; void fillCommandAndReplyMap() override; void modeChanged(void) override; - uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; @@ -97,7 +92,6 @@ private: float scaleFactorZ = 1.0 / RM3100::DEFAULT_GAIN; bool goToNormalModeAtStartup = false; - uint8_t switchId; uint32_t transitionDelay; ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index ec55f863..e940cf32 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -141,8 +141,8 @@ ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, s if(sendLen > spiCookie->getMaxBufferSize()) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "SpiComIF::sendMessage: Too much data sent, send length" << sendLen << - "larger than maximum buffer length" << spiCookie->getMaxBufferSize() << std::endl; + sif::warning << "SpiComIF::sendMessage: Too much data sent, send length " << sendLen << + "larger than maximum buffer length " << spiCookie->getMaxBufferSize() << std::endl; #else sif::printWarning("SpiComIF::sendMessage: Too much data sent, send length %lu larger " "than maximum buffer length %lu!\n", static_cast(sendLen), diff --git a/src/fsfw/globalfunctions/PeriodicOperationDivider.cpp b/src/fsfw/globalfunctions/PeriodicOperationDivider.cpp index 5d9e1624..98f83d04 100644 --- a/src/fsfw/globalfunctions/PeriodicOperationDivider.cpp +++ b/src/fsfw/globalfunctions/PeriodicOperationDivider.cpp @@ -7,14 +7,13 @@ PeriodicOperationDivider::PeriodicOperationDivider(uint32_t divider, } bool PeriodicOperationDivider::checkAndIncrement() { + counter++; bool opNecessary = check(); if(opNecessary) { if(resetAutomatically) { - counter = 1; + resetCounter(); } - return opNecessary; } - counter++; return opNecessary; }