hal/src/fsfw_hal/devicehandlers/CMakeLists.txt

@@ -1,3 +1,5 @@
 target_sources(${LIB_FSFW_NAME} PRIVATE
     GyroL3GD20Handler.cpp
+    MgmRM3100Handler.cpp
+    MgmLIS3MDLHandler.cpp
 )

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):
+        CookieIF *comCookie, uint32_t transitionDelayMs):
         DeviceHandlerBase(objectId, deviceCommunication, comCookie),
-        dataset(this) {
+        transitionDelayMs(transitionDelayMs), dataset(this) {
 #if FSFW_HAL_L3GD20_GYRO_DEBUG == 1
-    debugDivider = new PeriodicOperationDivider(5);
+    debugDivider = new PeriodicOperationDivider(3);
 #endif
 }
 
@@ -47,7 +47,7 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t
     switch(internalState) {
     case(InternalState::NONE):
     case(InternalState::NORMAL): {
-        return HasReturnvaluesIF::RETURN_OK;
+        return NOTHING_TO_SEND;
     }
     case(InternalState::CONFIGURE): {
         *id = L3GD20H::CONFIGURE_CTRL_REGS;
@@ -66,10 +66,11 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t
     default:
 #if FSFW_CPP_OSTREAM_ENABLED == 1
         /* Might be a configuration error. */
-        sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
+        sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: "
-        std::endl;
+                "Unknown internal state!" << std::endl;
 #else
-        sif::printDebug("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n");
+        sif::printDebug("GyroL3GD20Handler::buildTransitionDeviceCommand: "
+                "Unknown internal state!\n");
 #endif
         return HasReturnvaluesIF::RETURN_OK;
     }
@@ -144,7 +145,7 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(
 
 ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len,
         DeviceCommandId_t *foundId, size_t *foundLen) {
-    /* For SPI, the 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;
 
@@ -166,7 +167,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
             commandExecuted = true;
         }
         else {
-            /* Attempt reconfiguration. */
+            // Attempt reconfiguration
             internalState = InternalState::CONFIGURE;
             return DeviceHandlerIF::DEVICE_REPLY_INVALID;
         }
@@ -199,13 +200,12 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
         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:" <<
+            sif::info << "GyroHandlerL3GD20H: Angular velocities (deg/s):" << std::endl;
-                    std::endl;
+            sif::info << "X: " << angVelocX << std::endl;
-            sif::info << "X: " << angVelocX << " \xC2\xB0" << std::endl;
+            sif::info << "Y: " << angVelocY << std::endl;
-            sif::info << "Y: " << angVelocY << " \xC2\xB0" << std::endl;
+            sif::info << "Z: " << angVelocZ << std::endl;
-            sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl;
 #else
-            sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n");
+            sif::printInfo("GyroHandlerL3GD20H: Angular velocities (deg/s):\n");
             sif::printInfo("X: %f\n", angVelocX);
             sif::printInfo("Y: %f\n", angVelocY);
             sif::printInfo("Z: %f\n", angVelocZ);
@@ -215,11 +215,32 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
 
         PoolReadGuard readSet(&dataset);
         if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
+            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;
     }
@@ -231,23 +252,19 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id,
 
 
 uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) {
-    return 10000;
+    return this->transitionDelayMs;
 }
 
-void GyroHandlerL3GD20H::setGoNormalModeAtStartup() {
+void GyroHandlerL3GD20H::setToGoToNormalMode(bool enable) {
     this->goNormalModeImmediately = true;
 }
 
 ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool(
         localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
-    localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X,
+    localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry<float>({0.0}));
-            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry<float>({0.0}));
-    localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y,
+    localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry<float>({0.0}));
-            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry<float>({0.0}));
-    localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z,
-            new PoolEntry<float>({0.0}));
-    localDataPoolMap.emplace(L3GD20H::TEMPERATURE,
-            new PoolEntry<float>({0.0}));
     return HasReturnvaluesIF::RETURN_OK;
 }
 
@@ -260,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;
+}

hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h

@@ -7,10 +7,6 @@
 #include <fsfw/devicehandlers/DeviceHandlerBase.h>
 #include <fsfw/globalfunctions/PeriodicOperationDivider.h>
 
-#ifndef FSFW_HAL_L3GD20_GYRO_DEBUG
-#define FSFW_HAL_L3GD20_GYRO_DEBUG   0
-#endif /* FSFW_HAL_L3GD20_GYRO_DEBUG */
-
 /**
  * @brief 	Device Handler for the L3GD20H gyroscope sensor
  *          (https://www.st.com/en/mems-and-sensors/l3gd20h.html)
@@ -23,10 +19,22 @@
 class GyroHandlerL3GD20H: public DeviceHandlerBase {
 public:
     GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication,
-            CookieIF* comCookie);
+            CookieIF* comCookie, uint32_t transitionDelayMs);
     virtual ~GyroHandlerL3GD20H();
 
-    void setGoNormalModeAtStartup();
+    /**
+     * 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 setToGoToNormalMode(bool enable);
 protected:
 
     /* DeviceHandlerBase overrides */
@@ -41,18 +49,23 @@ 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;
 
 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,

hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h

@@ -0,0 +1,178 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_
+
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <cstdint>
+
+namespace MGMLIS3MDL {
+
+enum Set {
+    ON, OFF
+};
+enum OpMode {
+    LOW, MEDIUM, HIGH, ULTRA
+};
+
+enum Sensitivies: uint8_t {
+    GAUSS_4 = 4,
+    GAUSS_8 = 8,
+    GAUSS_12 = 12,
+    GAUSS_16 = 16
+};
+
+/* Actually 15, we just round up a bit */
+static constexpr size_t MAX_BUFFER_SIZE = 16;
+
+/* Field data register scaling */
+static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100;
+static constexpr float FIELD_LSB_PER_GAUSS_4_SENS = 1.0 / 6842.0;
+static constexpr float FIELD_LSB_PER_GAUSS_8_SENS = 1.0 / 3421.0;
+static constexpr float FIELD_LSB_PER_GAUSS_12_SENS = 1.0 / 2281.0;
+static constexpr float FIELD_LSB_PER_GAUSS_16_SENS = 1.0 / 1711.0;
+
+static const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x00;
+static const DeviceCommandId_t SETUP_MGM = 0x01;
+static const DeviceCommandId_t READ_TEMPERATURE = 0x02;
+static const DeviceCommandId_t IDENTIFY_DEVICE = 0x03;
+static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x04;
+static const DeviceCommandId_t ACCURACY_OP_MODE_SET = 0x05;
+
+/* Number of all control registers */
+static const uint8_t NR_OF_CTRL_REGISTERS = 5;
+/* Number of registers in the MGM */
+static const uint8_t NR_OF_REGISTERS = 19;
+/* Total number of adresses for all registers */
+static const uint8_t TOTAL_NR_OF_ADRESSES = 52;
+static const uint8_t NR_OF_DATA_AND_CFG_REGISTERS = 14;
+static const uint8_t TEMPERATURE_REPLY_LEN = 3;
+static const uint8_t SETUP_REPLY_LEN = 6;
+
+/*------------------------------------------------------------------------*/
+/* Register adresses */
+/*------------------------------------------------------------------------*/
+/* Register adress returns identifier of device with default 0b00111101 */
+static const uint8_t IDENTIFY_DEVICE_REG_ADDR = 0b00001111;
+static const uint8_t DEVICE_ID = 0b00111101;            // Identifier for Device
+
+/* Register adress to access register 1 */
+static const uint8_t CTRL_REG1 = 0b00100000;
+/* Register adress to access register 2 */
+static const uint8_t CTRL_REG2 = 0b00100001;
+/* Register adress to access register 3 */
+static const uint8_t CTRL_REG3 = 0b00100010;
+/* Register adress to access register 4 */
+static const uint8_t CTRL_REG4 = 0b00100011;
+/* Register adress to access register 5 */
+static const uint8_t CTRL_REG5 = 0b00100100;
+
+/* Register adress to access status register */
+static const uint8_t STATUS_REG_IDX = 8;
+static const uint8_t STATUS_REG = 0b00100111;
+
+/* Register adress to access low byte of x-axis */
+static const uint8_t X_LOWBYTE_IDX = 9;
+static const uint8_t X_LOWBYTE = 0b00101000;
+/* Register adress to access high byte of x-axis */
+static const uint8_t X_HIGHBYTE_IDX = 10;
+static const uint8_t X_HIGHBYTE = 0b00101001;
+/* Register adress to access low byte of y-axis */
+static const uint8_t Y_LOWBYTE_IDX = 11;
+static const uint8_t Y_LOWBYTE = 0b00101010;
+/* Register adress to access high byte of y-axis */
+static const uint8_t Y_HIGHBYTE_IDX = 12;
+static const uint8_t Y_HIGHBYTE = 0b00101011;
+/* Register adress to access low byte of z-axis */
+static const uint8_t Z_LOWBYTE_IDX = 13;
+static const uint8_t Z_LOWBYTE = 0b00101100;
+/* Register adress to access high byte of z-axis */
+static const uint8_t Z_HIGHBYTE_IDX = 14;
+static const uint8_t Z_HIGHBYTE = 0b00101101;
+
+/* Register adress to access low byte of temperature sensor */
+static const uint8_t TEMP_LOWBYTE = 0b00101110;
+/* Register adress to access high byte of temperature sensor */
+static const uint8_t TEMP_HIGHBYTE = 0b00101111;
+
+/*------------------------------------------------------------------------*/
+/* Initialize Setup Register set bits                                     */
+/*------------------------------------------------------------------------*/
+/* General transfer bits */
+// Read=1 / Write=0 Bit
+static const uint8_t RW_BIT = 7;
+// Continous Read/Write Bit, increment adress
+static const uint8_t MS_BIT = 6;
+
+/* CTRL_REG1 bits */
+static const uint8_t ST = 0;        // Self test enable bit, enabled = 1
+// Enable rates higher than 80 Hz enabled = 1
+static const uint8_t FAST_ODR = 1;
+static const uint8_t DO0 = 2;       // Output data rate bit 2
+static const uint8_t DO1 = 3;       // Output data rate bit 3
+static const uint8_t DO2 = 4;       // Output data rate bit 4
+static const uint8_t OM0 = 5;       // XY operating mode bit 5
+static const uint8_t OM1 = 6;       // XY operating mode bit 6
+static const uint8_t TEMP_EN = 7;   // Temperature sensor enable enabled = 1
+static const uint8_t CTRL_REG1_DEFAULT = (1 << TEMP_EN) | (1 << OM1) |
+        (1 << DO0) | (1 << DO1) | (1 << DO2);
+
+/* CTRL_REG2 bits */
+//reset configuration registers and user registers
+static const uint8_t SOFT_RST = 2;
+static const uint8_t REBOOT = 3;    //reboot memory content
+static const uint8_t FSO = 5;       //full-scale selection bit 5
+static const uint8_t FS1 = 6;       //full-scale selection bit 6
+static const uint8_t CTRL_REG2_DEFAULT = 0;
+
+/* CTRL_REG3 bits */
+static const uint8_t MD0 = 0;       //Operating mode bit 0
+static const uint8_t MD1 = 1;       //Operating mode bit 1
+//SPI serial interface mode selection enabled = 3-wire-mode
+static const uint8_t SIM = 2;
+static const uint8_t LP = 5;        //low-power mode
+static const uint8_t CTRL_REG3_DEFAULT = 0;
+
+/* CTRL_REG4 bits */
+//big/little endian data selection enabled = MSb at lower adress
+static const uint8_t BLE = 1;
+static const uint8_t OMZ0 = 2;      //Z operating mode bit 2
+static const uint8_t OMZ1 = 3;      //Z operating mode bit 3
+static const uint8_t CTRL_REG4_DEFAULT = (1 << OMZ1);
+
+/* CTRL_REG5 bits */
+static const uint8_t BDU = 6;       //Block data update
+static const uint8_t FAST_READ = 7; //Fast read enabled = 1
+static const uint8_t CTRL_REG5_DEFAULT = 0;
+
+static const uint32_t MGM_DATA_SET_ID = READ_CONFIG_AND_DATA;
+
+enum MgmPoolIds: lp_id_t {
+    FIELD_STRENGTH_X,
+    FIELD_STRENGTH_Y,
+    FIELD_STRENGTH_Z,
+    TEMPERATURE_CELCIUS
+};
+
+class MgmPrimaryDataset: public StaticLocalDataSet<4> {
+public:
+    MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner):
+        StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {}
+
+    MgmPrimaryDataset(object_id_t mgmId):
+        StaticLocalDataSet(sid_t(mgmId, MGM_DATA_SET_ID)) {}
+
+    lp_var_t<float> fieldStrengthX = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_X, this);
+    lp_var_t<float> fieldStrengthY = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Y, this);
+    lp_var_t<float> fieldStrengthZ = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Z, this);
+    lp_var_t<float> temperature = lp_var_t<float>(sid.objectId,
+            TEMPERATURE_CELCIUS, this);
+};
+
+}
+
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ */

hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h

@@ -0,0 +1,132 @@
+#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
+#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_
+
+#include <fsfw/datapoollocal/StaticLocalDataSet.h>
+#include <fsfw/datapoollocal/LocalPoolVariable.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/serialize/SerialLinkedListAdapter.h>
+#include <cstdint>
+
+namespace RM3100 {
+
+/* Actually 10, we round up a little bit */
+static constexpr size_t MAX_BUFFER_SIZE = 12;
+
+static constexpr uint8_t READ_MASK = 0x80;
+
+/*----------------------------------------------------------------------------*/
+/* CMM Register */
+/*----------------------------------------------------------------------------*/
+static constexpr uint8_t SET_CMM_CMZ = 1 << 6;
+static constexpr uint8_t SET_CMM_CMY = 1 << 5;
+static constexpr uint8_t SET_CMM_CMX = 1 << 4;
+static constexpr uint8_t SET_CMM_DRDM = 1 << 2;
+static constexpr uint8_t SET_CMM_START = 1;
+static constexpr uint8_t CMM_REGISTER = 0x01;
+
+static constexpr uint8_t CMM_VALUE = SET_CMM_CMZ | SET_CMM_CMY | SET_CMM_CMX |
+		SET_CMM_DRDM | SET_CMM_START;
+
+/*----------------------------------------------------------------------------*/
+/* Cycle count register */
+/*----------------------------------------------------------------------------*/
+// Default value (200)
+static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8;
+
+static constexpr float DEFAULT_GAIN = static_cast<float>(CYCLE_COUNT_VALUE) /
+		100 * 38;
+static constexpr uint8_t CYCLE_COUNT_START_REGISTER = 0x04;
+
+/*----------------------------------------------------------------------------*/
+/* TMRC register */
+/*----------------------------------------------------------------------------*/
+static constexpr uint8_t TMRC_150HZ_VALUE = 0x94;
+static constexpr uint8_t TMRC_75HZ_VALUE = 0x95;
+static constexpr uint8_t TMRC_DEFAULT_37HZ_VALUE = 0x96;
+
+static constexpr uint8_t TMRC_REGISTER = 0x0B;
+static constexpr uint8_t TMRC_DEFAULT_VALUE = TMRC_DEFAULT_37HZ_VALUE;
+
+static constexpr uint8_t MEASUREMENT_REG_START = 0x24;
+static constexpr uint8_t BIST_REGISTER = 0x33;
+static constexpr uint8_t DATA_READY_VAL = 0b1000'0000;
+static constexpr uint8_t STATUS_REGISTER = 0x34;
+static constexpr uint8_t REVID_REGISTER = 0x36;
+
+// Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM)
+static constexpr uint16_t RANGE = 800;
+
+static constexpr DeviceCommandId_t READ_DATA = 0;
+
+static constexpr DeviceCommandId_t CONFIGURE_CMM = 1;
+static constexpr DeviceCommandId_t READ_CMM = 2;
+
+static constexpr DeviceCommandId_t CONFIGURE_TMRC = 3;
+static constexpr DeviceCommandId_t READ_TMRC = 4;
+
+static constexpr DeviceCommandId_t CONFIGURE_CYCLE_COUNT = 5;
+static constexpr DeviceCommandId_t READ_CYCLE_COUNT = 6;
+
+class CycleCountCommand: public SerialLinkedListAdapter<SerializeIF> {
+public:
+	CycleCountCommand(bool oneCycleCount = true): oneCycleCount(oneCycleCount) {
+		setLinks(oneCycleCount);
+	}
+
+	ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size,
+			Endianness streamEndianness) override {
+		ReturnValue_t result = SerialLinkedListAdapter::deSerialize(buffer,
+				size, streamEndianness);
+		if(oneCycleCount) {
+			cycleCountY = cycleCountX;
+			cycleCountZ = cycleCountX;
+		}
+		return result;
+	}
+
+	SerializeElement<uint16_t> cycleCountX;
+	SerializeElement<uint16_t> cycleCountY;
+	SerializeElement<uint16_t> cycleCountZ;
+
+private:
+	void setLinks(bool oneCycleCount) {
+		setStart(&cycleCountX);
+		if(not oneCycleCount) {
+			cycleCountX.setNext(&cycleCountY);
+			cycleCountY.setNext(&cycleCountZ);
+		}
+	}
+
+	bool oneCycleCount;
+};
+
+static constexpr uint32_t MGM_DATASET_ID = READ_DATA;
+
+enum MgmPoolIds: lp_id_t {
+    FIELD_STRENGTH_X,
+    FIELD_STRENGTH_Y,
+    FIELD_STRENGTH_Z,
+};
+
+class Rm3100PrimaryDataset: public StaticLocalDataSet<3> {
+public:
+	Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner):
+        StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {}
+
+	Rm3100PrimaryDataset(object_id_t mgmId):
+        StaticLocalDataSet(sid_t(mgmId, MGM_DATASET_ID)) {}
+
+	// Field strengths in micro Tesla.
+    lp_var_t<float> fieldStrengthX = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_X, this);
+    lp_var_t<float> fieldStrengthY = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Y, this);
+    lp_var_t<float> fieldStrengthZ = lp_var_t<float>(sid.objectId,
+            FIELD_STRENGTH_Z, this);
+};
+
+}
+
+
+
+#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ */

hal/src/fsfw_hal/linux/UnixFileGuard.cpp

@@ -1,5 +1,10 @@
+#include "fsfw/FSFW.h"
+#include "fsfw/serviceinterface.h"
 #include "fsfw_hal/linux/UnixFileGuard.h"
 
+#include <cerrno>
+#include <cstring>
+
 UnixFileGuard::UnixFileGuard(std::string device, int* fileDescriptor, int flags,
         std::string diagnosticPrefix):
         fileDescriptor(fileDescriptor) {
@@ -10,12 +15,11 @@ UnixFileGuard::UnixFileGuard(std::string device, int* fileDescriptor, int flags,
     if (*fileDescriptor < 0) {
 #if FSFW_VERBOSE_LEVEL >= 1
 #if FSFW_CPP_OSTREAM_ENABLED == 1
-        sif::warning << diagnosticPrefix <<"Opening device failed with error code " << errno <<
+        sif::warning << diagnosticPrefix << ": Opening device failed with error code " <<
-                "." << std::endl;
+                errno << ": " << strerror(errno) << std::endl;
-        sif::warning << "Error description: " << strerror(errno) << std::endl;
 #else
-        sif::printError("%sOpening device failed with error code %d.\n", diagnosticPrefix);
+        sif::printWarning("%s: Opening device failed with error code %d: %s\n",
-        sif::printWarning("Error description: %s\n", strerror(errno));
+                diagnosticPrefix, errno, strerror(errno));
 #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
 #endif /* FSFW_VERBOSE_LEVEL >= 1 */
         openStatus = OPEN_FILE_FAILED;

hal/src/fsfw_hal/linux/spi/SpiComIF.cpp

@@ -15,11 +15,6 @@
 #include <cerrno>
 #include <cstring>
 
-/* Can be used for low-level debugging of the SPI bus */
-#ifndef FSFW_HAL_LINUX_SPI_WIRETAPPING
-#define FSFW_HAL_LINUX_SPI_WIRETAPPING                  0
-#endif
-
 SpiComIF::SpiComIF(object_id_t objectId, GpioIF* gpioComIF):
         SystemObject(objectId), gpioComIF(gpioComIF) {
     if(gpioComIF == nullptr) {
@@ -90,7 +85,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF *cookie) {
 
     int fileDescriptor = 0;
     UnixFileGuard fileHelper(spiCookie->getSpiDevice(), &fileDescriptor, O_RDWR,
-            "SpiComIF::initializeInterface: ");
+            "SpiComIF::initializeInterface");
     if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
         return fileHelper.getOpenResult();
     }
@@ -184,7 +179,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const
     /* Prepare transfer */
     int fileDescriptor = 0;
     std::string device = spiCookie->getSpiDevice();
-    UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage: ");
+    UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage");
     if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
         return OPENING_FILE_FAILED;
     }
@@ -193,7 +188,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const
     spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr);
     setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
     spiCookie->assignWriteBuffer(sendData);
-    spiCookie->assignTransferSize(sendLen);
+    spiCookie->setTransferSize(sendLen);
 
     bool fullDuplex = spiCookie->isFullDuplex();
     gpioId_t gpioId = spiCookie->getChipSelectPin();
@@ -273,7 +268,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) {
     std::string device = spiCookie->getSpiDevice();
     int fileDescriptor = 0;
     UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR,
-            "SpiComIF::requestReceiveMessage: ");
+            "SpiComIF::requestReceiveMessage");
     if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
         return OPENING_FILE_FAILED;
     }
@@ -335,6 +330,7 @@ ReturnValue_t SpiComIF::readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
 
     *buffer = rxBuf;
     *size = spiCookie->getCurrentTransferSize();
+    spiCookie->setTransferSize(0);
     return HasReturnvaluesIF::RETURN_OK;
 }
 

hal/src/fsfw_hal/linux/spi/SpiComIF.h

@@ -1,6 +1,7 @@
 #ifndef LINUX_SPI_SPICOMIF_H_
 #define LINUX_SPI_SPICOMIF_H_
 
+#include "fsfw/FSFW.h"
 #include "spiDefinitions.h"
 #include "returnvalues/classIds.h"
 #include "fsfw_hal/common/gpio/GpioIF.h"

hal/src/fsfw_hal/linux/spi/SpiCookie.cpp

@@ -121,7 +121,7 @@ bool SpiCookie::isFullDuplex() const {
     return not this->halfDuplex;
 }
 
-void SpiCookie::assignTransferSize(size_t transferSize) {
+void SpiCookie::setTransferSize(size_t transferSize) {
     spiTransferStruct.len = transferSize;
 }
 

hal/src/fsfw_hal/linux/spi/SpiCookie.h

@@ -103,10 +103,10 @@ public:
     void assignReadBuffer(uint8_t* rx);
     void assignWriteBuffer(const uint8_t* tx);
     /**
-     * Assign size for the next transfer.
+     * Set size for the next transfer. Set to 0 for no transfer
      * @param transferSize
      */
-    void assignTransferSize(size_t transferSize);
+    void setTransferSize(size_t transferSize);
     size_t getCurrentTransferSize() const;
 
     struct UncommonParameters {
@@ -158,8 +158,6 @@ private:
             std::string spiDev, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed,
             spi::send_callback_function_t callback, void* args);
 
-    size_t currentTransferSize = 0;
-
     address_t spiAddress;
     gpioId_t chipSelectPin;
     std::string spiDevice;

src/fsfw/FSFW.h.in

@@ -20,4 +20,12 @@
 #define FSFW_HAL_L3GD20_GYRO_DEBUG          0
 #endif /* FSFW_HAL_L3GD20_GYRO_DEBUG */
 
+#ifndef FSFW_HAL_RM3100_MGM_DEBUG
+#define FSFW_HAL_RM3100_MGM_DEBUG           0
+#endif /* FSFW_HAL_RM3100_MGM_DEBUG */
+
+#ifndef FSFW_HAL_LIS3MDL_MGM_DEBUG
+#define FSFW_HAL_LIS3MDL_MGM_DEBUG          0
+#endif /* FSFW_HAL_LIS3MDL_MGM_DEBUG */
+
 #endif /* FSFW_FSFW_H_ */

src/fsfw/FSFWVersion.h

@@ -4,7 +4,7 @@
 const char* const FSFW_VERSION_NAME = "ASTP";
 
 #define FSFW_VERSION        1
-#define FSFW_SUBVERSION     3
+#define FSFW_SUBVERSION     2
 #define FSFW_REVISION       0
 
 #endif /* FSFW_VERSION_H_ */

src/fsfw/action/ActionHelper.cpp

@@ -32,6 +32,17 @@ ReturnValue_t ActionHelper::initialize(MessageQueueIF* queueToUse_) {
         setQueueToUse(queueToUse_);
     }
 
+    if(queueToUse == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "ActionHelper::initialize: No queue set" << std::endl;
+#else
+        sif::printWarning("ActionHelper::initialize: No queue set\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
     return HasReturnvaluesIF::RETURN_OK;
 }
 

src/fsfw/devicehandlers/DeviceHandlerBase.cpp

@@ -469,7 +469,7 @@ ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceRep
     auto replyIter = deviceReplyMap.find(deviceReply);
     if (replyIter == deviceReplyMap.end()) {
         triggerEvent(INVALID_DEVICE_COMMAND, deviceReply);
-        return RETURN_FAILED;
+        return COMMAND_NOT_SUPPORTED;
     } else {
         DeviceReplyInfo *info = &(replyIter->second);
         if (maxDelayCycles != 0) {
@@ -481,6 +481,25 @@ ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceRep
     }
 }
 
+ReturnValue_t DeviceHandlerBase::updatePeriodicReply(bool enable, DeviceCommandId_t deviceReply) {
+    auto replyIter = deviceReplyMap.find(deviceReply);
+    if (replyIter == deviceReplyMap.end()) {
+        triggerEvent(INVALID_DEVICE_COMMAND, deviceReply);
+        return COMMAND_NOT_SUPPORTED;
+    } else {
+        DeviceReplyInfo *info = &(replyIter->second);
+        if(not info->periodic) {
+            return COMMAND_NOT_SUPPORTED;
+        }
+        if(enable) {
+            info->delayCycles = info->maxDelayCycles;
+        }
+        else {
+            info->delayCycles = 0;
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
 
 ReturnValue_t DeviceHandlerBase::setReplyDataset(DeviceCommandId_t replyId,
         LocalPoolDataSetBase *dataSet) {
@@ -1336,10 +1355,20 @@ void DeviceHandlerBase::buildInternalCommand(void) {
         DeviceCommandMap::iterator iter = deviceCommandMap.find(
                 deviceCommandId);
         if (iter == deviceCommandMap.end()) {
+#if FSFW_VERBOSE_LEVEL >= 1
+            char output[36];
+            sprintf(output, "Command 0x%08x unknown",
+                    static_cast<unsigned int>(deviceCommandId));
+            // so we can track misconfigurations
+            printWarningOrError(sif::OutputTypes::OUT_WARNING,
+                    "buildInternalCommand",
+                    COMMAND_NOT_SUPPORTED,
+                    output);
+#endif
             result = COMMAND_NOT_SUPPORTED;
         }
         else if (iter->second.isExecuting) {
-#if FSFW_DISABLE_PRINTOUT == 0
+#if FSFW_VERBOSE_LEVEL >= 1
             char output[36];
             sprintf(output, "Command 0x%08x is executing",
                     static_cast<unsigned int>(deviceCommandId));
@@ -1550,7 +1579,7 @@ LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() {
     return &poolManager;
 }
 
-MessageQueueId_t DeviceHandlerBase::getCommanderId(DeviceCommandId_t replyId) const {
+MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyId) const {
     auto commandIter = deviceCommandMap.find(replyId);
     if(commandIter == deviceCommandMap.end()) {
         return MessageQueueIF::NO_QUEUE;

src/fsfw/devicehandlers/DeviceHandlerBase.h

@@ -6,22 +6,22 @@
 #include "DeviceHandlerFailureIsolation.h"
 #include "DeviceHandlerThermalSet.h"
 
-#include "../serviceinterface/ServiceInterface.h"
+#include "fsfw/serviceinterface/ServiceInterface.h"
-#include "../serviceinterface/serviceInterfaceDefintions.h"
+#include "fsfw/serviceinterface/serviceInterfaceDefintions.h"
-#include "../objectmanager/SystemObject.h"
+#include "fsfw/objectmanager/SystemObject.h"
-#include "../tasks/ExecutableObjectIF.h"
+#include "fsfw/tasks/ExecutableObjectIF.h"
-#include "../returnvalues/HasReturnvaluesIF.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
-#include "../action/HasActionsIF.h"
+#include "fsfw/action/HasActionsIF.h"
-#include "../datapool/PoolVariableIF.h"
+#include "fsfw/datapool/PoolVariableIF.h"
-#include "../modes/HasModesIF.h"
+#include "fsfw/modes/HasModesIF.h"
-#include "../power/PowerSwitchIF.h"
+#include "fsfw/power/PowerSwitchIF.h"
-#include "../ipc/MessageQueueIF.h"
+#include "fsfw/ipc/MessageQueueIF.h"
-#include "../tasks/PeriodicTaskIF.h"
+#include "fsfw/tasks/PeriodicTaskIF.h"
-#include "../action/ActionHelper.h"
+#include "fsfw/action/ActionHelper.h"
-#include "../health/HealthHelper.h"
+#include "fsfw/health/HealthHelper.h"
-#include "../parameters/ParameterHelper.h"
+#include "fsfw/parameters/ParameterHelper.h"
-#include "../datapoollocal/HasLocalDataPoolIF.h"
+#include "fsfw/datapoollocal/HasLocalDataPoolIF.h"
-#include "../datapoollocal/LocalDataPoolManager.h"
+#include "fsfw/datapoollocal/LocalDataPoolManager.h"
 
 #include <map>
 
@@ -400,7 +400,7 @@ protected:
      */
     virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id,
             const uint8_t *packet) = 0;
-    MessageQueueId_t getCommanderId(DeviceCommandId_t replyId) const;
+    MessageQueueId_t getCommanderQueueId(DeviceCommandId_t replyId) const;
     /**
      * Helper function to get pending command. This is useful for devices
      * like SPI sensors to identify the last sent command.
@@ -450,7 +450,9 @@ protected:
      * @param replyLen Will be supplied to the requestReceiveMessage call of
      * the communication interface.
      * @param periodic	Indicates if the command is periodic (i.e. it is sent
-     * by the device repeatedly without request) or not. Default is aperiodic (0)
+     * by the device repeatedly without request) or not. Default is aperiodic (0).
+     * Please note that periodic replies are disabled by default. You can enable them with
+     * #updatePeriodicReply
      * @return	- @c RETURN_OK when the command was successfully inserted,
      *          - @c RETURN_FAILED else.
      */
@@ -465,7 +467,9 @@ protected:
      * @param maxDelayCycles The maximum number of delay cycles the reply waits
      * until it times out.
      * @param periodic	Indicates if the command is periodic (i.e. it is sent
-     * by the device repeatedly without request) or not. Default is aperiodic (0)
+     * by the device repeatedly without request) or not. Default is aperiodic (0).
+     * Please note that periodic replies are disabled by default. You can enable them with
+     * #updatePeriodicReply
      * @return	- @c RETURN_OK when the command was successfully inserted,
      *          - @c RETURN_FAILED else.
      */
@@ -481,6 +485,14 @@ protected:
      */
     ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand);
 
+    /**
+     * Enables a periodic reply for a given command. It sets to delay cycles to the specified
+     * maximum delay cycles for a given reply ID if enabled or to 0 if disabled.
+     * @param enable Specify whether to enable or disable a given periodic reply
+     * @return
+     */
+    ReturnValue_t updatePeriodicReply(bool enable, DeviceCommandId_t deviceReply);
+
     /**
      * @brief   This function returns the reply length of the next reply to read.
      *
@@ -494,16 +506,14 @@ protected:
     virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand);
 
     /**
-     * @brief 	This is a helper method to facilitate updating entries
+     * @brief 	This is a helper method to facilitate updating entries in the reply map.
-     *        	in the reply map.
      * @param deviceCommand	Identifier of the reply to update.
-     * @param delayCycles The current number of delay cycles to wait.
+     * @param delayCycles The current number of delay cycles to wait. As stated in
-     * As stated in #fillCommandAndCookieMap, to disable periodic commands,
+     * #fillCommandAndReplyMap, to disable periodic commands, this is set to zero.
-     * this is set to zero.
      * @param maxDelayCycles The maximum number of delay cycles the reply waits
      * until it times out. By passing 0 the entry remains untouched.
      * @param periodic Indicates if the command is periodic (i.e. it is sent
-     * by the device repeatedly without request) or not.Default is aperiodic (0).
+     * by the device repeatedly without request) or not. Default is aperiodic (0).
      * Warning: The setting always overrides the value that was entered in the map.
      * @return - @c RETURN_OK when the command was successfully inserted,
      *         - @c RETURN_FAILED else.

src/fsfw/globalfunctions/DleEncoder.cpp

@@ -154,6 +154,11 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_
     size_t encodedIndex = 0;
     size_t decodedIndex = 0;
     uint8_t nextByte;
+
+    //init to 0 so that we can just return in the first checks (which do not consume anything from
+    //the source stream)
+    *readLen = 0;
+
     if(maxDestStreamlen < 1) {
         return STREAM_TOO_SHORT;
     }
@@ -166,6 +171,8 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_
             and (sourceStream[encodedIndex] != STX_CHAR)) {
         if (sourceStream[encodedIndex] == DLE_CHAR) {
             if(encodedIndex + 1 >= sourceStreamLen) {
+                //reached the end of the sourceStream
+                *readLen = sourceStreamLen;
                 return DECODING_ERROR;
             }
             nextByte = sourceStream[encodedIndex + 1];
@@ -200,6 +207,8 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_
     }
     if (sourceStream[encodedIndex] != ETX_CHAR) {
         if(decodedIndex == maxDestStreamlen) {
+            //so far we did not find anything wrong here, so let user try again
+            *readLen = 0;
             return STREAM_TOO_SHORT;
         }
         else {
@@ -220,6 +229,11 @@ ReturnValue_t DleEncoder::decodeStreamNonEscaped(const uint8_t *sourceStream,
     size_t encodedIndex = 0;
     size_t decodedIndex = 0;
     uint8_t nextByte;
+
+    //init to 0 so that we can just return in the first checks (which do not consume anything from
+    //the source stream)
+    *readLen = 0;
+    
     if(maxDestStreamlen < 2) {
         return STREAM_TOO_SHORT;
     }
@@ -227,6 +241,7 @@ ReturnValue_t DleEncoder::decodeStreamNonEscaped(const uint8_t *sourceStream,
         return DECODING_ERROR;
     }
     if (sourceStream[encodedIndex++] != STX_CHAR) {
+        *readLen = 1;
         return DECODING_ERROR;
     }
     while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)) {
@@ -265,11 +280,13 @@ ReturnValue_t DleEncoder::decodeStreamNonEscaped(const uint8_t *sourceStream,
         ++encodedIndex;
         ++decodedIndex;
     }
-    *readLen = encodedIndex;
+
     if(decodedIndex == maxDestStreamlen) {
+        //so far we did not find anything wrong here, so let user try again
+        *readLen = 0;
         return STREAM_TOO_SHORT;
-    }
+    } else {
-    else {
+        *readLen = encodedIndex;
         return DECODING_ERROR;
     }
 }

src/fsfw/pus/servicepackets/Service1Packets.h

@@ -13,10 +13,10 @@
 
 /**
  * @brief 	FailureReport class to serialize a failure report
- * @brief 	Subservice 1, 3, 5, 7
+ * @brief 	Subservice 2, 4, 6, 8
  * @ingroup spacepackets
  */
-class FailureReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 1, 3, 5, 7
+class FailureReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 2, 4, 6, 8
 public:
 	FailureReport(uint8_t failureSubtype_, uint16_t packetId_,
 			uint16_t packetSequenceControl_, uint8_t stepNumber_,
@@ -108,10 +108,10 @@ private:
 };
 
 /**
- * @brief       Subservices 2, 4, 6, 8
+ * @brief       Subservices 1, 3, 5, 7
  * @ingroup     spacepackets
  */
-class SuccessReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 2, 4, 6, 8
+class SuccessReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 1, 3, 5, 7
 public:
 	SuccessReport(uint8_t subtype_, uint16_t packetId_,
 			uint16_t packetSequenceControl_,uint8_t stepNumber_) :