L3DG20H device handler tested

bsp_q7s/InitMission.cpp

@@ -11,8 +11,7 @@
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
 #include <fsfw/tasks/PeriodicTaskIF.h>
 #include <fsfw/tasks/TaskFactory.h>
-#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
+#include <fsfwconfig/pollingsequence/pollingSequenceFactory.h>
-
 #include <iostream>
 
 /* This is configured for linux without CR */

bsp_rpi/InitMission.cpp

@@ -3,8 +3,6 @@
 
 #include <fsfwconfig/objects/systemObjectList.h>
 #include <fsfwconfig/OBSWConfig.h>
-#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
-
 #include <mission/utility/InitMission.h>
 
 #include <fsfw/objectmanager/ObjectManagerIF.h>
@@ -14,6 +12,7 @@
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
 #include <fsfw/tasks/PeriodicTaskIF.h>
 #include <fsfw/tasks/TaskFactory.h>
+#include <fsfwconfig/pollingsequence/pollingSequenceFactory.h>
 
 #include <iostream>
 

bsp_rpi/ObjectFactory.cpp

@@ -1,23 +1,25 @@
 #include "ObjectFactory.h"
 #include <bsp_rpi/gpio/GPIORPi.h>
 
-#include <objects/systemObjectList.h>
+#include <fsfwconfig/objects/systemObjectList.h>
-#include <devices/addresses.h>
+#include <fsfwconfig/devices/addresses.h>
-#include <devices/gpioIds.h>
+#include <fsfwconfig/devices/gpioIds.h>
-#include <OBSWConfig.h>
+#include <fsfwconfig/OBSWConfig.h>
-#include <tmtc/apid.h>
+#include <fsfwconfig/tmtc/apid.h>
-#include <tmtc/pusIds.h>
+#include <fsfwconfig/tmtc/pusIds.h>
+#include <fsfwconfig/devices/spi.h>
 
 #include <linux/boardtest/LibgpiodTest.h>
 #include <linux/boardtest/SpiTestClass.h>
 #include <linux/gpio/GpioCookie.h>
 #include <linux/gpio/LinuxLibgpioIF.h>
-
 #include <linux/spi/SpiCookie.h>
+#include <linux/spi/SpiComIF.h>
 
 #include <mission/core/GenericFactory.h>
 #include <mission/utility/TmFunnel.h>
 #include <mission/devices/MGMHandlerLIS3MDL.h>
+#include <mission/devices/MGMHandlerRM3100.h>
 
 #include <fsfw/datapoollocal/LocalDataPoolManager.h>
 #include <fsfw/tmtcservices/CommandingServiceBase.h>
@@ -26,8 +28,8 @@
 #include <fsfw/tmtcpacket/pus/TmPacketStored.h>
 #include <fsfw/osal/linux/TcUnixUdpPollingTask.h>
 #include <fsfw/tasks/TaskFactory.h>
-#include <linux/spi/SpiComIF.h>
+#include <mission/devices/GyroL3GD20Handler.h>
-#include <mission/devices/MGMHandlerRM3100.h>
+
 
 void Factory::setStaticFrameworkObjectIds() {
     PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR;
@@ -99,16 +101,23 @@ void ObjectFactory::produce(){
     gpioIF->addGpios(gpioCookieAcsBoard);
 
     std::string spiDev = "/dev/spidev0.0";
-    SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev, 24,
+    SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3, gpioIds::MGM_0_LIS3_CS, spiDev,
-            spi::SpiMode::MODE_3, 3'900'000);
+            MGMLIS3MDL::MAX_BUFFER_SIZE, spi::DEFAULT_LIS3_MODE, spi::DEFAULT_LIS3_SPEED);
     auto mgmLis3Handler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER,
             objects::SPI_COM_IF, spiCookie);
     mgmLis3Handler->setStartUpImmediately();
-    spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev, 16,
+
-    		spi::SpiMode::MODE_3, 976'000);
+    spiCookie = new SpiCookie(addresses::MGM_1_RM3100, gpioIds::MGM_1_RM3100_CS, spiDev,
+            RM3100::MAX_BUFFER_SIZE, spi::DEFAULT_RM3100_MODE, spi::DEFAULT_RM3100_SPEED);
     auto mgmRm3100Handler = new MGMHandlerRM3100(objects::MGM_1_RM3100_HANDLER,
             objects::SPI_COM_IF, spiCookie);
     mgmRm3100Handler->setStartUpImmediately();
 
+    spiCookie = new SpiCookie(addresses::GYRO_1_L3G, gpioIds::GYRO_1_L3G_CS, spiDev,
+            L3GD20H::MAX_BUFFER_SIZE, spi::DEFAULT_L3G_MODE, spi::DEFAULT_L3G_SPEED);
+    auto gyroL3gHandler = new GyroHandlerL3GD20H(objects::GYRO_1_L3G_HANDLER, objects::SPI_COM_IF,
+            spiCookie);
+    gyroL3gHandler->setStartUpImmediately();
+
 #endif /* RPI_TEST_ACS_BOARD == 1 */
 }

bsp_rpi/boardconfig/rpi_config.h

@@ -7,8 +7,8 @@
 #define RPI_LOOPBACK_TEST_GPIO          0
 
 /* Only one of those 2 should be enabled! */
-#define RPI_ADD_SPI_TEST                1
+#define RPI_ADD_SPI_TEST                0
-#define RPI_TEST_ACS_BOARD              0
+#define RPI_TEST_ACS_BOARD              1
 
 /* Adapt these values accordingly */
 namespace gpio {

fsfwconfig/CMakeLists.txt

@@ -1,6 +1,6 @@
 target_sources(${TARGET_NAME} PRIVATE
 	ipc/MissionMessageTypes.cpp
-	pollingsequence/PollingSequenceFactory.cpp
+	pollingsequence/pollingSequenceFactory.cpp
 )
 
 target_include_directories(${TARGET_NAME} PUBLIC

fsfwconfig/OBSWConfig.h

@@ -14,20 +14,22 @@
 
 /* These defines should be disabled for mission code but are useful for
 debugging. */
-#define OBSW_VERBOSE_LEVEL          1
+#define OBSW_VERBOSE_LEVEL                          1
-#define OBSW_PRINT_MISSED_DEADLINES 1
+#define OBSW_PRINT_MISSED_DEADLINES                 1
-#define OBSW_ADD_TEST_CODE          1
+#define OBSW_ADD_TEST_CODE                          1
-#define TEST_LIBGPIOD               0
+#define TEST_LIBGPIOD                               0
 
-#define TE0720                      0
+#define TE0720                                      0
 
-#define P60DOCK_DEBUG               0
+#define P60DOCK_DEBUG                               0
-#define PDU1_DEBUG                  0
+#define PDU1_DEBUG                                  0
-#define PDU2_DEBUG                  0
+#define PDU2_DEBUG                                  0
-#define ACU_DEBUG                   1
+#define ACU_DEBUG                                   1
 
+/* Can be used to switch device to NORMAL mode immediately */
+#define OBSW_SWITCH_TO_NORMAL_MODE_AFTER_STARTUP    1
 /* Can be used for low-level debugging of the SPI bus */
-#define FSFW_LINUX_SPI_WIRETAPPING  0
+#define FSFW_LINUX_SPI_WIRETAPPING                  0
 
 #ifdef __cplusplus
 

fsfwconfig/devices/spi.h

@@ -0,0 +1,27 @@
+#ifndef FSFWCONFIG_DEVICES_SPI_H_
+#define FSFWCONFIG_DEVICES_SPI_H_
+
+#include <cstdint>
+#include <linux/spi/spiDefinitions.h>
+
+/**
+ * SPI configuration will be contained here to let the device handlers remain independent
+ * of SPI specific properties.
+ */
+namespace spi {
+
+/* Default values, changing them is not supported for now */
+static constexpr uint32_t DEFAULT_LIS3_SPEED = 3'900'000;
+static constexpr spi::SpiModes DEFAULT_LIS3_MODE = spi::SpiModes::MODE_3;
+
+static constexpr uint32_t DEFAULT_RM3100_SPEED = 976'000;
+static constexpr spi::SpiModes DEFAULT_RM3100_MODE = spi::SpiModes::MODE_3;
+
+static constexpr uint32_t DEFAULT_L3G_SPEED = 3'900'000;
+static constexpr spi::SpiModes DEFAULT_L3G_MODE = spi::SpiModes::MODE_3;
+
+}
+
+
+
+#endif /* FSFWCONFIG_DEVICES_SPI_H_ */

fsfwconfig/pollingsequence/pollingSequenceFactory.cpp

@@ -1,9 +1,11 @@
+#include "pollingSequenceFactory.h"
+
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
 #include <fsfw/devicehandlers/DeviceHandlerIF.h>
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
 #include <fsfwconfig/objects/systemObjectList.h>
-#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
+
 
 ReturnValue_t pst::pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence)
 {
@@ -120,6 +122,17 @@ ReturnValue_t pst::pollingSequenceAcsTest(FixedTimeslotTaskIF *thisSequence) {
             DeviceHandlerIF::SEND_READ);
     thisSequence->addSlot(objects::MGM_1_RM3100_HANDLER, length * 0.8,
             DeviceHandlerIF::GET_READ);
+
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0,
+            DeviceHandlerIF::PERFORM_OPERATION);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.2,
+            DeviceHandlerIF::SEND_WRITE);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.4,
+            DeviceHandlerIF::GET_WRITE);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.6,
+            DeviceHandlerIF::SEND_READ);
+    thisSequence->addSlot(objects::GYRO_1_L3G_HANDLER, length * 0.8,
+            DeviceHandlerIF::GET_READ);
     if (thisSequence->checkSequence() != HasReturnvaluesIF::RETURN_OK) {
         sif::error << "Initialization of ACS Board PST failed" << std::endl;
         return HasReturnvaluesIF::RETURN_FAILED;

fsfwconfig/pollingsequence/pollingSequenceFactory.h

@@ -1,5 +1,6 @@
 #ifndef POLLINGSEQUENCEFACTORY_H_
 #define POLLINGSEQUENCEFACTORY_H_
+
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
 
 class FixedTimeslotTaskIF;

linux/boardtest/SpiTestClass.cpp

@@ -71,7 +71,7 @@ void SpiTestClass::performRm3100Test(uint8_t mgmId) {
     }
     uint32_t rm3100speed = 976'000;
     uint8_t rm3100revidReg = 0x36;
-    spi::SpiMode rm3100mode = spi::SpiMode::MODE_3;
+    spi::SpiModes rm3100mode = spi::SpiModes::MODE_3;
 
 #ifdef RASPBERRY_PI
     std::string deviceName = "/dev/spidev0.0";
@@ -171,7 +171,7 @@ void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
         currentGpioId = gpioIds::MGM_2_LIS3_CS;
     }
     uint32_t spiSpeed = 3'900'000;
-    spi::SpiMode spiMode = spi::SpiMode::MODE_3;
+    spi::SpiModes spiMode = spi::SpiModes::MODE_3;
 #ifdef RASPBERRY_PI
     std::string deviceName = "/dev/spidev0.0";
 #else
@@ -217,7 +217,7 @@ void SpiTestClass::performL3gTest(uint8_t l3gId) {
         currentGpioId = gpioIds::GYRO_2_L3G_CS;
     }
     uint32_t spiSpeed = 3'900'000;
-    spi::SpiMode spiMode = spi::SpiMode::MODE_3;
+    spi::SpiModes spiMode = spi::SpiModes::MODE_3;
 #ifdef RASPBERRY_PI
     std::string deviceName = "/dev/spidev0.0";
 #else
@@ -334,7 +334,7 @@ void SpiTestClass::acsInit() {
     }
 }
 
-void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) {
+void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) {
     int mode_test = SPI_MODE_3;
     int retval = ioctl(spiFd, SPI_IOC_WR_MODE, &mode_test);//reinterpret_cast<uint8_t*>(&mode));
     if(retval != 0) {

linux/boardtest/SpiTestClass.h

@@ -50,7 +50,7 @@ private:
     static constexpr uint8_t RM3100_READ_MASK = STM_READ_MASK;
     static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000;
 
-    void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed);
+    void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed);
 
     void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
             bool autoIncrement);

linux/spi/SpiComIF.cpp

@@ -79,7 +79,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF *cookie) {
     }
 
     size_t spiSpeed = 0;
-    spi::SpiMode spiMode = spi::SpiMode::MODE_0;
+    spi::SpiModes spiMode = spi::SpiModes::MODE_0;
 
     SpiCookie::UncommonParameters params;
     spiCookie->getSpiParameters(spiMode, spiSpeed, &params);
@@ -163,7 +163,7 @@ ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, s
     if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
         return OPENING_FILE_FAILED;
     }
-    spi::SpiMode spiMode = spi::SpiMode::MODE_0;
+    spi::SpiModes spiMode = spi::SpiModes::MODE_0;
     uint32_t spiSpeed = 0;
     spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr);
     setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
@@ -308,7 +308,7 @@ ReturnValue_t SpiComIF::getReadBuffer(address_t spiAddress, uint8_t** buffer) {
     return HasReturnvaluesIF::RETURN_OK;
 }
 
-void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed) {
+void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) {
     int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast<uint8_t*>(&mode));
     if(retval != 0) {
         utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI mode failed!");

linux/spi/SpiComIF.h

@@ -57,7 +57,7 @@ private:
 
 
     ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer);
-    void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed);
+    void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed);
 };
 
 #endif /* LINUX_SPI_SPICOMIF_H_ */

linux/spi/SpiCookie.cpp

@@ -1,17 +1,17 @@
 #include "SpiCookie.h"
 
 SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev,
-        const size_t maxSize, spi::SpiMode spiMode, uint32_t spiSpeed): spiAddress(spiAddress),
+        const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed): spiAddress(spiAddress),
         chipSelectPin(chipSelect), spiDevice(spiDev), maxSize(maxSize), spiMode(spiMode),
         spiSpeed(spiSpeed) {
 }
 
 SpiCookie::SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxSize,
-        spi::SpiMode spiMode, uint32_t spiSpeed):
+        spi::SpiModes spiMode, uint32_t spiSpeed):
         SpiCookie(spiAddress, gpio::NO_GPIO, spiDev, maxSize, spiMode, spiSpeed) {
 }
 
-void SpiCookie::getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed,
+void SpiCookie::getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed,
         UncommonParameters* parameters) const {
     spiMode = this->spiMode;
     spiSpeed = this->spiSpeed;
@@ -97,3 +97,11 @@ void SpiCookie::assignTransferSize(size_t transferSize) {
 size_t SpiCookie::getCurrentTransferSize() const {
     return spiTransferStruct.len;
 }
+
+void SpiCookie::setSpiSpeed(uint32_t newSpeed) {
+    this->spiSpeed = newSpeed;
+}
+
+void SpiCookie::setSpiMode(spi::SpiModes newMode) {
+    this->spiMode = newMode;
+}

linux/spi/SpiCookie.h

@@ -19,19 +19,25 @@ public:
      * @param maxSize
      */
     SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev,
-            const size_t maxReplySize, spi::SpiMode spiMode, uint32_t spiSpeed);
+            const size_t maxReplySize, spi::SpiModes spiMode, uint32_t spiSpeed);
 
     /**
      * Like constructor above, but without a dedicated GPIO CS. Can be used for hardware
      * slave select or if CS logic is performed with decoders.
      */
     SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxReplySize,
-            spi::SpiMode spiMode, uint32_t spiSpeed);
+            spi::SpiModes spiMode, uint32_t spiSpeed);
 
     address_t getSpiAddress() const;
     std::string getSpiDevice() const;
     gpioId_t getChipSelectPin() const;
     size_t getMaxBufferSize() const;
+
+    /** Enables changing SPI speed at run-time */
+    void setSpiSpeed(uint32_t newSpeed);
+    /** Enables changing the SPI mode at run-time */
+    void setSpiMode(spi::SpiModes newMode);
+
     /**
      * True if SPI transfers should be performed in full duplex mode
      * @return
@@ -81,7 +87,7 @@ public:
     void setCsHigh(bool enable);
     void setBitsPerWord(uint8_t bitsPerWord);
 
-    void getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed,
+    void getSpiParameters(spi::SpiModes& spiMode, uint32_t& spiSpeed,
             UncommonParameters* parameters = nullptr) const;
 
     /**
@@ -100,7 +106,7 @@ private:
     std::string spiDevice;
 
     const size_t maxSize;
-    spi::SpiMode spiMode;
+    spi::SpiModes spiMode;
     uint32_t spiSpeed;
     bool halfDuplex = false;
 

linux/spi/spiDefinitions.h

@@ -5,7 +5,7 @@
 
 namespace spi {
 
-enum SpiMode: uint8_t {
+enum SpiModes: uint8_t {
     MODE_0,
     MODE_1,
     MODE_2,

mission/devices/GyroL3GD20Handler.cpp

@@ -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) {
+	if(internalState == InternalState::NONE) {
-		internalState = InternalState::STATE_CONFIGURE;
+		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::NONE):
-	case(InternalState::STATE_NORMAL): {
+	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.
+			/* Attempt reconfiguration. */
-			internalState = InternalState::STATE_CONFIGURE;
+			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;
 }

mission/devices/GyroL3GD20Handler.h

@@ -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,
+	    NONE,
-		STATE_CONFIGURE,
+		CONFIGURE,
-		STATE_NORMAL
+		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
 };
 
 

mission/devices/MGMHandlerLIS3MDL.cpp

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

mission/devices/MGMHandlerRM3100.cpp

@@ -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): {
+    case(InternalState::NONE): {
-        internalState = InternalState::STATE_CONFIGURE_CMM;
+        internalState = InternalState::CONFIGURE_CMM;
         break;
     }
-    case(InternalState::STATE_CONFIGURE_CMM): {
+    case(InternalState::CONFIGURE_CMM): {
-        internalState = InternalState::STATE_READ_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::NONE):
-    case(InternalState::STATE_NORMAL): {
+    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.
+        /* Might be a configuration error. */
-        sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown "
+        sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
-        << "internal state!" << std::endl;
+                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) {

mission/devices/MGMHandlerRM3100.h

@@ -61,18 +61,18 @@ protected:
 private:
 
 	enum class InternalState {
-	    STATE_NONE,
+	    NONE,
-		STATE_CONFIGURE_CMM,
+		CONFIGURE_CMM,
-		STATE_READ_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;
 

mission/devices/devicedefinitions/GyroL3GD20Definitions.h

@@ -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_ */

mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h

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

mission/devices/devicedefinitions/MGMHandlerRM3100Definitions.h

@@ -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;
 
 /*----------------------------------------------------------------------------*/