moved some files

.gitmodules

@@ -1,5 +1,5 @@
 [submodule "etl"]
-	path = etl
+	path = thirdparty/etl
 	url = https://github.com/ETLCPP/etl.git
 [submodule "arduino"]
 	path = arduino
@@ -10,3 +10,6 @@
 [submodule "tmtc"]
 	path = tmtc
 	url = https://egit.irs.uni-stuttgart.de/eive/eive_tmtc.git
+[submodule "thirdparty/lwgps"]
+	path = thirdparty/lwgps
+	url = https://github.com/rmspacefish/lwgps.git

README.md

@@ -208,7 +208,16 @@ or
 ssh eive@192.168.199.227
 ```
 
-To access the console of the Q7S run the following:
+If the static IP address of the Q7S has already been set,
+you can access it with ssh
+
+```sh
+ssh root@192.168.133.10
+```
+
+If this has not been done yet, you can access the serial
+console of the Q7S like this to set it
+
 ```sh
 picocom -b 115200 /dev/ttyUSB0
 ```

bsp_hosted/InitMission.cpp

@@ -11,9 +11,10 @@
 #include <fsfw/tasks/PeriodicTaskIF.h>
 #include <fsfw/tasks/TaskFactory.h>
 
+#include <mission/utility/InitMission.h>
+
 #include <iostream>
 
-// This is configured for linux without \cr
 #ifdef LINUX
 ServiceInterfaceStream sif::debug("DEBUG");
 ServiceInterfaceStream sif::info("INFO");
@@ -28,9 +29,9 @@ ServiceInterfaceStream sif::error("ERROR", true, false, true);
 
 ObjectManagerIF *objectManager = nullptr;
 
-void InitMission::initMission() {
-	sif::info << "Building global objects.." << std::endl;
-	/* Instantiate global object manager and also create all objects */
+void initmission::initMission() {
+    sif::info << "Building global objects.." << std::endl;
+    /* Instantiate global object manager and also create all objects */
     objectManager = new ObjectManager(ObjectFactory::produce);
     sif::info << "Initializing all objects.." << std::endl;
     objectManager->initialize();
@@ -39,117 +40,120 @@ void InitMission::initMission() {
     initTasks();
 }
 
-void InitMission::initTasks(){
-	/* TMTC Distribution */
-	PeriodicTaskIF* TmTcDistributor = TaskFactory::instance()->
-			createPeriodicTask("DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.100, nullptr);
-	ReturnValue_t result = TmTcDistributor->addComponent(
-			objects::CCSDS_PACKET_DISTRIBUTOR);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-	result = TmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-    result = TmTcDistributor->addComponent(objects::TM_FUNNEL);
+void initmission::initTasks() {
+    TaskFactory* factory = TaskFactory::instance();
+    if(factory == nullptr) {
+        /* Should never happen ! */
+        return;
+    }
+#if OBSW_PRINT_MISSED_DEADLINES == 1
+    void (*missedDeadlineFunc) (void) = TaskFactory::printMissedDeadline;
+#else
+    void (*missedDeadlineFunc) (void) = nullptr;
+#endif
+
+    /* TMTC Distribution */
+    PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask(
+            "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
+    ReturnValue_t result = tmTcDistributor->addComponent(
+            objects::CCSDS_PACKET_DISTRIBUTOR);
+    if(result != HasReturnvaluesIF::RETURN_OK){
+        sif::error << "Object add component failed" << std::endl;
+    }
+    result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
+    if(result != HasReturnvaluesIF::RETURN_OK){
+        sif::error << "Object add component failed" << std::endl;
+    }
+    result = tmTcDistributor->addComponent(objects::TM_FUNNEL);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Object add component failed" << std::endl;
+        sif::error << "Object add component failed" << std::endl;
     }
 
     /* UDP bridge */
-    PeriodicTaskIF* UdpBridgeTask = TaskFactory::instance()->createPeriodicTask(
-    		"UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-			0.2, nullptr);
-    result = UdpBridgeTask->addComponent(objects::UDP_BRIDGE);
+    PeriodicTaskIF* udpBridgeTask = factory->createPeriodicTask(
+            "UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
+    result = udpBridgeTask->addComponent(objects::UDP_BRIDGE);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Add component UDP Unix Bridge failed" << std::endl;
+        sif::error << "Add component UDP Unix Bridge failed" << std::endl;
     }
-    PeriodicTaskIF* UdpPollingTask = TaskFactory::instance()->
-    		createPeriodicTask("UDP_POLLING", 80,
-    		PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, nullptr);
-    result = UdpPollingTask->addComponent(objects::UDP_POLLING_TASK);
+    PeriodicTaskIF* udpPollingTask = factory->createPeriodicTask(
+            "UDP_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
+    result = udpPollingTask->addComponent(objects::UDP_POLLING_TASK);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Add component UDP Polling failed" << std::endl;
+        sif::error << "Add component UDP Polling failed" << std::endl;
     }
 
-	/* PUS Services */
-	PeriodicTaskIF* PusVerification = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
+    /* PUS Services */
+    PeriodicTaskIF* pusVerification = factory->createPeriodicTask(
+            "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
+    result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
+    if(result != HasReturnvaluesIF::RETURN_OK){
+        sif::error << "Object add component failed" << std::endl;
+    }
 
-	PeriodicTaskIF* PusEvents = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 60,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
+    PeriodicTaskIF* pusEvents = factory->createPeriodicTask(
+            "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
+    result = pusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
+    if(result != HasReturnvaluesIF::RETURN_OK){
+        initmission::printAddObjectError("PUS5", objects::PUS_SERVICE_5_EVENT_REPORTING);
+    }
 
-	PeriodicTaskIF* PusHighPrio = TaskFactory::instance()->
-			createPeriodicTask("PUS_HIGH_PRIO", 50,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.200, nullptr);
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
+    PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
+            "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
+    result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS);
+    }
+    result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT);
+    }
 
-	PeriodicTaskIF* PusMedPrio = TaskFactory::instance()->
-			createPeriodicTask("PUS_HIGH_PRIO", 40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.8, nullptr);
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
+    PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask(
+            "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc);
+    result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT);
+    }
+    result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT);
+    }
+    result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS);
+    }
 
-	PeriodicTaskIF* PusLowPrio = TaskFactory::instance()->
-			createPeriodicTask("PUSB", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					1.6, nullptr);
-	result = PusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
-	if(result!=HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
+    PeriodicTaskIF* pusLowPrio = factory->createPeriodicTask(
+            "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc);
+    result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST);
+    }
 
+    PeriodicTaskIF* testTask = factory->createPeriodicTask(
+            "TEST_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
+#if OBSW_ADD_TEST_CODE == 1
+    result = testTask->addComponent(objects::TEST_TASK);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK);
+    }
+#endif /* OBSW_ADD_TEST_CODE == 1 */
+
+    sif::info << "Starting tasks.." << std::endl;
+    tmTcDistributor->startTask();
+    udpBridgeTask->startTask();
+    udpPollingTask->startTask();
+
+    pusVerification->startTask();
+    pusEvents->startTask();
+    pusHighPrio->startTask();
+    pusMedPrio->startTask();
+    pusLowPrio->startTask();
 
 #if OBSW_ADD_TEST_CODE == 1
-	FixedTimeslotTaskIF* TestTimeslotTask = TaskFactory::instance()->
-			createFixedTimeslotTask("PST_TEST_TASK", 10,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
-	result = pst::pollingSequenceTestFunction(TestTimeslotTask);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		sif::error << "InitMission::createTasks: Test PST initialization "
-				<< "failed!" << std::endl;
-	}
-#endif
+    testTask->startTask();
+#endif /* OBSW_ADD_TEST_CODE == 1 */
 
-	//Main thread sleep
-	sif::info << "Starting tasks.." << std::endl;
-	TmTcDistributor->startTask();
-	UdpBridgeTask->startTask();
-	UdpPollingTask->startTask();
-
-	PusVerification->startTask();
-	PusEvents->startTask();
-	PusHighPrio->startTask();
-	PusMedPrio->startTask();
-	PusLowPrio->startTask();
-#if OBSW_ADD_TEST_CODE == 1
-	TestTimeslotTask->startTask();
-#endif
-	sif::info << "Tasks started.." << std::endl;
+    sif::info << "Tasks started.." << std::endl;
 }

bsp_hosted/InitMission.h

@@ -1,7 +1,7 @@
 #ifndef BSP_LINUX_INITMISSION_H_
 #define BSP_LINUX_INITMISSION_H_
 
-namespace InitMission {
+namespace initmission {
 void initMission();
 void initTasks();
 };

bsp_hosted/fsfwconfig/OBSWConfig.h

@@ -6,7 +6,9 @@
 #ifndef CONFIG_OBSWCONFIG_H_
 #define CONFIG_OBSWCONFIG_H_
 
-#define OBSW_ADD_TEST_CODE      0
+#include "commonConfig.h"
+
+#define OBSW_ADD_TEST_CODE      1
 
 /* These defines should be disabled for mission code but are useful for
 debugging. */

bsp_hosted/fsfwconfig/devices/gpioIds.h

@@ -0,0 +1,35 @@
+#ifndef FSFWCONFIG_DEVICES_GPIOIDS_H_
+#define FSFWCONFIG_DEVICES_GPIOIDS_H_
+
+#include <linux/gpio/GpioIF.h>
+
+namespace gpioIds {
+	enum gpioId_t {
+		HEATER_0,
+		HEATER_1,
+		HEATER_2,
+		HEATER_3,
+		HEATER_4,
+		HEATER_5,
+		HEATER_6,
+		HEATER_7,
+		DEPLSA1,
+		DEPLSA2,
+
+		MGM_0_LIS3_CS,
+		MGM_1_RM3100_CS,
+		GYRO_0_ADIS_CS,
+		GYRO_1_L3G_CS,
+		GYRO_2_L3G_CS,
+		MGM_2_LIS3_CS,
+		MGM_3_RM3100_CS,
+
+		TEST_ID_0,
+		TEST_ID_1
+	};
+}
+
+
+
+
+#endif /* FSFWCONFIG_DEVICES_GPIOIDS_H_ */

bsp_hosted/fsfwconfig/devices/powerSwitcherList.h

@@ -0,0 +1,58 @@
+#ifndef FSFWCONFIG_DEVICES_POWERSWITCHERLIST_H_
+#define FSFWCONFIG_DEVICES_POWERSWITCHERLIST_H_
+
+#include <OBSWConfig.h>
+
+namespace pcduSwitches {
+	/* Switches are uint8_t datatype and go from 0 to 255 */
+	enum switcherList {
+	    Q7S,
+        PAYLOAD_PCDU_CH1,
+        RW,
+        TCS_BOARD_8V_HEATER_IN,
+        SUS_REDUNDANT,
+        DEPLOYMENT_MECHANISM,
+        PAYLOAD_PCDU_CH6,
+        ACS_BOARD_SIDE_B,
+        PAYLOAD_CAMERA,
+        TCS_BOARD_3V3,
+        SYRLINKS,
+        STAR_TRACKER,
+        MGT,
+        SUS_NOMINAL,
+        SOLAR_CELL_EXP,
+        PLOC,
+        ACS_BORAD_SIDE_A,
+        NUMBER_OF_SWITCHES
+	};
+
+	static const uint8_t ON = 1;
+	static const uint8_t OFF = 0;
+
+	/* Output states after reboot of the PDUs */
+	static const uint8_t INIT_STATE_Q7S = ON;
+	static const uint8_t INIT_STATE_PAYLOAD_PCDU_CH1 = OFF;
+	static const uint8_t INIT_STATE_RW = OFF;
+#if TE0720 == 1
+	/* Because the TE0720 is not connected to the PCDU, this switch is always on */
+	static const uint8_t INIT_STATE_TCS_BOARD_8V_HEATER_IN = ON;
+#else
+	static const uint8_t INIT_STATE_TCS_BOARD_8V_HEATER_IN = OFF;
+#endif
+	static const uint8_t INIT_STATE_SUS_REDUNDANT = OFF;
+	static const uint8_t INIT_STATE_DEPLOYMENT_MECHANISM = OFF;
+	static const uint8_t INIT_STATE_PAYLOAD_PCDU_CH6 = OFF;
+	static const uint8_t INIT_STATE_ACS_BOARD_SIDE_B = OFF;
+	static const uint8_t INIT_STATE_PAYLOAD_CAMERA = OFF;
+	static const uint8_t INIT_STATE_TCS_BOARD_3V3 = OFF;
+	static const uint8_t INIT_STATE_SYRLINKS = OFF;
+	static const uint8_t INIT_STATE_STAR_TRACKER = OFF;
+	static const uint8_t INIT_STATE_MGT = OFF;
+	static const uint8_t INIT_STATE_SUS_NOMINAL = OFF;
+	static const uint8_t INIT_STATE_SOLAR_CELL_EXP = OFF;
+	static const uint8_t INIT_STATE_PLOC = OFF;
+	static const uint8_t INIT_STATE_ACS_BOARD_SIDE_A = OFF;
+}
+
+
+#endif /* FSFWCONFIG_DEVICES_POWERSWITCHERLIST_H_ */

bsp_hosted/fsfwconfig/objects/systemObjectList.h

@@ -27,6 +27,21 @@ namespace objects {
 		DUMMY_INTERFACE = 0xCAFECAFE,
 		DUMMY_HANDLER = 0x4400AFFE,
 
+        /* 0x44 ('D') for device handlers */
+        P60DOCK_HANDLER = 0x44000001,
+        PDU1_HANDLER = 0x44000002,
+        PDU2_HANDLER = 0x44000003,
+        ACU_HANDLER = 0x44000004,
+        TMP1075_HANDLER_1 = 0x44000005,
+        TMP1075_HANDLER_2 = 0x44000006,
+        MGM_0_LIS3_HANDLER = 0x4400007,
+        MGM_1_RM3100_HANDLER = 0x44000008,
+        MGM_2_LIS3_HANDLER = 0x44000009,
+        MGM_3_RM3100_HANDLER = 0x44000010,
+        GYRO_0_ADIS_HANDLER = 0x44000011,
+        GYRO_1_L3G_HANDLER = 0x44000012,
+        GYRO_2_L3G_HANDLER = 0x44000013,
+
 		/* 0x49 ('I') for Communication Interfaces **/
 		ARDUINO_COM_IF = 0x49000001
 	};

bsp_hosted/main.cpp

@@ -24,7 +24,7 @@ int main(void)
             << SW_SUBVERSION << "." << SW_SUBSUBVERSION << " -- " << std::endl;
     std::cout << "-- " <<  __DATE__ << " " << __TIME__ << " --" << std::endl;
 
-	InitMission::initMission();
+	initmission::initMission();
 
 	for(;;) {
 		// suspend main thread by sleeping it.

bsp_q7s/CMakeLists.txt

@@ -6,6 +6,7 @@ target_sources(${TARGET_NAME} PUBLIC
 
 add_subdirectory(boardconfig)
 add_subdirectory(comIF)
+add_subdirectory(devices)
 add_subdirectory(boardtest)
 
 

bsp_q7s/InitMission.h

@@ -1,7 +1,7 @@
 #ifndef BSP_Q7S_INITMISSION_H_
 #define BSP_Q7S_INITMISSION_H_
 
-namespace InitMission {
+namespace initmission {
 void initMission();
 void initTasks();
 };

bsp_q7s/boardtest/CMakeLists.txt

@@ -1,5 +1,4 @@
 target_sources(${TARGET_NAME} PRIVATE
-	LibgpiodTest.cpp
 )
 
 

bsp_q7s/devices/CMakeLists.txt

@@ -0,0 +1,4 @@
+target_sources(${TARGET_NAME} PRIVATE
+	HeaterHandler.cpp
+	SolarArrayDeploymentHandler.cpp
+)

bsp_q7s/devices/HeaterHandler.cpp

@@ -0,0 +1,356 @@
+#include "HeaterHandler.h"
+#include <fsfwconfig/devices/powerSwitcherList.h>
+#include <fsfw/ipc/QueueFactory.h>
+#include <devices/gpioIds.h>
+#include <linux/gpio/GpioCookie.h>
+
+HeaterHandler::HeaterHandler(object_id_t setObjectId_, object_id_t gpioDriverId_,
+        CookieIF * gpioCookie_, object_id_t mainLineSwitcherObjectId_, uint8_t mainLineSwitch_) :
+        SystemObject(setObjectId_), gpioDriverId(gpioDriverId_), gpioCookie(gpioCookie_),
+        mainLineSwitcherObjectId(mainLineSwitcherObjectId_), mainLineSwitch(mainLineSwitch_),
+        actionHelper(this, nullptr) {
+    commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
+            MessageQueueMessage::MAX_MESSAGE_SIZE);
+}
+
+HeaterHandler::~HeaterHandler() {
+}
+
+ReturnValue_t HeaterHandler::performOperation(uint8_t operationCode) {
+
+	if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
+		readCommandQueue();
+		handleActiveCommands();
+		return RETURN_OK;
+	}
+	return RETURN_OK;
+}
+
+ReturnValue_t HeaterHandler::initialize() {
+	ReturnValue_t result = SystemObject::initialize();
+	if (result != RETURN_OK) {
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	result = initializeHeaterMap();
+	if (result != RETURN_OK) {
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	gpioInterface = objectManager->get<GpioIF>(gpioDriverId);
+	if (gpioInterface == nullptr) {
+        sif::error << "HeaterHandler::initialize: Invalid Gpio interface." << std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
+	if (result != RETURN_OK) {
+        sif::error << "HeaterHandler::initialize: Failed to initialize Gpio interface" << std::endl;
+	    return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+	if (IPCStore == nullptr) {
+		sif::error << "HeaterHandler::initialize: IPC store not set up in factory." << std::endl;
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	if(mainLineSwitcherObjectId != objects::NO_OBJECT) {
+		mainLineSwitcher = objectManager->get<PowerSwitchIF>(mainLineSwitcherObjectId);
+		if (mainLineSwitcher == nullptr) {
+			sif::error << "HeaterHandler::initialize: Main line switcher failed to fetch object"
+					<< "from object ID." << std::endl;
+			return ObjectManagerIF::CHILD_INIT_FAILED;
+		}
+	}
+
+	result = actionHelper.initialize(commandQueue);
+	if (result != RETURN_OK) {
+		return ObjectManagerIF::CHILD_INIT_FAILED;
+	}
+
+	return RETURN_OK;
+}
+
+ReturnValue_t HeaterHandler::initializeHeaterMap(){
+	HeaterCommandInfo_t heaterCommandInfo;
+	for(switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
+		std::pair status = heaterMap.emplace(switchNr, heaterCommandInfo);
+		if (status.second == false) {
+			sif::error << "HeaterHandler::initializeHeaterMap: Failed to initialize heater map"
+					<< std::endl;
+			return RETURN_FAILED;
+		}
+	}
+	return RETURN_OK;
+}
+
+void HeaterHandler::setInitialSwitchStates() {
+	for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
+		switchStates[switchNr] = OFF;
+	}
+}
+
+void HeaterHandler::readCommandQueue() {
+	CommandMessage command;
+	ReturnValue_t result = commandQueue->receiveMessage(&command);
+	if (result != RETURN_OK) {
+		return;
+	}
+
+	result = actionHelper.handleActionMessage(&command);
+	if (result == RETURN_OK) {
+		return;
+	}
+}
+
+ReturnValue_t HeaterHandler::executeAction(ActionId_t actionId,
+		MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
+    ReturnValue_t result;
+    if (actionId != SWITCH_HEATER) {
+		result = COMMAND_NOT_SUPPORTED;
+	} else {
+	    switchNr_t switchNr = *data;
+	    HeaterMapIter heaterMapIter = heaterMap.find(switchNr);
+		if (heaterMapIter != heaterMap.end()) {
+		    if (heaterMapIter->second.active) {
+		        return COMMAND_ALREADY_WAITING;
+		    }
+			heaterMapIter->second.action = *(data + 1);
+			heaterMapIter->second.active = true;
+			heaterMapIter->second.replyQueue = commandedBy;
+		}
+		else {
+			sif::error << "HeaterHandler::executeAction: Invalid switchNr" << std::endl;
+			return INVALID_SWITCH_NR;
+		}
+		result = RETURN_OK;
+	}
+	return result;
+}
+
+
+void HeaterHandler::sendSwitchCommand(uint8_t switchNr,
+		ReturnValue_t onOff) const {
+
+	ReturnValue_t result;
+	store_address_t storeAddress;
+	uint8_t commandData[2];
+
+	switch(onOff) {
+	case PowerSwitchIF::SWITCH_ON:
+		commandData[0] = switchNr;
+		commandData[1] = SET_SWITCH_ON;
+		break;
+	case PowerSwitchIF::SWITCH_OFF:
+		commandData[0] = switchNr;
+		commandData[1] = SET_SWITCH_OFF;
+		break;
+	default:
+		sif::error << "HeaterHandler::sendSwitchCommand: Invalid switch request"
+				<< std::endl;
+		break;
+	}
+
+	result = IPCStore->addData(&storeAddress, commandData, sizeof(commandData));
+	if (result == RETURN_OK) {
+		CommandMessage message;
+		ActionMessage::setCommand(&message, SWITCH_HEATER, storeAddress);
+		/* Send heater command to own command queue */
+		result = commandQueue->sendMessage(commandQueue->getId(), &message, 0);
+		if (result != RETURN_OK) {
+			sif::debug << "HeaterHandler::sendSwitchCommand: Failed to send switch"
+					<< "message" << std::endl;
+		}
+	}
+}
+
+void HeaterHandler::handleActiveCommands(){
+
+	HeaterMapIter heaterMapIter = heaterMap.begin();
+	for (; heaterMapIter != heaterMap.end(); heaterMapIter++) {
+		if (heaterMapIter->second.active) {
+			switch(heaterMapIter->second.action) {
+			case SET_SWITCH_ON:
+			    handleSwitchOnCommand(heaterMapIter);
+				break;
+			case SET_SWITCH_OFF:
+			    handleSwitchOffCommand(heaterMapIter);
+			    break;
+			default:
+                sif::error << "HeaterHandler::handleActiveCommands: Invalid action commanded"
+                        << std::endl;
+                break;
+			}
+		}
+	}
+}
+
+void HeaterHandler::handleSwitchOnCommand(HeaterMapIter heaterMapIter) {
+
+    ReturnValue_t result = RETURN_OK;
+    switchNr_t switchNr;
+
+    /* Check if command waits for main switch being set on and whether the timeout has expired */
+    if (heaterMapIter->second.waitMainSwitchOn
+            && heaterMapIter->second.mainSwitchCountdown.hasTimedOut()) {
+        //TODO - This requires the initiation of an FDIR procedure
+        triggerEvent(MAIN_SWITCH_TIMEOUT);
+        sif::error << "HeaterHandler::handleSwitchOnCommand: Main switch setting on timeout"
+                << std::endl;
+        heaterMapIter->second.active = false;
+        heaterMapIter->second.waitMainSwitchOn = false;
+        if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
+            actionHelper.finish(heaterMapIter->second.replyQueue, heaterMapIter->second.action,
+                    MAIN_SWITCH_SET_TIMEOUT );
+        }
+        return;
+    }
+
+    switchNr = heaterMapIter->first;
+    /* Check state of main line switch */
+    ReturnValue_t mainSwitchState = mainLineSwitcher->getSwitchState(mainLineSwitch);
+    if (mainSwitchState == PowerSwitchIF::SWITCH_ON) {
+        if (!checkSwitchState(switchNr)) {
+            gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
+            result = gpioInterface->pullHigh(gpioId);
+            if (result != RETURN_OK) {
+                sif::error << "HeaterHandler::handleSwitchOnCommand: Failed to pull gpio with id "
+                        << gpioId << " high" << std::endl;
+                triggerEvent(GPIO_PULL_HIGH_FAILED, result);
+            }
+            else {
+                switchStates[switchNr] = ON;
+            }
+        }
+        else {
+            triggerEvent(SWITCH_ALREADY_ON, switchNr);
+        }
+        /* There is no need to send action finish replies if the sender was the
+         * HeaterHandler itself. */
+        if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
+            actionHelper.finish(heaterMapIter->second.replyQueue,
+                    heaterMapIter->second.action, result);
+        }
+        heaterMapIter->second.active = false;
+        heaterMapIter->second.waitMainSwitchOn = false;
+    }
+    else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF
+            && heaterMapIter->second.waitMainSwitchOn) {
+        /* Just waiting for the main switch being set on */
+        return;
+    }
+    else if (mainSwitchState == PowerSwitchIF::SWITCH_OFF) {
+        mainLineSwitcher->sendSwitchCommand(mainLineSwitch,
+                PowerSwitchIF::SWITCH_ON);
+        heaterMapIter->second.mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
+        heaterMapIter->second.waitMainSwitchOn = true;
+    }
+    else {
+        sif::debug << "HeaterHandler::handleActiveCommands: Failed to get state of"
+                << " main line switch" << std::endl;
+        if (heaterMapIter->second.replyQueue != commandQueue->getId()) {
+            actionHelper.finish(heaterMapIter->second.replyQueue,
+                    heaterMapIter->second.action, mainSwitchState);
+        }
+        heaterMapIter->second.active = false;
+    }
+}
+
+void HeaterHandler::handleSwitchOffCommand(HeaterMapIter heaterMapIter) {
+    ReturnValue_t result = RETURN_OK;
+    switchNr_t switchNr = heaterMapIter->first;
+    /* Check whether switch is already off */
+    if (checkSwitchState(switchNr)) {
+        gpioId_t gpioId = getGpioIdFromSwitchNr(switchNr);
+        result = gpioInterface->pullLow(gpioId);
+        if (result != RETURN_OK) {
+            sif::error << "HeaterHandler::handleSwitchOffCommand: Failed to pull gpio with id"
+                    << gpioId << " low" << std::endl;
+            triggerEvent(GPIO_PULL_LOW_FAILED, result);
+        }
+        else {
+            switchStates[switchNr] = OFF;
+            /* When all switches are off, also main line switch will be turned off */
+            if (allSwitchesOff()) {
+                mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+            }
+        }
+    }
+    else {
+        sif::info << "HeaterHandler::handleSwitchOffCommand: Switch already off" << std::endl;
+        triggerEvent(SWITCH_ALREADY_OFF, switchNr);
+    }
+    if (heaterMapIter->second.replyQueue != NO_COMMANDER) {
+        actionHelper.finish(heaterMapIter->second.replyQueue,
+                heaterMapIter->second.action, result);
+    }
+    heaterMapIter->second.active = false;
+}
+
+bool HeaterHandler::checkSwitchState(int switchNr) {
+    return switchStates[switchNr];
+}
+
+bool HeaterHandler::allSwitchesOff() {
+    bool allSwitchesOrd = false;
+    /* Or all switches. As soon one switch is on, allSwitchesOrd will be true */
+    for (switchNr_t switchNr = 0; switchNr < heaterSwitches::NUMBER_OF_SWITCHES; switchNr++) {
+        allSwitchesOrd = allSwitchesOrd  || switchStates[switchNr];
+    }
+    return !allSwitchesOrd;
+}
+
+gpioId_t HeaterHandler::getGpioIdFromSwitchNr(int switchNr) {
+    gpioId_t gpioId = 0xFFFF;
+    switch(switchNr) {
+    case heaterSwitches::HEATER_0:
+        gpioId = gpioIds::HEATER_0;
+        break;
+    case heaterSwitches::HEATER_1:
+        gpioId = gpioIds::HEATER_1;
+        break;
+    case heaterSwitches::HEATER_2:
+        gpioId = gpioIds::HEATER_2;
+        break;
+    case heaterSwitches::HEATER_3:
+        gpioId = gpioIds::HEATER_3;
+        break;
+    case heaterSwitches::HEATER_4:
+        gpioId = gpioIds::HEATER_4;
+        break;
+    case heaterSwitches::HEATER_5:
+        gpioId = gpioIds::HEATER_5;
+        break;
+    case heaterSwitches::HEATER_6:
+        gpioId = gpioIds::HEATER_6;
+        break;
+    case heaterSwitches::HEATER_7:
+        gpioId = gpioIds::HEATER_7;
+        break;
+    default:
+        sif::error << "HeaterHandler::getGpioIdFromSwitchNr: Unknown heater switch number"
+                << std::endl;
+        break;
+    }
+    return gpioId;
+}
+
+MessageQueueId_t HeaterHandler::getCommandQueue() const {
+    return commandQueue->getId();
+}
+
+void HeaterHandler::sendFuseOnCommand(uint8_t fuseNr) const {
+}
+
+ReturnValue_t HeaterHandler::getSwitchState( uint8_t switchNr ) const {
+    return 0;
+}
+
+ReturnValue_t HeaterHandler::getFuseState( uint8_t fuseNr ) const {
+    return 0;
+}
+
+uint32_t HeaterHandler::getSwitchDelayMs(void) const {
+    return 0;
+}

bsp_q7s/devices/HeaterHandler.h

@@ -0,0 +1,177 @@
+#ifndef MISSION_DEVICES_HEATERHANDLER_H_
+#define MISSION_DEVICES_HEATERHANDLER_H_
+
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <fsfw/action/HasActionsIF.h>
+#include <fsfw/power/PowerSwitchIF.h>
+#include <fsfwconfig/devices/heaterSwitcherList.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/devicehandlers/CookieIF.h>
+#include <fsfw/timemanager/Countdown.h>
+#include <linux/gpio/GpioIF.h>
+#include <unordered_map>
+
+/**
+ * @brief	This class intends the control of heaters.
+ *
+ * @author	J. Meier
+ */
+class HeaterHandler: public ExecutableObjectIF,
+        public PowerSwitchIF,
+        public SystemObject,
+        public HasActionsIF {
+public:
+
+	/** Device command IDs */
+	static const DeviceCommandId_t SWITCH_HEATER = 0x0;
+
+    HeaterHandler(object_id_t setObjectId, object_id_t gpioDriverId, CookieIF * gpioCookie,
+            object_id_t mainLineSwitcherObjectId, uint8_t mainLineSwitch);
+
+    virtual ~HeaterHandler();
+
+	virtual ReturnValue_t performOperation(uint8_t operationCode = 0) override;
+
+	virtual void sendSwitchCommand(uint8_t switchNr, ReturnValue_t onOff) const override;
+	virtual void sendFuseOnCommand(uint8_t fuseNr) const override;
+	/**
+	 * @brief	This function will be called from the Heater object to check
+	 * 			the current switch state.
+	 */
+	virtual ReturnValue_t getSwitchState( uint8_t switchNr ) const override;
+	virtual ReturnValue_t getFuseState( uint8_t fuseNr ) const override;
+	virtual uint32_t getSwitchDelayMs(void) const override;
+
+	virtual MessageQueueId_t getCommandQueue() const override;
+    virtual ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
+            const uint8_t* data, size_t size) override;
+	virtual ReturnValue_t initialize() override;
+
+private:
+
+	static const uint8_t INTERFACE_ID = CLASS_ID::HEATER_HANDLER;
+
+	static const ReturnValue_t COMMAND_NOT_SUPPORTED = MAKE_RETURN_CODE(0xA1);
+	static const ReturnValue_t INIT_FAILED = MAKE_RETURN_CODE(0xA2);
+	static const ReturnValue_t INVALID_SWITCH_NR = MAKE_RETURN_CODE(0xA3);
+	static const ReturnValue_t MAIN_SWITCH_SET_TIMEOUT = MAKE_RETURN_CODE(0xA4);
+	static const ReturnValue_t COMMAND_ALREADY_WAITING = MAKE_RETURN_CODE(0xA5);
+
+	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::HEATER_HANDLER;
+	static const Event GPIO_PULL_HIGH_FAILED = MAKE_EVENT(0, severity::LOW);
+	static const Event GPIO_PULL_LOW_FAILED = MAKE_EVENT(1, severity::LOW);
+	static const Event SWITCH_ALREADY_ON = MAKE_EVENT(2, severity::LOW);
+	static const Event SWITCH_ALREADY_OFF = MAKE_EVENT(3, severity::LOW);
+	static const Event MAIN_SWITCH_TIMEOUT = MAKE_EVENT(4, severity::LOW);
+
+	static const MessageQueueId_t NO_COMMANDER = 0;
+
+	enum SwitchState : bool {
+        ON = true,
+        OFF = false
+      };
+
+
+    /**
+     * @brief   Struct holding information about a heater command to execute.
+     *
+     * @param action    The action to perform.
+     * @param replyQueue    The queue of the commander to which status replies
+     *                      will be sent.
+     * @param active    True if command is waiting for execution, otherwise false.
+     * @param waitSwitchOn  True if the command is waiting for the main switch being set on.
+     * @param mainSwitchCountdown   Sets timeout to wait for main switch being set on.
+     */
+    typedef struct HeaterCommandInfo {
+        uint8_t action;
+        MessageQueueId_t replyQueue;
+        bool active = false;
+        bool waitMainSwitchOn = false;
+        Countdown mainSwitchCountdown;
+    } HeaterCommandInfo_t;
+
+    enum SwitchAction {
+        SET_SWITCH_OFF,
+        SET_SWITCH_ON
+    };
+
+    using switchNr_t = uint8_t;
+    using HeaterMap = std::unordered_map<switchNr_t, HeaterCommandInfo_t>;
+    using HeaterMapIter = HeaterMap::iterator;
+
+    HeaterMap heaterMap;
+
+    bool switchStates[heaterSwitches::NUMBER_OF_SWITCHES];
+
+    /** Size of command queue */
+    size_t cmdQueueSize = 20;
+
+    /**
+     * The object ID of the GPIO driver which enables and disables the
+     * heaters.
+     */
+    object_id_t gpioDriverId;
+
+    CookieIF * gpioCookie;
+
+    GpioIF* gpioInterface = nullptr;
+
+    /** Queue to receive messages from other objects. */
+    MessageQueueIF* commandQueue = nullptr;
+
+    object_id_t mainLineSwitcherObjectId;
+
+    /** Switch number of the heater power supply switch */
+    uint8_t mainLineSwitch;
+
+    /**
+     * Power switcher object which controls the 8V main line of the heater
+     * logic on the TCS board.
+     */
+    PowerSwitchIF *mainLineSwitcher = nullptr;
+
+    ActionHelper actionHelper;
+
+    StorageManagerIF *IPCStore = nullptr;
+
+	void readCommandQueue();
+
+	/**
+	 * @brief   Returns the state of a switch (ON - true, or OFF - false).
+	 * @param switchNr  The number of the switch to check.
+	 */
+	bool checkSwitchState(int switchNr);
+
+	/**
+	 * @brief   Returns the ID of the GPIO related to a heater identified by the switch number
+	 *          which is defined in the heaterSwitches list.
+	 */
+	gpioId_t getGpioIdFromSwitchNr(int switchNr);
+
+	/**
+	 * @brief	This function runs commands waiting for execution.
+	 */
+	void handleActiveCommands();
+
+	ReturnValue_t initializeHeaterMap();
+
+	/**
+	 * @brief	Sets all switches to OFF.
+	 */
+	void setInitialSwitchStates();
+
+	void handleSwitchOnCommand(HeaterMapIter heaterMapIter);
+
+	void handleSwitchOffCommand(HeaterMapIter heaterMapIter);
+
+	/**
+	 * @brief   Checks if all switches are off.
+	 * @return  True if all switches are off, otherwise false.
+	 */
+	bool allSwitchesOff();
+
+};
+
+#endif /* MISSION_DEVICES_HEATERHANDLER_H_ */

bsp_q7s/devices/SolarArrayDeploymentHandler.cpp

@@ -0,0 +1,201 @@
+#include "SolarArrayDeploymentHandler.h"
+
+#include <devices/powerSwitcherList.h>
+#include <devices/gpioIds.h>
+
+#include <linux/gpio/GpioCookie.h>
+#include <fsfw/ipc/QueueFactory.h>
+
+
+SolarArrayDeploymentHandler::SolarArrayDeploymentHandler(object_id_t setObjectId_,
+        object_id_t gpioDriverId_, CookieIF * gpioCookie_, object_id_t mainLineSwitcherObjectId_,
+        uint8_t mainLineSwitch_, gpioId_t deplSA1, gpioId_t deplSA2, uint32_t burnTimeMs) :
+        SystemObject(setObjectId_), gpioDriverId(gpioDriverId_), gpioCookie(gpioCookie_),
+        mainLineSwitcherObjectId(mainLineSwitcherObjectId_), mainLineSwitch(mainLineSwitch_),
+        deplSA1(deplSA1), deplSA2(deplSA2), burnTimeMs(burnTimeMs), actionHelper(this, nullptr) {
+    commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize,
+            MessageQueueMessage::MAX_MESSAGE_SIZE);
+}
+
+SolarArrayDeploymentHandler::~SolarArrayDeploymentHandler() {
+}
+
+ReturnValue_t SolarArrayDeploymentHandler::performOperation(uint8_t operationCode) {
+
+    if (operationCode == DeviceHandlerIF::PERFORM_OPERATION) {
+        handleStateMachine();
+        return RETURN_OK;
+    }
+    return RETURN_OK;
+}
+
+ReturnValue_t SolarArrayDeploymentHandler::initialize() {
+    ReturnValue_t result = SystemObject::initialize();
+    if (result != RETURN_OK) {
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+
+    gpioInterface = objectManager->get<GpioIF>(gpioDriverId);
+    if (gpioInterface == nullptr) {
+        sif::error << "SolarArrayDeploymentHandler::initialize: Invalid Gpio interface."
+                << std::endl;
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+
+    result = gpioInterface->addGpios(dynamic_cast<GpioCookie*>(gpioCookie));
+    if (result != RETURN_OK) {
+        sif::error << "SolarArrayDeploymentHandler::initialize: Failed to initialize Gpio interface"
+                << std::endl;
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+
+    if (mainLineSwitcherObjectId != objects::NO_OBJECT) {
+        mainLineSwitcher = objectManager->get<PowerSwitchIF>(mainLineSwitcherObjectId);
+        if (mainLineSwitcher == nullptr) {
+            sif::error
+                    << "SolarArrayDeploymentHandler::initialize: Main line switcher failed to fetch object"
+                    << "from object ID." << std::endl;
+            return ObjectManagerIF::CHILD_INIT_FAILED;
+        }
+    }
+
+    result = actionHelper.initialize(commandQueue);
+    if (result != RETURN_OK) {
+        return ObjectManagerIF::CHILD_INIT_FAILED;
+    }
+
+    return RETURN_OK;
+}
+
+void SolarArrayDeploymentHandler::handleStateMachine() {
+    switch (stateMachine) {
+    case WAIT_ON_DELOYMENT_COMMAND:
+        readCommandQueue();
+        break;
+    case SWITCH_8V_ON:
+        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_ON);
+        mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
+        stateMachine = WAIT_ON_8V_SWITCH;
+        break;
+    case WAIT_ON_8V_SWITCH:
+        performWaitOn8VActions();
+        break;
+    case SWITCH_DEPL_GPIOS:
+        switchDeploymentTransistors();
+        break;
+    case WAIT_ON_DEPLOYMENT_FINISH:
+        handleDeploymentFinish();
+        break;
+    case WAIT_FOR_MAIN_SWITCH_OFF:
+        if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_OFF) {
+            stateMachine = WAIT_ON_DELOYMENT_COMMAND;
+        } else if (mainSwitchCountdown.hasTimedOut()) {
+            triggerEvent(MAIN_SWITCH_OFF_TIMEOUT);
+            sif::error << "SolarArrayDeploymentHandler::handleStateMachine: Failed to switch main"
+                    << " switch off" << std::endl;
+            stateMachine = WAIT_ON_DELOYMENT_COMMAND;
+        }
+        break;
+    default:
+        sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Invalid state" << std::endl;
+        break;
+    }
+}
+
+void SolarArrayDeploymentHandler::performWaitOn8VActions() {
+    if (mainLineSwitcher->getSwitchState(mainLineSwitch) == PowerSwitchIF::SWITCH_ON) {
+        stateMachine = SWITCH_DEPL_GPIOS;
+    } else {
+        if (mainSwitchCountdown.hasTimedOut()) {
+            triggerEvent(MAIN_SWITCH_ON_TIMEOUT);
+            actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
+                    MAIN_SWITCH_TIMEOUT_FAILURE);
+            stateMachine = WAIT_ON_DELOYMENT_COMMAND;
+        }
+    }
+}
+
+void SolarArrayDeploymentHandler::switchDeploymentTransistors() {
+    ReturnValue_t result = RETURN_OK;
+    result = gpioInterface->pullHigh(deplSA1);
+    if (result != RETURN_OK) {
+        sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
+                " array deployment switch 1 high " << std::endl;
+        /* If gpio switch high failed, state machine is reset to wait for a command reinitiating
+         * the deployment sequence. */
+        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
+        triggerEvent(DEPL_SA1_GPIO_SWTICH_ON_FAILED);
+        actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
+            SWITCHING_DEPL_SA2_FAILED);
+        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+    }
+    result = gpioInterface->pullHigh(deplSA2);
+    if (result != RETURN_OK) {
+        sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
+                " array deployment switch 2 high " << std::endl;
+        stateMachine = WAIT_ON_DELOYMENT_COMMAND;
+        triggerEvent(DEPL_SA2_GPIO_SWTICH_ON_FAILED);
+        actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS,
+            SWITCHING_DEPL_SA2_FAILED);
+        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+    }
+    deploymentCountdown.setTimeout(burnTimeMs);
+    stateMachine = WAIT_ON_DEPLOYMENT_FINISH;
+}
+
+void SolarArrayDeploymentHandler::handleDeploymentFinish() {
+    ReturnValue_t result = RETURN_OK;
+    if (deploymentCountdown.hasTimedOut()) {
+        actionHelper.finish(rememberCommanderId, DEPLOY_SOLAR_ARRAYS, RETURN_OK);
+        result = gpioInterface->pullLow(deplSA1);
+        if (result != RETURN_OK) {
+            sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
+                    " array deployment switch 1 low " << std::endl;
+        }
+        result = gpioInterface->pullLow(deplSA2);
+        if (result != RETURN_OK) {
+            sif::debug << "SolarArrayDeploymentHandler::handleStateMachine: Failed to pull solar"
+                    " array deployment switch 2 low " << std::endl;
+        }
+        mainLineSwitcher->sendSwitchCommand(mainLineSwitch, PowerSwitchIF::SWITCH_OFF);
+        mainSwitchCountdown.setTimeout(mainLineSwitcher->getSwitchDelayMs());
+        stateMachine = WAIT_FOR_MAIN_SWITCH_OFF;
+    }
+}
+
+void SolarArrayDeploymentHandler::readCommandQueue() {
+    CommandMessage command;
+    ReturnValue_t result = commandQueue->receiveMessage(&command);
+    if (result != RETURN_OK) {
+        return;
+    }
+
+    result = actionHelper.handleActionMessage(&command);
+    if (result == RETURN_OK) {
+        return;
+    }
+}
+
+ReturnValue_t SolarArrayDeploymentHandler::executeAction(ActionId_t actionId,
+        MessageQueueId_t commandedBy, const uint8_t* data, size_t size) {
+    ReturnValue_t result;
+    if (stateMachine != WAIT_ON_DELOYMENT_COMMAND) {
+        sif::error << "SolarArrayDeploymentHandler::executeAction: Received command while not in"
+                << "waiting-on-command-state" << std::endl;
+        return DEPLOYMENT_ALREADY_EXECUTING;
+    }
+    if (actionId != DEPLOY_SOLAR_ARRAYS) {
+        sif::error << "SolarArrayDeploymentHandler::executeAction: Received invalid command"
+                << std::endl;
+        result = COMMAND_NOT_SUPPORTED;
+    } else {
+        stateMachine = SWITCH_8V_ON;
+        rememberCommanderId = commandedBy;
+        result = RETURN_OK;
+    }
+    return result;
+}
+
+MessageQueueId_t SolarArrayDeploymentHandler::getCommandQueue() const {
+    return commandQueue->getId();
+}

bsp_q7s/devices/SolarArrayDeploymentHandler.h

@@ -0,0 +1,160 @@
+#ifndef MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_
+#define MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_
+
+#include <linux/gpio/GpioIF.h>
+
+#include <fsfw/objectmanager/SystemObject.h>
+#include <fsfw/tasks/ExecutableObjectIF.h>
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <fsfw/action/HasActionsIF.h>
+#include <fsfw/power/PowerSwitchIF.h>
+#include <fsfw/devicehandlers/CookieIF.h>
+#include <fsfw/devicehandlers/DeviceHandlerIF.h>
+#include <fsfw/timemanager/Countdown.h>
+
+#include <unordered_map>
+
+/**
+ * @brief	This class is used to control the solar array deployment.
+ *
+ * @author	J. Meier
+ */
+class SolarArrayDeploymentHandler: public ExecutableObjectIF,
+        public SystemObject,
+        public HasReturnvaluesIF,
+        public HasActionsIF {
+public:
+
+	static const DeviceCommandId_t DEPLOY_SOLAR_ARRAYS = 0x5;
+
+	/**
+	 * @brief constructor
+	 *
+	 * @param setObjectId   The object id of the SolarArrayDeploymentHandler.
+	 * @param gpioDriverId  The id of the gpio com if.
+	 * @param gpioCookie    GpioCookie holding information about the gpios used to switch the
+	 *                      transistors.
+	 * @param mainLineSwitcherObjectId  The object id of the object responsible for switching
+	 *                                  the 8V power source. This is normally the PCDU.
+	 * @param mainLineSwitch    The id of the main line switch. This is defined in
+	 *                          powerSwitcherList.h.
+	 * @param deplSA1   gpioId of the GPIO controlling the deployment 1 transistor.
+	 * @param deplSA2   gpioId of the GPIO controlling the deployment 2 transistor.
+	 * @param burnTimeMs  Time duration the power will be applied to the burn wires.
+	 */
+    SolarArrayDeploymentHandler(object_id_t setObjectId, object_id_t gpioDriverId,
+            CookieIF * gpioCookie, object_id_t mainLineSwitcherObjectId, uint8_t mainLineSwitch,
+            gpioId_t deplSA1, gpioId_t deplSA2, uint32_t burnTimeMs);
+
+    virtual ~SolarArrayDeploymentHandler();
+
+	virtual ReturnValue_t performOperation(uint8_t operationCode = 0) override;
+
+	virtual MessageQueueId_t getCommandQueue() const override;
+    virtual ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy,
+            const uint8_t* data, size_t size) override;
+	virtual ReturnValue_t initialize() override;
+
+private:
+
+	static const uint8_t INTERFACE_ID = CLASS_ID::SA_DEPL_HANDLER;
+	static const ReturnValue_t COMMAND_NOT_SUPPORTED = MAKE_RETURN_CODE(0xA0);
+	static const ReturnValue_t DEPLOYMENT_ALREADY_EXECUTING = MAKE_RETURN_CODE(0xA1);
+	static const ReturnValue_t MAIN_SWITCH_TIMEOUT_FAILURE = MAKE_RETURN_CODE(0xA2);
+	static const ReturnValue_t SWITCHING_DEPL_SA1_FAILED = MAKE_RETURN_CODE(0xA3);
+	static const ReturnValue_t SWITCHING_DEPL_SA2_FAILED = MAKE_RETURN_CODE(0xA4);
+
+	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SA_DEPL_HANDLER;
+	static const Event MAIN_SWITCH_ON_TIMEOUT = MAKE_EVENT(0, severity::LOW);
+	static const Event MAIN_SWITCH_OFF_TIMEOUT = MAKE_EVENT(1, severity::LOW);
+	static const Event DEPLOYMENT_FAILED = MAKE_EVENT(2, severity::HIGH);
+	static const Event DEPL_SA1_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(3, severity::HIGH);
+	static const Event DEPL_SA2_GPIO_SWTICH_ON_FAILED = MAKE_EVENT(4, severity::HIGH);
+
+
+	enum StateMachine {
+	    WAIT_ON_DELOYMENT_COMMAND,
+	    SWITCH_8V_ON,
+	    WAIT_ON_8V_SWITCH,
+	    SWITCH_DEPL_GPIOS,
+	    WAIT_ON_DEPLOYMENT_FINISH,
+	    WAIT_FOR_MAIN_SWITCH_OFF
+	};
+
+	StateMachine stateMachine = WAIT_ON_DELOYMENT_COMMAND;
+
+	/**
+	 * This countdown is used to check if the PCDU sets the 8V line on in the intended time.
+	 */
+	Countdown mainSwitchCountdown;
+
+	/**
+	 * This countdown is used to wait for the burn wire being successful cut.
+	 */
+	Countdown deploymentCountdown;
+
+
+	/**
+	 * The message queue id of the component commanding an action will be stored in this variable.
+	 * This is necessary to send later the action finish replies.
+	 */
+	MessageQueueId_t rememberCommanderId = 0;
+
+    /** Size of command queue */
+    size_t cmdQueueSize = 20;
+
+    /** The object ID of the GPIO driver which switches the deployment transistors */
+    object_id_t gpioDriverId;
+
+    CookieIF * gpioCookie;
+
+    /** Object id of the object responsible to switch the 8V power input. Typically the PCDU. */
+    object_id_t mainLineSwitcherObjectId;
+
+    /** Switch number of the 8V power switch */
+    uint8_t mainLineSwitch;
+
+    gpioId_t deplSA1;
+    gpioId_t deplSA2;
+
+    GpioIF* gpioInterface = nullptr;
+
+    /** Time duration switches are active to cut the burn wire */
+    uint32_t burnTimeMs;
+
+    /** Queue to receive messages from other objects. */
+    MessageQueueIF* commandQueue = nullptr;
+
+    /**
+     * After initialization this pointer will hold the reference to the main line switcher object.
+     */
+    PowerSwitchIF *mainLineSwitcher = nullptr;
+
+    ActionHelper actionHelper;
+
+	void readCommandQueue();
+
+	/**
+	 * @brief   This function performs actions dependent on the current state.
+	 */
+	void handleStateMachine();
+
+	/**
+	 * @brief   This function polls the 8V switch state and changes the state machine when the
+	 *          switch has been enabled.
+	 */
+	void performWaitOn8VActions();
+
+	/**
+	 * @brief   This functions handles the switching of the solar array deployment transistors.
+	 */
+	void switchDeploymentTransistors();
+
+	/**
+	 * @brief   This function performs actions to finish the deployment. Essentially switches
+	 *          are turned of after the burn time has expired.
+	 */
+	void handleDeploymentFinish();
+};
+
+#endif /* MISSION_DEVICES_SOLARARRAYDEPLOYMENT_H_ */

bsp_q7s/main.cpp

@@ -13,15 +13,15 @@
 int main(void)
 {
     std::cout << "-- EIVE OBSW --" << std::endl;
-    std::cout << "-- Compiled for Linux " << " --" << std::endl;
+    std::cout << "-- Compiled for Linux (Xiphos Q7S) --" << std::endl;
     std::cout << "-- Software version " << SW_NAME << " v" << SW_VERSION << "."
     		<< SW_SUBVERSION << "." << SW_SUBSUBVERSION << " -- " << std::endl;
     std::cout << "-- " <<  __DATE__ << " " << __TIME__ << " --" << std::endl;
 
-	InitMission::initMission();
+	initmission::initMission();
 
 	for(;;) {
-		// suspend main thread by sleeping it.
+		/* Suspend main thread by sleeping it. */
 		TaskFactory::delayTask(5000);
 	}
 }

bsp_rpi/CMakeLists.txt

@@ -6,11 +6,8 @@ target_sources(${TARGET_NAME} PUBLIC
 
 add_subdirectory(boardconfig)
 add_subdirectory(boardtest)
-
-# wiringPi is deprecated unfortunately..
-#target_link_libraries(${TARGET_NAME} PRIVATE
-#    wiringPi
-#)
+add_subdirectory(gpio)
+
 
 
 

bsp_rpi/InitMission.cpp

@@ -1,16 +1,19 @@
 #include "InitMission.h"
 #include "ObjectFactory.h"
 
+#include <fsfwconfig/objects/systemObjectList.h>
+#include <fsfwconfig/OBSWConfig.h>
+#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
+
+#include <mission/utility/InitMission.h>
+
 #include <fsfw/objectmanager/ObjectManagerIF.h>
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
-#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
 #include <fsfw/objectmanager/ObjectManager.h>
 #include <fsfw/tasks/FixedTimeslotTaskIF.h>
 #include <fsfw/tasks/PeriodicTaskIF.h>
 #include <fsfw/tasks/TaskFactory.h>
-#include <fsfwconfig/objects/systemObjectList.h>
-#include <fsfwconfig/OBSWConfig.h>
-#include <fsfwconfig/pollingsequence/PollingSequenceFactory.h>
 
 #include <iostream>
 
@@ -21,9 +24,9 @@ ServiceInterfaceStream sif::error("ERROR");
 
 ObjectManagerIF *objectManager = nullptr;
 
-void InitMission::initMission() {
-	sif::info << "Building global objects.." << std::endl;
-	/* Instantiate global object manager and also create all objects */
+void initmission::initMission() {
+    sif::info << "Building global objects.." << std::endl;
+    /* Instantiate global object manager and also create all objects */
     objectManager = new ObjectManager(ObjectFactory::produce);
     sif::info << "Initializing all objects.." << std::endl;
     objectManager->initialize();
@@ -32,129 +35,144 @@ void InitMission::initMission() {
     initTasks();
 }
 
-void InitMission::initTasks(){
-	/* TMTC Distribution */
-	PeriodicTaskIF* TmTcDistributor = TaskFactory::instance()->
-			createPeriodicTask("DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.100, nullptr);
-	ReturnValue_t result = TmTcDistributor->addComponent(
-			objects::CCSDS_PACKET_DISTRIBUTOR);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-	result = TmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-    result = TmTcDistributor->addComponent(objects::TM_FUNNEL);
+void initmission::initTasks() {
+    TaskFactory* factory = TaskFactory::instance();
+    if(factory == nullptr) {
+        /* Should never happen ! */
+        return;
+    }
+#if OBSW_PRINT_MISSED_DEADLINES == 1
+    void (*missedDeadlineFunc) (void) = TaskFactory::printMissedDeadline;
+#else
+    void (*missedDeadlineFunc) (void) = nullptr;
+#endif
+
+    /* TMTC Distribution */
+    PeriodicTaskIF* tmTcDistributor = factory->createPeriodicTask(
+            "DIST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
+    ReturnValue_t result = tmTcDistributor->addComponent(
+            objects::CCSDS_PACKET_DISTRIBUTOR);
+    if(result != HasReturnvaluesIF::RETURN_OK){
+        sif::error << "Object add component failed" << std::endl;
+    }
+    result = tmTcDistributor->addComponent(objects::PUS_PACKET_DISTRIBUTOR);
+    if(result != HasReturnvaluesIF::RETURN_OK){
+        sif::error << "Object add component failed" << std::endl;
+    }
+    result = tmTcDistributor->addComponent(objects::TM_FUNNEL);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Object add component failed" << std::endl;
+        sif::error << "Object add component failed" << std::endl;
     }
 
     /* UDP bridge */
-    PeriodicTaskIF* UdpBridgeTask = TaskFactory::instance()->createPeriodicTask(
-    		"UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-			0.2, nullptr);
-    result = UdpBridgeTask->addComponent(objects::UDP_BRIDGE);
+    PeriodicTaskIF* udpBridgeTask = factory->createPeriodicTask(
+            "UDP_UNIX_BRIDGE", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.2, missedDeadlineFunc);
+    result = udpBridgeTask->addComponent(objects::UDP_BRIDGE);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Add component UDP Unix Bridge failed" << std::endl;
+        sif::error << "Add component UDP Unix Bridge failed" << std::endl;
     }
-    PeriodicTaskIF* UdpPollingTask = TaskFactory::instance()->
-    		createPeriodicTask("UDP_POLLING", 80,
-    		PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, nullptr);
-    result = UdpPollingTask->addComponent(objects::UDP_POLLING_TASK);
+    PeriodicTaskIF* udpPollingTask = factory->createPeriodicTask(
+            "UDP_POLLING", 80, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
+    result = udpPollingTask->addComponent(objects::UDP_POLLING_TASK);
     if(result != HasReturnvaluesIF::RETURN_OK) {
-    	sif::error << "Add component UDP Polling failed" << std::endl;
+        sif::error << "Add component UDP Polling failed" << std::endl;
     }
 
-	/* PUS Services */
-	PeriodicTaskIF* PusVerification = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-
-	PeriodicTaskIF* PusEvents = TaskFactory::instance()->
-			createPeriodicTask("PUS_VERIF_1", 60,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, nullptr);
-	result = PusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-
-	PeriodicTaskIF* PusHighPrio = TaskFactory::instance()->
-			createPeriodicTask("PUS_HIGH_PRIO", 50,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.200, nullptr);
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-	result = PusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-
-	PeriodicTaskIF* PusMedPrio = TaskFactory::instance()->
-			createPeriodicTask("PUS_HIGH_PRIO", 40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					0.8, nullptr);
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-	result = PusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-
-	PeriodicTaskIF* PusLowPrio = TaskFactory::instance()->
-			createPeriodicTask("PUSB", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-					1.6, nullptr);
-	result = PusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
-	if(result != HasReturnvaluesIF::RETURN_OK){
-		sif::error << "Object add component failed" << std::endl;
-	}
-
-
-#if OBSW_ADD_TEST_CODE == 1
-	FixedTimeslotTaskIF* TestTimeslotTask = TaskFactory::instance()->
-			createFixedTimeslotTask("PST_TEST_TASK", 10,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.0, nullptr);
-	result = pst::pollingSequenceTestFunction(TestTimeslotTask);
-	if(result != HasReturnvaluesIF::RETURN_OK) {
-		sif::error << "InitMission::createTasks: Test PST initialization "
-				<< "failed!" << std::endl;
-	}
-
-#endif
-
-	PeriodicTaskIF* SpiTestTask = TaskFactory::instance()->
-            createPeriodicTask("SPI_TEST", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE,
-                    2.0, nullptr);
-    result = SpiTestTask->addComponent(objects::SPI_TEST);
+    /* PUS Services */
+    PeriodicTaskIF* pusVerification = factory->createPeriodicTask(
+            "PUS_VERIF", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
+    result = pusVerification->addComponent(objects::PUS_SERVICE_1_VERIFICATION);
     if(result != HasReturnvaluesIF::RETURN_OK){
-        sif::error << "Object add SPI test failed" << std::endl;
+        sif::error << "Object add component failed" << std::endl;
     }
 
-	//Main thread sleep
-	sif::info << "Starting tasks.." << std::endl;
-	TmTcDistributor->startTask();
-	UdpBridgeTask->startTask();
-	UdpPollingTask->startTask();
+    PeriodicTaskIF* pusEvents = factory->createPeriodicTask(
+            "PUS_EVENTS", 60, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
+    result = pusVerification->addComponent(objects::PUS_SERVICE_5_EVENT_REPORTING);
+    if(result != HasReturnvaluesIF::RETURN_OK){
+        initmission::printAddObjectError("PUS5", objects::PUS_SERVICE_5_EVENT_REPORTING);
+    }
 
-	PusVerification->startTask();
-	PusEvents->startTask();
-	PusHighPrio->startTask();
-	PusMedPrio->startTask();
-	PusLowPrio->startTask();
+    PeriodicTaskIF* pusHighPrio = factory->createPeriodicTask(
+            "PUS_HIGH_PRIO", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.200, missedDeadlineFunc);
+    result = pusHighPrio->addComponent(objects::PUS_SERVICE_2_DEVICE_ACCESS);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS2", objects::PUS_SERVICE_2_DEVICE_ACCESS);
+    }
+    result = pusHighPrio->addComponent(objects::PUS_SERVICE_9_TIME_MGMT);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS9", objects::PUS_SERVICE_9_TIME_MGMT);
+    }
 
-	SpiTestTask->startTask();
+    PeriodicTaskIF* pusMedPrio = factory->createPeriodicTask(
+            "PUS_MED_PRIO", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, missedDeadlineFunc);
+    result = pusMedPrio->addComponent(objects::PUS_SERVICE_8_FUNCTION_MGMT);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS8", objects::PUS_SERVICE_8_FUNCTION_MGMT);
+    }
+    result = pusMedPrio->addComponent(objects::PUS_SERVICE_200_MODE_MGMT);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS200", objects::PUS_SERVICE_200_MODE_MGMT);
+    }
+    result = pusMedPrio->addComponent(objects::PUS_SERVICE_20_PARAMETERS);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS20", objects::PUS_SERVICE_20_PARAMETERS);
+    }
+
+    PeriodicTaskIF* pusLowPrio = factory->createPeriodicTask(
+            "PUS_LOW_PRIO", 30, PeriodicTaskIF::MINIMUM_STACK_SIZE, 1.6, missedDeadlineFunc);
+    result = pusLowPrio->addComponent(objects::PUS_SERVICE_17_TEST);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("PUS17", objects::PUS_SERVICE_17_TEST);
+    }
+
+#if RPI_TEST_ACS_BOARD == 1
+    FixedTimeslotTaskIF* acsTask = factory->createFixedTimeslotTask(
+            "ACS_PST", 50, PeriodicTaskIF::MINIMUM_STACK_SIZE * 2, 2.0, missedDeadlineFunc);
+    result = pst::pollingSequenceAcsTest(acsTask);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        sif::warning << "initmission::initTasks: ACS PST initialization failed!" << std::endl;
+    }
+#endif /* RPI_TEST_ACS_BOARD == 1 */
+
+    PeriodicTaskIF* testTask = factory->createPeriodicTask(
+            "TEST_TASK", 40, PeriodicTaskIF::MINIMUM_STACK_SIZE, 2.0, missedDeadlineFunc);
+#if OBSW_ADD_TEST_CODE == 1
+    result = testTask->addComponent(objects::TEST_TASK);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("TEST_TASK", objects::TEST_TASK);
+    }
+#endif /* OBSW_ADD_TEST_CODE == 1 */
+#if RPI_ADD_SPI_TEST == 1
+    result = testTask->addComponent(objects::SPI_TEST);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("SPI_TEST", objects::SPI_TEST);
+    }
+#endif /* RPI_ADD_SPI_TEST == 1 */
+#if RPI_ADD_GPIO_TEST == 1
+    result = testTask->addComponent(objects::LIBGPIOD_TEST);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        initmission::printAddObjectError("GPIOD_TEST", objects::LIBGPIOD_TEST);
+    }
+#endif /* RPI_ADD_GPIO_TEST == 1 */
+
+    sif::info << "Starting tasks.." << std::endl;
+    tmTcDistributor->startTask();
+    udpBridgeTask->startTask();
+    udpPollingTask->startTask();
+
+    pusVerification->startTask();
+    pusEvents->startTask();
+    pusHighPrio->startTask();
+    pusMedPrio->startTask();
+    pusLowPrio->startTask();
 
 #if OBSW_ADD_TEST_CODE == 1
-	TestTimeslotTask->startTask();
-#endif
-	sif::info << "Tasks started.." << std::endl;
+    testTask->startTask();
+#endif /* OBSW_ADD_TEST_CODE == 1 */
+
+#if RPI_TEST_ACS_BOARD == 1
+    acsTask->startTask();
+#endif /* RPI_TEST_ACS_BOARD == 1 */
+    sif::info << "Tasks started.." << std::endl;
 }

bsp_rpi/InitMission.h

@@ -1,7 +1,7 @@
 #ifndef BSP_LINUX_INITMISSION_H_
 #define BSP_LINUX_INITMISSION_H_
 
-namespace InitMission {
+namespace initmission {
 void initMission();
 void initTasks();
 };

bsp_rpi/ObjectFactory.cpp

@@ -1,48 +1,107 @@
 #include "ObjectFactory.h"
-#include <bsp_rpi/boardtest/SpiTest.h>
-#include <fsfw/datapoollocal/LocalDataPoolManager.h>
+#include <bsp_rpi/gpio/GPIORPi.h>
 
 #include <objects/systemObjectList.h>
+#include <devices/addresses.h>
+#include <devices/gpioIds.h>
 #include <OBSWConfig.h>
 #include <tmtc/apid.h>
 #include <tmtc/pusIds.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 <mission/core/GenericFactory.h>
+#include <mission/utility/TmFunnel.h>
+#include <mission/devices/MGMHandlerLIS3MDL.h>
+
+#include <fsfw/datapoollocal/LocalDataPoolManager.h>
 #include <fsfw/tmtcservices/CommandingServiceBase.h>
 #include <fsfw/tmtcservices/PusServiceBase.h>
 #include <fsfw/osal/linux/TmTcUnixUdpBridge.h>
 #include <fsfw/tmtcpacket/pus/TmPacketStored.h>
 #include <fsfw/osal/linux/TcUnixUdpPollingTask.h>
-
-#include <mission/core/GenericFactory.h>
-#include <mission/utility/TmFunnel.h>
+#include <fsfw/tasks/TaskFactory.h>
+#include <linux/spi/SpiComIF.h>
 
 void Factory::setStaticFrameworkObjectIds() {
-	PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR;
-	PusServiceBase::packetDestination = objects::TM_FUNNEL;
+    PusServiceBase::packetSource = objects::PUS_PACKET_DISTRIBUTOR;
+    PusServiceBase::packetDestination = objects::TM_FUNNEL;
 
-	CommandingServiceBase::defaultPacketSource = objects::PUS_PACKET_DISTRIBUTOR;
-	CommandingServiceBase::defaultPacketDestination = objects::TM_FUNNEL;
+    CommandingServiceBase::defaultPacketSource = objects::PUS_PACKET_DISTRIBUTOR;
+    CommandingServiceBase::defaultPacketDestination = objects::TM_FUNNEL;
 
-	TmFunnel::downlinkDestination = objects::UDP_BRIDGE;
-	// No storage object for now.
-	TmFunnel::storageDestination = objects::NO_OBJECT;
+    TmFunnel::downlinkDestination = objects::UDP_BRIDGE;
+    // No storage object for now.
+    TmFunnel::storageDestination = objects::NO_OBJECT;
 
-	LocalDataPoolManager::defaultHkDestination = objects::NO_OBJECT;
+    LocalDataPoolManager::defaultHkDestination = objects::NO_OBJECT;
 
-	VerificationReporter::messageReceiver = objects::PUS_SERVICE_1_VERIFICATION;
-	TmPacketStored::timeStamperId = objects::TIME_STAMPER;
+    VerificationReporter::messageReceiver = objects::PUS_SERVICE_1_VERIFICATION;
+    TmPacketStored::timeStamperId = objects::TIME_STAMPER;
 }
 
 
 
 void ObjectFactory::produce(){
-	Factory::setStaticFrameworkObjectIds();
-	ObjectFactory::produceGenericObjects();
+    Factory::setStaticFrameworkObjectIds();
+    ObjectFactory::produceGenericObjects();
 
-	new TmTcUnixUdpBridge(objects::UDP_BRIDGE,
-			objects::CCSDS_PACKET_DISTRIBUTOR,
-			objects::TM_STORE, objects::TC_STORE);
-	new TcUnixUdpPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE);
+    new TmTcUnixUdpBridge(objects::UDP_BRIDGE,
+            objects::CCSDS_PACKET_DISTRIBUTOR,
+            objects::TM_STORE, objects::TC_STORE);
+    new TcUnixUdpPollingTask(objects::UDP_POLLING_TASK, objects::UDP_BRIDGE);
 
-	new SpiTest(objects::SPI_TEST);
+    GpioIF* gpioIF = new LinuxLibgpioIF(objects::GPIO_IF);
+
+#if RPI_ADD_SPI_TEST == 1
+    new SpiTestClass(objects::SPI_TEST, gpioIF);
+#endif
+
+#if RPI_LOOPBACK_TEST_GPIO == 1
+    GpioCookie* gpioCookieLoopback = new GpioCookie();
+    /* Loopback pins. Adapt according to setup */
+    gpioId_t gpioIdSender = gpioIds::TEST_ID_0;
+    int bcmPinSender = 26;
+    gpioId_t gpioIdReader = gpioIds::TEST_ID_1;
+    int bcmPinReader = 16;
+    gpio::createRpiGpioConfig(gpioCookieLoopback, gpioIdSender, bcmPinSender, "GPIO_LB_SENDER",
+            gpio::Direction::OUT, 0);
+    gpio::createRpiGpioConfig(gpioCookieLoopback, gpioIdReader, bcmPinReader, "GPIO_LB_READER",
+            gpio::Direction::IN, 0);
+    new LibgpiodTest(objects::LIBGPIOD_TEST, objects::GPIO_IF, gpioCookieLoopback);
+#endif /* RPI_LOOPBACK_TEST_GPIO == 1 */
+
+    new SpiComIF(objects::SPI_COM_IF, gpioIF);
+
+#if RPI_TEST_ACS_BOARD == 1
+
+    GpioCookie* gpioCookieAcsBoard = new GpioCookie();
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_0_LIS3_CS, gpio::MGM_0_BCM_PIN,
+            "MGM_0_LIS3", gpio::Direction::OUT, 1);
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_1_RM3100_CS, gpio::MGM_1_BCM_PIN,
+            "MGM_1_RM3100", gpio::Direction::OUT, 1);
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_2_LIS3_CS, gpio::MGM_2_BCM_PIN,
+            "MGM_2_LIS3", gpio::Direction::OUT, 1);
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::MGM_3_RM3100_CS, gpio::MGM_3_BCM_PIN,
+            "MGM_3_RM3100", gpio::Direction::OUT, 1);
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_0_ADIS_CS, gpio::GYRO_0_BCM_PIN,
+            "GYRO_0_ADIS", gpio::Direction::OUT, 1);
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_1_L3G_CS, gpio::GYRO_1_BCM_PIN,
+            "GYRO_1_L3G", gpio::Direction::OUT, 1);
+    gpio::createRpiGpioConfig(gpioCookieAcsBoard, gpioIds::GYRO_2_L3G_CS, gpio::GYRO_2_BCM_PIN,
+            "GYRO_2_L3G", gpio::Direction::OUT, 1);
+    gpioIF->addGpios(gpioCookieAcsBoard);
+
+    SpiCookie* spiCookie = new SpiCookie(addresses::MGM_0_LIS3,
+            gpioIds::MGM_0_LIS3_CS, "/dev/spidev0.0", 24, spi::SpiMode::MODE_3, 3'900'000);
+    auto mgmHandler = new MGMHandlerLIS3MDL(objects::MGM_0_LIS3_HANDLER,
+            objects::SPI_COM_IF, spiCookie);
+    mgmHandler->setStartUpImmediately();
+
+#endif /* RPI_TEST_ACS_BOARD == 1 */
 }

bsp_rpi/boardconfig/rpi_config.h

@@ -0,0 +1,24 @@
+#ifndef BSP_RPI_BOARDCONFIG_RPI_CONFIG_H_
+#define BSP_RPI_BOARDCONFIG_RPI_CONFIG_H_
+
+#include <cstdint>
+
+#define RPI_ADD_GPIO_TEST               0
+#define RPI_LOOPBACK_TEST_GPIO          0
+
+/* Only one of those 2 should be enabled! */
+#define RPI_ADD_SPI_TEST                0
+#define RPI_TEST_ACS_BOARD              1
+
+/* Adapt these values accordingly */
+namespace gpio {
+static constexpr uint8_t MGM_0_BCM_PIN = 0;
+static constexpr uint8_t MGM_1_BCM_PIN = 1;
+static constexpr uint8_t MGM_2_BCM_PIN = 17;
+static constexpr uint8_t MGM_3_BCM_PIN = 27;
+static constexpr uint8_t GYRO_0_BCM_PIN = 5;
+static constexpr uint8_t GYRO_1_BCM_PIN = 6;
+static constexpr uint8_t GYRO_2_BCM_PIN = 4;
+}
+
+#endif /* BSP_RPI_BOARDCONFIG_RPI_CONFIG_H_ */

bsp_rpi/boardtest/CMakeLists.txt

@@ -1,6 +1,4 @@
 target_sources(${TARGET_NAME} PRIVATE
-	SpiTest.cpp
-	RPiGPIO.cpp
 )
 
 

bsp_rpi/gpio/CMakeLists.txt

@@ -0,0 +1,9 @@
+target_sources(${TARGET_NAME} PUBLIC
+	GPIORPi.cpp
+)
+
+
+
+
+
+

bsp_rpi/gpio/GPIORPi.cpp

@@ -0,0 +1,36 @@
+#include "GPIORPi.h"
+#include <FSFWConfig.h>
+
+#include <fsfw/serviceinterface/ServiceInterface.h>
+#include <linux/gpio/GpioCookie.h>
+
+ReturnValue_t gpio::createRpiGpioConfig(GpioCookie* cookie, gpioId_t gpioId, int bcmPin,
+        std::string consumer, gpio::Direction direction, int initValue) {
+    if(cookie == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    GpiodRegular config;
+    /* Default chipname for Raspberry Pi. There is still gpiochip1 for expansion, but most users
+    will not need this */
+    config.chipname = "gpiochip0";
+
+    config.consumer = consumer;
+    config.direction = direction;
+    config.initValue = initValue;
+
+    /* Sanity check for the BCM pins before assigning it */
+    if(bcmPin > 27) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "createRpiGpioConfig: BCM pin " << bcmPin << " invalid!" << std::endl;
+#else
+        sif::printError("createRpiGpioConfig: BCM pin %d invalid!\n", bcmPin);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    config.lineNum = bcmPin;
+    cookie->addGpio(gpioId, config);
+    return HasReturnvaluesIF::RETURN_OK;
+}

bsp_rpi/gpio/GPIORPi.h

@@ -0,0 +1,26 @@
+#ifndef BSP_RPI_GPIO_GPIORPI_H_
+#define BSP_RPI_GPIO_GPIORPI_H_
+
+#include <fsfw/returnvalues/HasReturnvaluesIF.h>
+#include <linux/gpio/gpioDefinitions.h>
+
+class GpioCookie;
+
+namespace gpio {
+
+/**
+ * Create a GpioConfig_t. This function does a sanity check on the BCM pin number and fails if the
+ * BCM pin is invalid.
+ * @param cookie    Adds the configuration to this cookie directly
+ * @param gpioId    ID which identifies the GPIO configuration
+ * @param bcmPin    Raspberry Pi BCM pin
+ * @param consumer  Information string
+ * @param direction GPIO direction
+ * @param initValue Intial value for output pins, 0 for low, 1 for high
+ * @return
+ */
+ReturnValue_t createRpiGpioConfig(GpioCookie* cookie, gpioId_t gpioId, int bcmPin,
+        std::string consumer, gpio::Direction direction, int initValue);
+}
+
+#endif /* BSP_RPI_GPIO_GPIORPI_H_ */

bsp_rpi/main.cpp

@@ -1,28 +1,26 @@
 #include "InitMission.h"
-
 #include <OBSWVersion.h>
+
 #include <fsfw/tasks/TaskFactory.h>
 
 #include <iostream>
 
-#include <unistd.h>
-
 /**
- * @brief 	This is the main program for the target hardware.
+ * @brief 	    This is the main program and entry point for the Raspberry Pi.
  * @return
  */
 int main(void)
 {
     std::cout << "-- EIVE OBSW --" << std::endl;
-    std::cout << "-- Compiled for Linux " << " --" << std::endl;
+    std::cout << "-- Compiled for Linux (Raspberry Pi) --" << std::endl;
     std::cout << "-- Software version " << SW_NAME << " v" << SW_VERSION << "."
     		<< SW_SUBVERSION << "." << SW_SUBSUBVERSION << " -- " << std::endl;
     std::cout << "-- " <<  __DATE__ << " " << __TIME__ << " --" << std::endl;
 
-	InitMission::initMission();
+	initmission::initMission();
 
 	for(;;) {
-		// suspend main thread by sleeping it.
+		/* Suspend main thread by sleeping it. */
 		TaskFactory::delayTask(5000);
 	}
 }

cmake/scripts/Host/create_cmake_debug_cfg.sh

@@ -15,6 +15,7 @@ if [ "${counter}" -ge 5 ];then
 fi
 
 build_generator=""
+build_dir="Debug-Host"
 os_fsfw="host"
 if [ "${OS}" = "Windows_NT" ]; then
 	build_generator="MinGW Makefiles"
@@ -23,4 +24,4 @@ else
 	build_generator="Unix Makefiles"
 fi
 
-python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "debug"
+python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "debug" -l"${build_dir}"

cmake/scripts/Host/create_cmake_release_cfg.sh

@@ -16,6 +16,7 @@ fi
 
 build_generator=""
 os_fsfw="host"
+build_dir="Debug-Release"
 if [ "${OS}" = "Windows_NT" ]; then
 	build_generator="MinGW Makefiles"
 # Could be other OS but this works for now.
@@ -23,4 +24,4 @@ else
 	build_generator="Unix Makefiles"
 fi
 
-python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "release"
+python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "release" -l"${build_dir}"

cmake/scripts/RPi/create_cmake_debug_cfg.sh

@@ -17,6 +17,7 @@ fi
 os_fsfw="linux"
 tgt_bsp="arm/raspberrypi"
 build_generator=""
+build_dir="Debug-RPi"
 if [ "${OS}" = "Windows_NT" ]; then
 	build_generator="MinGW Makefiles"
 # Could be other OS but this works for now.
@@ -24,4 +25,5 @@ else
 	build_generator="Unix Makefiles"
 fi
 
-python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "debug" -t "${tgt_bsp}"
+python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "debug" -t "${tgt_bsp}" \
+		-l"${build_dir}"

cmake/scripts/RPi/create_cmake_release_cfg.sh

@@ -17,6 +17,7 @@ fi
 os_fsfw="linux"
 tgt_bsp="arm/raspberrypi"
 build_generator=""
+build_dir="Release-RPi"
 if [ "${OS}" = "Windows_NT" ]; then
 	build_generator="MinGW Makefiles"
 # Could be other OS but this works for now.
@@ -24,4 +25,5 @@ else
 	build_generator="Unix Makefiles"
 fi
 
-python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "release" -t "${tgt_bsp}"
+python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "release" -t "${tgt_bsp}"  \
+		-l"${build_dir}"

cmake/scripts/RPi/create_cmake_relwithdeb_cfg.sh

@@ -17,6 +17,7 @@ fi
 os_fsfw="linux"
 tgt_bsp="arm/raspberrypi"
 build_generator=""
+build_dir="RelWithDeb-RPi"
 if [ "${OS}" = "Windows_NT" ]; then
 	build_generator="MinGW Makefiles"
 # Could be other OS but this works for now.
@@ -24,4 +25,5 @@ else
 	build_generator="Unix Makefiles"
 fi
 
-python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "reldeb" -t "${tgt_bsp}"
+python3 cmake_build_config.py -o "${os_fsfw}" -g "${build_generator}" -b "reldeb" -t "${tgt_bsp}" \
+		-l"${build_dir}"

cmake/scripts/cmake_build_config.py

@@ -61,9 +61,10 @@ def main():
     else:
         cmake_target_cfg_cmd = ""
 
-    # TODO: Use builddir if given (need to check whether path is relative or absolute)
     build_folder = cmake_build_type
-
+    if args.builddir is not None:
+        build_folder = args.builddir
+    
     build_path = source_location + os.path.sep + build_folder
     if os.path.isdir(build_path):
         remove_old_dir = input(f"{build_folder} folder already exists. Remove old directory? [y/n]: ")

common/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(config)

common/config/CMakeLists.txt

@@ -0,0 +1,3 @@
+target_include_directories(${TARGET_NAME} PRIVATE
+    ${CMAKE_CURRENT_SOURCE_DIR}
+)

common/config/commonConfig.h

@@ -0,0 +1,7 @@
+#ifndef COMMON_CONFIG_COMMONCONFIG_H_
+#define COMMON_CONFIG_COMMONCONFIG_H_
+
+#define OBSW_ADD_LWGPS_TEST     0
+
+
+#endif /* COMMON_CONFIG_COMMONCONFIG_H_ */

common/config/lwgps_opts.h

@@ -0,0 +1,44 @@
+/**
+ * \file            lwgps_opts_template.h
+ * \brief           LwGPS configuration file
+ */
+
+/*
+ * Copyright (c) 2020 Tilen MAJERLE
+ *
+ * Permission is hereby granted, free of charge, to any person
+ * obtaining a copy of this software and associated documentation
+ * files (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
+ * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE
+ * AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
+ * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+ * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * This file is part of LwGPS - Lightweight GPS NMEA parser library.
+ *
+ * Author:          Tilen MAJERLE <tilen@majerle.eu>
+ * Version:         v2.1.0
+ */
+#ifndef LWGPS_HDR_OPTS_H
+#define LWGPS_HDR_OPTS_H
+
+/* Rename this file to "lwgps_opts.h" for your application */
+
+/*
+ * Open "include/lwgps/lwgps_opt.h" and
+ * copy & replace here settings you want to change values
+ */
+
+#endif /* LWGPS_HDR_OPTS_H */

fsfwconfig/OBSWVersion.h

@@ -4,8 +4,8 @@
 //! TODO: Think of a cool name for the software releases.
 const char* const SW_NAME = "eive";
 
-#define SW_VERSION 0
-#define SW_SUBVERSION 2
+#define SW_VERSION 1
+#define SW_SUBVERSION 0
 #define SW_SUBSUBVERSION 0
 
 

fsfwconfig/devices/addresses.h

@@ -1,20 +1,24 @@
-/**
- * \file addresses.cpp
- *
- * \date 07.11.2019
- */
-
 #ifndef FSFWCONFIG_DEVICES_ADDRESSES_H_
 #define FSFWCONFIG_DEVICES_ADDRESSES_H_
-#include <stdint.h>
+
 #include <fsfw/devicehandlers/CookieIF.h>
 #include <fsfwconfig/objects/systemObjectList.h>
+#include <cstdint>
 
 namespace addresses {
 	/* Logical addresses have uint32_t datatype */
 	enum logicalAddresses: address_t {
 		PCDU,
 
+        MGM_0_LIS3 = objects::MGM_0_LIS3_HANDLER,
+		MGM_1_RM3100 = objects::MGM_1_RM3100_HANDLER,
+		MGM_2_LIS3 = objects::MGM_2_LIS3_HANDLER,
+		MGM_3_RM3100 = objects::MGM_3_RM3100_HANDLER,
+
+		GYRO_0_ADIS = objects::GYRO_0_ADIS_HANDLER,
+		GYRO_1_L3G = objects::GYRO_1_L3G_HANDLER,
+		GYRO_2_L3G = objects::GYRO_2_L3G_HANDLER,
+
 		/* Dummy and Test Addresses */
 		DUMMY_ECHO = 129,
 		DUMMY_GPS0  = 130,

fsfwconfig/devices/gpioIds.h

@@ -15,7 +15,17 @@ namespace gpioIds {
 		HEATER_7,
 		DEPLSA1,
 		DEPLSA2,
-		Test_ID
+
+		MGM_0_LIS3_CS,
+		MGM_1_RM3100_CS,
+		GYRO_0_ADIS_CS,
+		GYRO_1_L3G_CS,
+		GYRO_2_L3G_CS,
+		MGM_2_LIS3_CS,
+		MGM_3_RM3100_CS,
+
+		TEST_ID_0,
+		TEST_ID_1
 	};
 }
 

fsfwconfig/pollingsequence/PollingSequenceFactory.h

@@ -32,6 +32,8 @@ ReturnValue_t pollingSequenceInitDefault(FixedTimeslotTaskIF *thisSequence);
  * 			blocking calls when requesting data from devices.
  */
 ReturnValue_t gomspacePstInit(FixedTimeslotTaskIF *thisSequence);
+
+ReturnValue_t pollingSequenceAcsTest(FixedTimeslotTaskIF* thisSequence);
 }
 
 

linux/boardtest/CMakeLists.txt

@@ -0,0 +1,10 @@
+target_sources(${TARGET_NAME} PRIVATE
+	LibgpiodTest.cpp
+	I2cTestClass.cpp
+	SpiTestClass.cpp
+	UartTestClass.cpp
+)
+
+
+
+

linux/boardtest/I2cTestClass.cpp

@@ -0,0 +1,8 @@
+#include <linux/boardtest/I2cTestClass.h>
+
+I2cTestClass::I2cTestClass(object_id_t objectId): TestTask(objectId) {
+}
+
+ReturnValue_t I2cTestClass::performPeriodicAction() {
+    return HasReturnvaluesIF::RETURN_OK;
+}

linux/boardtest/I2cTestClass.h

@@ -0,0 +1,17 @@
+#ifndef LINUX_BOARDTEST_I2CTESTCLASS_H_
+#define LINUX_BOARDTEST_I2CTESTCLASS_H_
+
+#include <test/testtasks/TestTask.h>
+
+class I2cTestClass: public TestTask {
+public:
+    I2cTestClass(object_id_t objectId);
+
+    ReturnValue_t performPeriodicAction() override;
+private:
+
+};
+
+
+
+#endif /* LINUX_BOARDTEST_I2CTESTCLASS_H_ */

linux/boardtest/LibgpiodTest.cpp

@@ -0,0 +1,99 @@
+#include "LibgpiodTest.h"
+
+#include <fsfwconfig/devices/gpioIds.h>
+#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include <fsfw/objectmanager/ObjectManagerIF.h>
+#include <fsfw/tasks/TaskFactory.h>
+
+LibgpiodTest::LibgpiodTest(object_id_t objectId, object_id_t gpioIfobjectId,
+        GpioCookie* gpioCookie):
+        TestTask(objectId) {
+
+    gpioInterface = objectManager->get<GpioIF>(gpioIfobjectId);
+    if (gpioInterface == nullptr) {
+        sif::error << "LibgpiodTest::LibgpiodTest: Invalid Gpio interface." << std::endl;
+    }
+    gpioInterface->addGpios(gpioCookie);
+    testCase = TestCases::LOOPBACK;
+}
+
+LibgpiodTest::~LibgpiodTest() {
+}
+
+ReturnValue_t LibgpiodTest::performPeriodicAction() {
+    int gpioState;
+    ReturnValue_t result;
+
+    switch(testCase) {
+    case(TestCases::READ): {
+        result = gpioInterface->readGpio(gpioIds::TEST_ID_0, &gpioState);
+        if (result != RETURN_OK) {
+            sif::debug << "LibgpiodTest::performPeriodicAction: Failed to read gpio "
+                    << std::endl;
+            return RETURN_FAILED;
+        }
+        else {
+            sif::debug << "LibgpiodTest::performPeriodicAction: MIO 0 state = " << gpioState
+                    << std::endl;
+        }
+        break;
+    }
+    case(TestCases::LOOPBACK): {
+        break;
+    }
+    }
+
+
+    return RETURN_OK;
+}
+
+ReturnValue_t LibgpiodTest::performOneShotAction() {
+    int gpioState;
+    ReturnValue_t result;
+
+    switch(testCase) {
+    case(TestCases::READ): {
+        break;
+    }
+    case(TestCases::LOOPBACK): {
+        result = gpioInterface->pullHigh(gpioIds::TEST_ID_0);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            sif::info << "LibgpiodTest::performOneShotAction: "
+                    "GPIO pulled high successfully for loopback test" << std::endl;
+        }
+        else {
+            sif::warning << "LibgpiodTest::performOneShotAction: Could not pull GPIO high!"
+                    << std::endl;
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+        result = gpioInterface->readGpio(gpioIds::TEST_ID_1, &gpioState);
+        if(result == HasReturnvaluesIF::RETURN_OK and gpioState == 1) {
+            sif::info << "LibgpiodTest::performOneShotAction: "
+                    "GPIO state read successfully and is high" << std::endl;
+        }
+        else {
+            sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not high!"
+                    << std::endl;
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+
+        result = gpioInterface->pullLow(gpioIds::TEST_ID_0);
+        if(result == HasReturnvaluesIF::RETURN_OK) {
+            sif::info << "LibgpiodTest::performOneShotAction: "
+                    "GPIO pulled low successfully for loopback test" << std::endl;
+        }
+        result = gpioInterface->readGpio(gpioIds::TEST_ID_1, &gpioState);
+        if(result == HasReturnvaluesIF::RETURN_OK and gpioState == 0) {
+            sif::info << "LibgpiodTest::performOneShotAction: "
+                    "GPIO state read successfully and is low" << std::endl;
+        }
+        else {
+            sif::warning << "LibgpiodTest::performOneShotAction: GPIO read and is not low!"
+                    << std::endl;
+            return HasReturnvaluesIF::RETURN_OK;
+        }
+        break;
+    }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}

linux/boardtest/LibgpiodTest.h

@@ -12,11 +12,19 @@
  */
 class LibgpiodTest: public TestTask {
 public:
+    enum TestCases {
+        READ = 0,
+        LOOPBACK = 1
+    };
+
+    TestCases testCase;
+
     LibgpiodTest(object_id_t objectId, object_id_t gpioIfobjectId, GpioCookie* gpioCookie);
     virtual ~LibgpiodTest();
 
 protected:
-    virtual ReturnValue_t performPeriodicAction() override;
+    ReturnValue_t performOneShotAction() override;
+    ReturnValue_t performPeriodicAction() override;
 
 private:
     GpioIF* gpioInterface;

linux/boardtest/SpiTestClass.cpp

@@ -0,0 +1,231 @@
+#include "SpiTestClass.h"
+
+#include <fsfwconfig/devices/gpioIds.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+#include <linux/spi/spidev.h>
+#include <fcntl.h>
+#include <fsfw/timemanager/Stopwatch.h>
+#include <linux/gpio/gpioDefinitions.h>
+#include <linux/gpio/GpioCookie.h>
+#include <linux/utility/Utility.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <bitset>
+
+
+SpiTestClass::SpiTestClass(object_id_t objectId, GpioIF* gpioIF): TestTask(objectId),
+        gpioIF(gpioIF) {
+    if(gpioIF == nullptr) {
+        sif::error << "SpiTestClass::SpiTestClass: Invalid GPIO ComIF!" << std::endl;
+    }
+    testMode = TestModes::MGM_LIS3MDL;
+    spiTransferStruct.rx_buf = reinterpret_cast<__u64>(recvBuffer.data());
+    spiTransferStruct.tx_buf = reinterpret_cast<__u64>(sendBuffer.data());
+}
+
+ReturnValue_t SpiTestClass::performOneShotAction() {
+    switch(testMode) {
+    case(TestModes::NONE): {
+        break;
+    }
+    case(TestModes::MGM_LIS3MDL): {
+        performLis3MdlTest(mgm0Lis3ChipSelect);
+        break;
+    }
+    case(TestModes::MGM_RM3100): {
+        performRm3100Test(mgm1Rm3100ChipSelect);
+        break;
+    }
+    case(TestModes::GYRO_L3GD20H): {
+        break;
+    }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SpiTestClass::performPeriodicAction() {
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void SpiTestClass::performRm3100Test(uint8_t mgmId) {
+    /* Configure all SPI chip selects and pull them high */
+    acsInit();
+
+    /* Adapt accordingly */
+    if(mgmId != mgm1Rm3100ChipSelect and mgmId != mgm3Rm3100ChipSelect) {
+        sif::warning << "SpiTestClass::performRm3100Test: Invalid MGM ID!" << std::endl;
+    }
+    gpioId_t currentGpioId = 0;
+    uint8_t chipSelectPin = mgmId;
+    if(chipSelectPin == mgm1Rm3100ChipSelect) {
+        currentGpioId = gpioIds::MGM_1_RM3100_CS;
+    }
+    else {
+        currentGpioId = gpioIds::MGM_3_RM3100_CS;
+    }
+    uint32_t rm3100speed = 3'900'000;
+    uint8_t rm3100revidReg = 0x36;
+    spi::SpiMode rm3100mode = spi::SpiMode::MODE_3;
+    //spiTransferStruct.speed_hz = rm3100Speed;
+#ifdef RASPBERRY_PI
+    std::string deviceName = "/dev/spidev0.0";
+#else
+    std::string deviceName = "placeholder";
+#endif
+    int fileDescriptor = 0;
+
+
+    utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR,
+            "SpiComIF::initializeInterface: ");
+    if(fileHelper.getOpenResult()) {
+        sif::error << "SpiTestClass::performRm3100Test: File descriptor could not be opened!"
+                << std::endl;
+        return;
+    }
+    setSpiSpeedAndMode(fileDescriptor, rm3100mode, rm3100speed);
+
+    uint8_t revId = readStmRegister(fileDescriptor, currentGpioId, rm3100revidReg, false);
+    sif::info << "SpiTestClass::performRm3100Test: Revision ID 0b" << std::bitset<8>(revId) <<
+            std::endl;
+}
+
+void SpiTestClass::performLis3MdlTest(uint8_t lis3Id) {
+    /* Configure all SPI chip selects and pull them high */
+    acsInit();
+
+    /* Adapt accordingly */
+    if(lis3Id != mgm0Lis3ChipSelect and lis3Id != mgm2Lis3mdlChipSelect) {
+        sif::warning << "SpiTestClass::performLis3MdlTest: Invalid MGM ID!" << std::endl;
+    }
+    gpioId_t currentGpioId = 0;
+    uint8_t chipSelectPin = lis3Id;
+    uint8_t whoAmIReg = 0b0000'1111;
+    if(chipSelectPin == mgm0Lis3ChipSelect) {
+        currentGpioId = gpioIds::MGM_0_LIS3_CS;
+    }
+    else {
+        currentGpioId = gpioIds::MGM_2_LIS3_CS;
+    }
+    uint32_t spiSpeed = 3'900'000;
+    spi::SpiMode spiMode = spi::SpiMode::MODE_3;
+#ifdef RASPBERRY_PI
+    std::string deviceName = "/dev/spidev0.0";
+#else
+    std::string deviceName = "placeholder";
+#endif
+    int fileDescriptor = 0;
+
+    utility::UnixFileHelper fileHelper(deviceName, &fileDescriptor, O_RDWR,
+            "SpiComIF::initializeInterface: ");
+    if(fileHelper.getOpenResult()) {
+        sif::error << "SpiTestClass::performLis3Mdl3100Test: File descriptor could not be opened!"
+                << std::endl;
+        return;
+    }
+    setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
+
+    uint8_t whoAmIRegVal = readStmRegister(fileDescriptor, currentGpioId, whoAmIReg, false);
+    sif::info << "SpiTestClass::performLis3MdlTest: WHO AM I Regiter 0b" <<
+            std::bitset<8>(whoAmIRegVal) << std::endl;
+}
+
+
+void SpiTestClass::acsInit() {
+    GpioCookie* gpioCookie = new GpioCookie();
+    std::string rpiGpioName = "gpiochip0";
+    {
+        GpiodRegular gpio(rpiGpioName, mgm0Lis3ChipSelect, "MGM_0_LIS3",
+                gpio::Direction::OUT, 1);
+        gpioCookie->addGpio(gpioIds::MGM_0_LIS3_CS, gpio);
+    }
+    {
+        GpiodRegular gpio(rpiGpioName, mgm1Rm3100ChipSelect, "MGM_1_RM3100",
+                gpio::Direction::OUT, 1);
+        gpioCookie->addGpio(gpioIds::MGM_1_RM3100_CS, gpio);
+    }
+    {
+        GpiodRegular gpio(rpiGpioName, gyro0AdisChipSelect, "GYRO_0_ADIS",
+                gpio::Direction::OUT, 1);
+        gpioCookie->addGpio(gpioIds::GYRO_0_ADIS_CS, gpio);
+    }
+    {
+        GpiodRegular gpio(rpiGpioName, gyro1L3gd20ChipSelect, "GYRO_1_L3G",
+                gpio::Direction::OUT, 1);
+        gpioCookie->addGpio(gpioIds::GYRO_1_L3G_CS, gpio);
+    }
+    {
+        GpiodRegular gpio(rpiGpioName, gyro2L3gd20ChipSelect, "GYRO_2_L3G",
+                gpio::Direction::OUT, 1);
+        gpioCookie->addGpio(gpioIds::GYRO_2_L3G_CS, gpio);
+    }
+    {
+        GpiodRegular gpio(rpiGpioName, mgm2Lis3mdlChipSelect, "MGM_2_LIS3",
+                gpio::Direction::OUT, 1);
+        gpioCookie->addGpio(gpioIds::MGM_2_LIS3_CS, gpio);
+    }
+    {
+        GpiodRegular gpio(rpiGpioName, mgm3Rm3100ChipSelect, "MGM_3_RM3100",
+                gpio::Direction::OUT, 1);
+        gpioCookie->addGpio(gpioIds::MGM_3_RM3100_CS, gpio);
+    }
+    if(gpioIF != nullptr) {
+        gpioIF->addGpios(gpioCookie);
+    }
+}
+
+void SpiTestClass::writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
+        bool autoIncrement) {
+    if(autoIncrement) {
+        reg |= STM_AUTO_INCR_MASK;
+    }
+    spiTransferStruct.len = 2;
+    sendBuffer[0] = reg;
+    sendBuffer[1] = value;
+
+    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
+        gpioIF->pullLow(chipSelect);
+    }
+    int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
+    if(retval != 0) {
+        utility::handleIoctlError("SpiTestClass::writeStmRegister: Write failed");
+    }
+    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
+        gpioIF->pullHigh(chipSelect);
+    }
+}
+
+void SpiTestClass::setSpiSpeedAndMode(int spiFd, spi::SpiMode 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!");
+    }
+
+    retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
+    if(retval != 0) {
+        utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!");
+    }
+}
+
+uint8_t SpiTestClass::readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg,
+        bool autoIncrement) {
+    reg |= STM_READ_MASK;
+    if(autoIncrement) {
+        reg |= STM_AUTO_INCR_MASK;
+    }
+    spiTransferStruct.len = 2;
+    sendBuffer[0] = reg;
+    sendBuffer[1] = 0;
+
+    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
+        gpioIF->pullLow(chipSelect);
+    }
+    int retval = ioctl(fd, SPI_IOC_MESSAGE(1), &spiTransferStruct);
+    if(retval < 0) {
+        utility::handleIoctlError("SpiTestClass::readStmRegiste: Read failed");
+    }
+    if(gpioIF != nullptr and chipSelect != gpio::NO_GPIO) {
+        gpioIF->pullHigh(chipSelect);
+    }
+    return recvBuffer[1];
+}

linux/boardtest/SpiTestClass.h

@@ -0,0 +1,61 @@
+#ifndef LINUX_BOARDTEST_SPITESTCLASS_H_
+#define LINUX_BOARDTEST_SPITESTCLASS_H_
+
+#include <linux/gpio/GpioIF.h>
+#include <linux/spi/SpiCookie.h>
+#include <test/testtasks/TestTask.h>
+
+#include <vector>
+
+class SpiTestClass: public TestTask {
+public:
+    enum TestModes {
+        NONE,
+        MGM_LIS3MDL,
+        MGM_RM3100,
+        GYRO_L3GD20H,
+    };
+
+    TestModes testMode;
+
+    SpiTestClass(object_id_t objectId, GpioIF* gpioIF);
+
+    ReturnValue_t performOneShotAction() override;
+    ReturnValue_t performPeriodicAction() override;
+private:
+
+    GpioIF* gpioIF;
+
+    std::array<uint8_t, 128> recvBuffer;
+    std::array<uint8_t, 128> sendBuffer;
+    struct spi_ioc_transfer spiTransferStruct;
+
+    void performRm3100Test(uint8_t mgmId);
+    void performLis3MdlTest(uint8_t lis3Id);
+
+    /* ACS board specific code which pulls all GPIOs high */
+    void acsInit();
+
+    /* ACS board specific variables */
+    uint8_t mgm0Lis3ChipSelect = 0;
+    uint8_t mgm1Rm3100ChipSelect = 1;
+    uint8_t gyro0AdisChipSelect = 5;
+    uint8_t gyro1L3gd20ChipSelect = 6;
+    uint8_t gyro2L3gd20ChipSelect = 4;
+    uint8_t mgm2Lis3mdlChipSelect = 17;
+    uint8_t mgm3Rm3100ChipSelect = 27;
+
+    static constexpr uint8_t STM_READ_MASK = 0b1000'0000;
+    static constexpr uint8_t STM_AUTO_INCR_MASK = 0b0100'0000;
+
+    void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed);
+    void writeStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, uint8_t value,
+            bool autoIncrement);
+    uint8_t readStmRegister(int fd, gpioId_t chipSelect, uint8_t reg, bool autoIncrement);
+
+};
+
+
+
+
+#endif /* LINUX_BOARDTEST_SPITESTCLASS_H_ */

linux/boardtest/UartTestClass.cpp

@@ -0,0 +1,8 @@
+#include <linux/boardtest/UartTestClass.h>
+
+UartTestClass::UartTestClass(object_id_t objectId): TestTask(objectId) {
+}
+
+ReturnValue_t UartTestClass::performPeriodicAction() {
+    return HasReturnvaluesIF::RETURN_OK;
+}

linux/boardtest/UartTestClass.h

@@ -0,0 +1,15 @@
+#ifndef LINUX_BOARDTEST_UARTTESTCLASS_H_
+#define LINUX_BOARDTEST_UARTTESTCLASS_H_
+
+#include <test/testtasks/TestTask.h>
+
+class UartTestClass: public TestTask {
+public:
+    UartTestClass(object_id_t objectId);
+
+    ReturnValue_t performPeriodicAction() override;
+private:
+
+};
+
+#endif /* LINUX_BOARDTEST_UARTTESTCLASS_H_ */

linux/csp/CspCookie.h

@@ -1,5 +1,5 @@
-#ifndef BSP_Q7S_COMIF_COOKIES_CSPCOOKIE_H_
-#define BSP_Q7S_COMIF_COOKIES_CSPCOOKIE_H_
+#ifndef LINUX_CSP_CSPCOOKIE_H_
+#define LINUX_CSP_CSPCOOKIE_H_
 
 #include <fsfw/devicehandlers/CookieIF.h>
 #include <cstdint>
@@ -24,4 +24,4 @@ private:
 	uint8_t cspAddress;
 };
 
-#endif /* BSP_Q7S_COMIF_COOKIES_CSPCOOKIE_H_ */
+#endif /* LINUX_CSP_CSPCOOKIE_H_ */

linux/gpio/CMakeLists.txt

@@ -3,6 +3,10 @@ target_sources(${TARGET_NAME} PUBLIC
 	LinuxLibgpioIF.cpp
 )
 
+target_link_libraries(${TARGET_NAME} PUBLIC
+    gpiod
+)
+
 
 
 

linux/gpio/GpioCookie.cpp

@@ -1,25 +1,29 @@
 #include "GpioCookie.h"
-#include <fsfw/serviceinterface/ServiceInterfaceStream.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
 
 GpioCookie::GpioCookie() {
 }
 
-void GpioCookie::addGpio(gpioId_t gpioId, GpioConfig_t gpioConfig){
-	gpioMapIter = gpioMap.find(gpioId);
+ReturnValue_t GpioCookie::addGpio(gpioId_t gpioId, GpiodRegular& gpioConfig){
+	auto gpioMapIter = gpioMap.find(gpioId);
 	if(gpioMapIter == gpioMap.end()) {
-		std::pair status = gpioMap.emplace(gpioId, gpioConfig);
-		if (status.second == false) {
-			sif::error << "GpioCookie::addGpio: Failed to add GPIO "
-					<< gpioId << "to GPIO map" << std::endl;
+		auto statusPair = gpioMap.emplace(gpioId, new GpiodRegular(gpioConfig));
+		if (statusPair.second == false) {
+#if FSFW_VERBOSE_LEVEL >= 1
+			sif::error << "GpioCookie::addGpio: Failed to add GPIO " << gpioId <<
+			        "to GPIO map" << std::endl;
+#endif
+			return HasReturnvaluesIF::RETURN_FAILED;
 		}
+		return HasReturnvaluesIF::RETURN_OK;
 	}
-	else {
-		sif::error << "GpioCookie::addGpio: GPIO already exists in GPIO map "
-				<< std::endl;
-	}
+#if FSFW_VERBOSE_LEVEL >= 1
+	sif::error << "GpioCookie::addGpio: GPIO already exists in GPIO map " << std::endl;
+#endif
+	return HasReturnvaluesIF::RETURN_FAILED;
 }
 
-GpioMap GpioCookie::getGpioMap() const{
+GpioMap GpioCookie::getGpioMap() const {
 	return gpioMap;
 }
 

linux/gpio/GpioCookie.h

@@ -1,48 +1,10 @@
-#ifndef SAM9G20_COMIF_COOKIES_GPIO_COOKIE_H_
-#define SAM9G20_COMIF_COOKIES_GPIO_COOKIE_H_
+#ifndef LINUX_GPIO_GPIOCOOKIE_H_
+#define LINUX_GPIO_GPIOCOOKIE_H_
 
 #include "GpioIF.h"
+#include "gpioDefinitions.h"
 #include <fsfw/devicehandlers/CookieIF.h>
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
-#include <string>
-#include <unordered_map>
-
-namespace gpio {
-enum Direction {
-    IN = 0,
-    OUT = 1
-};
-}
-
-/**
- * @brief	Struct containing information about the GPIO to use. This is
- * 			required by the libgpiod to access and drive a GPIO.
- * @param chipname	String of the chipname specifying the group which contains
- *  	  			the GPIO to access. E.g. gpiochip0. To detect names of
- *  	  			GPIO groups run gpiodetect on the linux command line.
- * @param lineNum	The offset of the GPIO within the GPIO group.
- * @param consumer	Name of the consumer. Simply a description of the GPIO configuration.
- * @param direction Specifies whether the GPIO should be used as in- or output.
- * @param initValue Defines the initial state of the GPIO when configured as output. Only required
- *                  for output GPIOs.
- * @param lineHandle    The handle returned by gpiod_chip_get_line will be later written to this
- *                      pointer.
- */
-typedef struct GpioConfig {
-    GpioConfig(std::string chipname_, int lineNum_, std::string consumer_,
-            gpio::Direction direction_, int initValue_) :
-                chipname(chipname_), lineNum(lineNum_), consumer(consumer_), direction(direction_),
-                initValue(initValue_) {
-    }
-    std::string chipname;
-    int lineNum;
-    std::string consumer;
-    gpio::Direction direction;
-    int initValue;
-    struct gpiod_line* lineHandle = nullptr;
-} GpioConfig_t;
-using GpioMap = std::unordered_map<gpioId_t, GpioConfig_t>;
-using GpioMapIter = GpioMap::iterator;
 
 /**
  * @brief   Cookie for the GpioIF. Allows the GpioIF to determine which
@@ -61,16 +23,17 @@ public:
 
     virtual ~GpioCookie();
 
-    void addGpio(gpioId_t gpioId, GpioConfig_t gpioConfig);
+    ReturnValue_t addGpio(gpioId_t gpioId, GpiodRegular& gpioConfig);
     /**
      * @brief	Get map with registered GPIOs.
      */
     GpioMap getGpioMap() const;
 
 private:
-
+    /**
+     * Returns a copy of the internal GPIO map.
+     */
     GpioMap gpioMap;
-    GpioMapIter gpioMapIter;
 };
 
-#endif
+#endif /* LINUX_GPIO_GPIOCOOKIE_H_ */

linux/gpio/GpioIF.h

@@ -1,33 +1,35 @@
-#ifndef BSP_Q7S_GPIO_GPIOIF_H_
-#define BSP_Q7S_GPIO_GPIOIF_H_
+#ifndef LINUX_GPIO_GPIOIF_H_
+#define LINUX_GPIO_GPIOIF_H_
 
+#include "gpioDefinitions.h"
 #include <fsfw/returnvalues/HasReturnvaluesIF.h>
 #include <fsfw/devicehandlers/CookieIF.h>
 
-typedef uint16_t gpioId_t;
+class GpioCookie;
 
 /**
  * @brief	This class defines the interface for objects requiring the control
  * 			over GPIOs.
  * @author	J. Meier
  */
-class GpioIF : public HasReturnvaluesIF{
+class GpioIF : public HasReturnvaluesIF {
 public:
 
 	virtual ~GpioIF() {};
 
 	/**
-	 * @brief	Called by the GPIO using object.
+	 * @brief   Called by the GPIO using object.
 	 * @param cookie	Cookie specifying informations of the GPIOs required
 	 * 					by a object.
 	 */
-	virtual ReturnValue_t initialize(CookieIF * cookie) = 0;
+	virtual ReturnValue_t addGpios(GpioCookie* cookie) = 0;
 
 	/**
 	 * @brief	By implementing this function a child must provide the
 	 * 			functionality to pull a certain GPIO to high logic level.
 	 *
 	 * @param gpioId	A unique number which specifies the GPIO to drive.
+	 * @return  Returns RETURN_OK for success. This should never return RETURN_FAILED.
 	 */
 	virtual ReturnValue_t pullHigh(gpioId_t gpioId) = 0;
 
@@ -49,4 +51,4 @@ public:
     virtual ReturnValue_t readGpio(gpioId_t gpioId, int* gpioState) = 0;
 };
 
-#endif /* BSP_Q7S_GPIO_GPIOIF_H_ */
+#endif /* LINUX_GPIO_GPIOIF_H_ */

linux/gpio/LinuxLibgpioIF.h

@@ -1,11 +1,12 @@
-#ifndef BSP_Q7S_GPIO_LINUXLIBGPIOIF_H_
-#define BSP_Q7S_GPIO_LINUXLIBGPIOIF_H_
+#ifndef LINUX_GPIO_LINUXLIBGPIOIF_H_
+#define LINUX_GPIO_LINUXLIBGPIOIF_H_
 
 #include <linux/gpio/GpioIF.h>
-#include <linux/gpio/GpioCookie.h>
 #include <fsfwconfig/returnvalues/classIds.h>
 #include <fsfw/objectmanager/SystemObject.h>
 
+class GpioCookie;
+
 /**
  * @brief	This class implements the GpioIF for a linux based system. The
  * 			implementation is based on the libgpiod lib which requires linux 4.8
@@ -16,21 +17,27 @@
 class LinuxLibgpioIF : public GpioIF, public SystemObject {
 public:
 
-    static const uint8_t INTERFACE_ID = CLASS_ID::LINUX_LIBGPIO_IF;
+    static const uint8_t gpioRetvalId = CLASS_ID::LINUX_LIBGPIO_IF;
 
-	static const ReturnValue_t DRIVE_GPIO_FAILURE = MAKE_RETURN_CODE(0x2);
+    static constexpr ReturnValue_t UNKNOWN_GPIO_ID =
+            HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 1);
+    static constexpr ReturnValue_t DRIVE_GPIO_FAILURE =
+            HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 2);
+    static constexpr ReturnValue_t GPIO_TYPE_FAILURE =
+            HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 3);
+    static constexpr ReturnValue_t GPIO_INVALID_INSTANCE =
+            HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 4);
 
 	LinuxLibgpioIF(object_id_t objectId);
 	virtual ~LinuxLibgpioIF();
 
-	ReturnValue_t initialize(CookieIF * cookie) override;
+	ReturnValue_t addGpios(GpioCookie* gpioCookie) override;
 	ReturnValue_t pullHigh(gpioId_t gpioId) override;
 	ReturnValue_t pullLow(gpioId_t gpioId) override;
 	ReturnValue_t readGpio(gpioId_t gpioId, int* gpioState) override;
 
 private:
-
-	/*Holds the information and configuration of all used GPIOs */
+	/* Holds the information and configuration of all used GPIOs */
 	GpioMap gpioMap;
 	GpioMapIter gpioMapIter;
 
@@ -40,8 +47,9 @@ private:
 	 * @param gpioId	The GPIO ID of the GPIO to drive.
 	 * @param logiclevel	The logic level to set. O or 1.
 	 */
-	ReturnValue_t driveGpio(gpioId_t gpioId,
-			unsigned int logiclevel);
+	ReturnValue_t driveGpio(gpioId_t gpioId, GpiodRegular* regularGpio, unsigned int logiclevel);
+
+	ReturnValue_t configureRegularGpio(gpioId_t gpioId, GpiodRegular* regularGpio);
 
 	/**
 	 * @brief	This function checks if GPIOs are already registered and whether
@@ -52,12 +60,18 @@ private:
 	 *
 	 * @return	RETURN_OK if successful, otherwise RETURN_FAILED
 	 */
-	ReturnValue_t checkForConflicts(GpioMap mapToAdd);
+	ReturnValue_t checkForConflicts(GpioMap& mapToAdd);
+
+	ReturnValue_t checkForConflictsRegularGpio(gpioId_t gpiodId, GpiodRegular* regularGpio,
+	        GpioMap& mapToAdd);
+    ReturnValue_t checkForConflictsCallbackGpio(gpioId_t gpiodId, GpioCallback* regularGpio,
+            GpioMap& mapToAdd);
 
 	/**
 	 * @brief   Performs the initial configuration of all GPIOs specified in the GpioMap mapToAdd.
 	 */
-	ReturnValue_t configureGpios(GpioMap* mapToAdd);
+	ReturnValue_t configureGpios(GpioMap& mapToAdd);
+
 };
 
-#endif /* BSP_Q7S_GPIO_LINUXLIBGPIOIF_H_ */
+#endif /* LINUX_GPIO_LINUXLIBGPIOIF_H_ */

linux/gpio/gpioDefinitions.h

@@ -0,0 +1,92 @@
+#ifndef LINUX_GPIO_GPIODEFINITIONS_H_
+#define LINUX_GPIO_GPIODEFINITIONS_H_
+
+#include <string>
+#include <unordered_map>
+
+using gpioId_t = uint16_t;
+
+namespace gpio {
+enum Direction {
+    IN = 0,
+    OUT = 1
+};
+
+enum GpioOperation {
+    READ,
+    WRITE
+};
+
+enum GpioTypes {
+    NONE,
+    GPIOD_REGULAR,
+    CALLBACK
+};
+
+static constexpr gpioId_t NO_GPIO = -1;
+}
+
+/**
+ * @brief   Struct containing information about the GPIO to use. This is
+ *          required by the libgpiod to access and drive a GPIO.
+ * @param chipname      String of the chipname specifying the group which contains the GPIO to
+ *                      access. E.g. gpiochip0. To detect names of GPIO groups run gpiodetect on
+ *                      the linux command line.
+ * @param lineNum       The offset of the GPIO within the GPIO group.
+ * @param consumer      Name of the consumer. Simply a description of the GPIO configuration.
+ * @param direction     Specifies whether the GPIO should be used as in- or output.
+ * @param initValue     Defines the initial state of the GPIO when configured as output.
+ *                      Only required for output GPIOs.
+ * @param lineHandle    The handle returned by gpiod_chip_get_line will be later written to this
+ *                      pointer.
+ */
+class GpioBase {
+public:
+
+    GpioBase() = default;
+
+    GpioBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction,
+            int initValue):
+            gpioType(gpioType), consumer(consumer),direction(direction), initValue(initValue) {}
+
+    virtual~ GpioBase() {};
+
+    /* Can be used to cast GpioBase to a concrete child implementation */
+    gpio::GpioTypes gpioType = gpio::GpioTypes::NONE;
+    std::string consumer;
+    gpio::Direction direction = gpio::Direction::IN;
+    int initValue = 0;
+};
+
+class GpiodRegular: public GpioBase {
+public:
+    GpiodRegular(): GpioBase(gpio::GpioTypes::GPIOD_REGULAR, std::string(),
+            gpio::Direction::IN, 0) {};
+
+    GpiodRegular(std::string chipname_, int lineNum_, std::string consumer_,
+            gpio::Direction direction_, int initValue_):
+            GpioBase(gpio::GpioTypes::GPIOD_REGULAR, consumer_, direction_, initValue_),
+            chipname(chipname_), lineNum(lineNum_) {}
+    std::string chipname;
+    int lineNum = 0;
+    struct gpiod_line* lineHandle = nullptr;
+};
+
+class GpioCallback: public GpioBase {
+public:
+    GpioCallback(std::string consumer, gpio::Direction direction_, int initValue_,
+            void (* callback) (gpioId_t gpioId, gpio::GpioOperation gpioOp, int value, void* args),
+            void* callbackArgs):
+            GpioBase(gpio::GpioTypes::CALLBACK, consumer, direction_, initValue_),
+            callback(callback), callbackArgs(callbackArgs) {}
+
+    void (* callback) (gpioId_t gpioId, gpio::GpioOperation gpioOp,
+            int value, void* args) = nullptr;
+    void* callbackArgs = nullptr;
+};
+
+
+using GpioMap = std::unordered_map<gpioId_t, GpioBase*>;
+using GpioMapIter = GpioMap::iterator;
+
+#endif /* LINUX_GPIO_GPIODEFINITIONS_H_ */

linux/i2c/I2cComIF.h

@@ -1,5 +1,5 @@
-#ifndef BSP_Q7S_COMIF_I2COMIF_H_
-#define BSP_Q7S_COMIF_I2COMIF_H_
+#ifndef LINUX_I2C_I2COMIF_H_
+#define LINUX_I2C_I2COMIF_H_
 
 #include "I2cCookie.h"
 #include <fsfw/objectmanager/SystemObject.h>
@@ -45,8 +45,8 @@ private:
 	I2cDeviceMapIter i2cDeviceMapIter;
 
 	/**
-	 * @brief	This function opens an i2c device and binds the opened file
-	 * 			to a specific i2c address.
+	 * @brief	This function opens an I2C device and binds the opened file
+	 * 			to a specific I2C address.
 	 * @param deviceFile	The name of the device file. E.g. i2c-0
 	 * @param i2cAddress	The address of the i2c slave device.
 	 * @param fileDescriptor	Pointer to device descriptor.
@@ -56,4 +56,4 @@ private:
 			address_t i2cAddress, int* fileDescriptor);
 };
 
-#endif /* BSP_Q7S_COMIF_I2COMIF_H_ */
+#endif /* LINUX_I2C_I2COMIF_H_ */

linux/i2c/I2cCookie.cpp

@@ -2,8 +2,7 @@
 
 I2cCookie::I2cCookie(address_t i2cAddress_, size_t maxReplyLen_,
 		std::string deviceFile_) :
-		i2cAddress(i2cAddress_), maxReplyLen(maxReplyLen_), deviceFile(
-				deviceFile_) {
+		i2cAddress(i2cAddress_), maxReplyLen(maxReplyLen_), deviceFile(deviceFile_) {
 }
 
 address_t I2cCookie::getAddress() const {

linux/spi/CMakeLists.txt

@@ -1,4 +1,6 @@
 target_sources(${TARGET_NAME} PUBLIC
+    SpiComIF.cpp
+    SpiCookie.cpp
 )
 
 

linux/spi/SpiComIF.cpp

@@ -0,0 +1,308 @@
+#include "SpiComIF.h"
+
+#include <linux/utility/Utility.h>
+#include <linux/spi/SpiCookie.h>
+
+#include <fsfw/ipc/MutexFactory.h>
+#include <fsfw/ipc/MutexHelper.h>
+#include <fsfw/globalfunctions/arrayprinter.h>
+
+#include <linux/spi/spidev.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+
+#include <cerrno>
+#include <cstring>
+
+SpiComIF::SpiComIF(object_id_t objectId, GpioIF* gpioComIF): SystemObject(objectId),
+        gpioComIF(gpioComIF) {
+    if(gpioComIF == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "SpiComIF::SpiComIF: GPIO communication interface invalid!" << std::endl;
+#else
+        sif::printError("SpiComIF::SpiComIF: GPIO communication interface invalid!\n");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+    }
+
+    spiMutex = MutexFactory::instance()->createMutex();
+}
+
+ReturnValue_t SpiComIF::initializeInterface(CookieIF *cookie) {
+    int retval = 0;
+    SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+    if(spiCookie == nullptr) {
+        return NULLPOINTER;
+    }
+
+    address_t spiAddress = spiCookie->getSpiAddress();
+
+    auto iter = spiDeviceMap.find(spiAddress);
+    if(iter == spiDeviceMap.end()) {
+        size_t bufferSize = spiCookie->getMaxBufferSize();
+        SpiInstance spiInstance = {std::vector<uint8_t>(bufferSize)};
+        auto statusPair = spiDeviceMap.emplace(spiAddress, spiInstance);
+        if (not statusPair.second) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::error << "SpiComIF::initializeInterface: Failed to insert device with address " <<
+                    spiAddress << "to SPI device map" << std::endl;
+#else
+            sif::printError("SpiComIF::initializeInterface: Failed to insert device with address "
+                    "%lu to SPI device map\n", static_cast<unsigned long>(spiAddress));
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+            return HasReturnvaluesIF::RETURN_FAILED;
+        }
+        /* Now we emplaced the read buffer in the map, we still need to assign that location
+        to the SPI driver transfer struct */
+        spiCookie->assignReadBuffer(statusPair.first->second.replyBuffer.data());
+    }
+    else {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::error << "SpiComIF::initializeInterface: SPI address already exists!" << std::endl;
+#else
+        sif::printError("SpiComIF::initializeInterface: SPI address already exists!\n");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    /* Pull CS high in any case to be sure that device is inactive */
+    gpioId_t gpioId = spiCookie->getChipSelectPin();
+    if(gpioId != gpio::NO_GPIO) {
+        gpioComIF->pullHigh(gpioId);
+    }
+
+    size_t spiSpeed = 0;
+    spi::SpiMode spiMode = spi::SpiMode::MODE_0;
+
+    SpiCookie::UncommonParameters params;
+    spiCookie->getSpiParameters(spiMode, spiSpeed, &params);
+
+    int fileDescriptor = 0;
+    utility::UnixFileHelper fileHelper(spiCookie->getSpiDevice(), &fileDescriptor, O_RDWR,
+            "SpiComIF::initializeInterface: ");
+    if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
+        return fileHelper.getOpenResult();
+    }
+
+    /* These flags are rather uncommon */
+    if(params.threeWireSpi or params.noCs or params.csHigh) {
+        uint32_t currentMode = 0;
+        retval = ioctl(fileDescriptor, SPI_IOC_RD_MODE32, &currentMode);
+        if(retval != 0) {
+            utility::handleIoctlError("SpiComIF::initialiezInterface: Could not read full mode!");
+        }
+
+        if(params.threeWireSpi) {
+            currentMode |= SPI_3WIRE;
+        }
+        if(params.noCs) {
+            /* Some drivers like the Raspberry Pi ignore this flag in any case */
+            currentMode |= SPI_NO_CS;
+        }
+        if(params.csHigh) {
+            currentMode |= SPI_CS_HIGH;
+        }
+        /* Write adapted mode */
+        retval = ioctl(fileDescriptor, SPI_IOC_WR_MODE32, &currentMode);
+        if(retval != 0) {
+            utility::handleIoctlError("SpiComIF::initialiezInterface: Could not write full mode!");
+        }
+    }
+    if(params.lsbFirst) {
+        retval = ioctl(fileDescriptor, SPI_IOC_WR_LSB_FIRST, &params.lsbFirst);
+        if(retval != 0) {
+            utility::handleIoctlError("SpiComIF::initializeInterface: Setting LSB first failed");
+        }
+    }
+    if(params.bitsPerWord != 8) {
+        retval = ioctl(fileDescriptor, SPI_IOC_WR_BITS_PER_WORD, &params.bitsPerWord);
+        if(retval != 0) {
+            utility::handleIoctlError("SpiComIF::initializeInterface: "
+                    "Could not write bits per word!");
+        }
+    }
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SpiComIF::sendMessage(CookieIF *cookie, const uint8_t *sendData, size_t sendLen) {
+    SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    int retval = 0;
+
+    if(spiCookie == nullptr) {
+        return NULLPOINTER;
+    }
+
+    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;
+#else
+        sif::printWarning("SpiComIF::sendMessage: Too much data sent, send length %lu larger "
+                "than maximum buffer length %lu!\n", static_cast<unsigned long>(sendLen),
+                static_cast<unsigned long>(spiCookie->getMaxBufferSize()));
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return DeviceCommunicationIF::TOO_MUCH_DATA;
+    }
+
+
+    /* Prepare transfer */
+    int fileDescriptor = 0;
+    std::string device = spiCookie->getSpiDevice();
+    utility::UnixFileHelper fileHelper(device, &fileDescriptor, O_RDWR,
+            "SpiComIF::sendMessage: ");
+    if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
+        return OPENING_FILE_FAILED;
+    }
+    spi::SpiMode spiMode = spi::SpiMode::MODE_0;
+    uint32_t spiSpeed = 0;
+    spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr);
+    setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed);
+    spiCookie->assignWriteBuffer(sendData);
+    spiCookie->assignTransferSize(sendLen);
+
+    bool fullDuplex = spiCookie->isFullDuplex();
+    gpioId_t gpioId = spiCookie->getChipSelectPin();
+
+    /* GPIO access is mutex protected */
+    MutexHelper(spiMutex, timeoutType, timeoutMs);
+
+    /* Pull SPI CS low. For now, no support for active high given  */
+    if(gpioId != gpio::NO_GPIO) {
+        gpioComIF->pullLow(gpioId);
+    }
+
+    /* Execute transfer */
+    if(fullDuplex) {
+        /* Initiate a full duplex SPI transfer. */
+        retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), spiCookie->getTransferStructHandle());
+        if(retval < 0) {
+            utility::handleIoctlError("SpiComIF::sendMessage: ioctl error.");
+            result = FULL_DUPLEX_TRANSFER_FAILED;
+        }
+    }
+    else {
+        /* We write with a blocking half-duplex transfer here */
+        if (write(fileDescriptor, sendData, sendLen) != static_cast<ssize_t>(sendLen)) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "SpiComIF::sendMessage: Half-Duplex write operation failed!" <<
+                    std::endl;
+#else
+            sif::printWarning("SpiComIF::sendMessage: Half-Duplex write operation failed!\n");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+            result = HALF_DUPLEX_TRANSFER_FAILED;
+        }
+    }
+
+    if(gpioId != gpio::NO_GPIO) {
+        gpioComIF->pullHigh(gpioId);
+    }
+    return result;
+}
+
+ReturnValue_t SpiComIF::getSendSuccess(CookieIF *cookie) {
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SpiComIF::requestReceiveMessage(CookieIF *cookie, size_t requestLen) {
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+    if(spiCookie == nullptr) {
+        return NULLPOINTER;
+    }
+
+    bool fullDuplex = spiCookie->isFullDuplex();
+    if(fullDuplex) {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+
+    std::string device = spiCookie->getSpiDevice();
+    int fileDescriptor = 0;
+    utility::UnixFileHelper fileHelper(device, &fileDescriptor, O_RDWR,
+            "SpiComIF::requestReceiveMessage: ");
+    if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) {
+        return OPENING_FILE_FAILED;
+    }
+
+    uint8_t* rxBuf = nullptr;
+    size_t readSize = spiCookie->getCurrentTransferSize();
+    result = getReadBuffer(spiCookie->getSpiAddress(), &rxBuf);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    gpioId_t gpioId = spiCookie->getChipSelectPin();
+    MutexHelper(spiMutex, timeoutType, timeoutMs);
+    if(gpioId != gpio::NO_GPIO) {
+        gpioComIF->pullLow(gpioId);
+    }
+
+    if(read(fileDescriptor, rxBuf, readSize) != static_cast<ssize_t>(readSize)) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "SpiComIF::sendMessage: Half-Duplex read operation failed!" << std::endl;
+#else
+        sif::printWarning("SpiComIF::sendMessage: Half-Duplex read operation failed!\n");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        result = HALF_DUPLEX_TRANSFER_FAILED;
+    }
+
+    if(gpioId != gpio::NO_GPIO) {
+        gpioComIF->pullHigh(gpioId);
+    }
+
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SpiComIF::readReceivedMessage(CookieIF *cookie, uint8_t **buffer, size_t *size) {
+    SpiCookie* spiCookie = dynamic_cast<SpiCookie*>(cookie);
+    if(spiCookie == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+    uint8_t* rxBuf = nullptr;
+    ReturnValue_t result = getReadBuffer(spiCookie->getSpiAddress(), &rxBuf);
+    if(result != HasReturnvaluesIF::RETURN_OK) {
+        return result;
+    }
+
+    *buffer = rxBuf;
+    *size = spiCookie->getCurrentTransferSize();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+ReturnValue_t SpiComIF::getReadBuffer(address_t spiAddress, uint8_t** buffer) {
+    if(buffer == nullptr) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    auto iter = spiDeviceMap.find(spiAddress);
+    if(iter == spiDeviceMap.end()) {
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
+    *buffer = iter->second.replyBuffer.data();
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiMode 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!");
+    }
+
+    retval = ioctl(spiFd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
+    if(retval != 0) {
+        utility::handleIoctlError("SpiTestClass::performRm3100Test: Setting SPI speed failed!");
+    }
+}

linux/spi/SpiComIF.h

@@ -0,0 +1,63 @@
+#ifndef LINUX_SPI_SPICOMIF_H_
+#define LINUX_SPI_SPICOMIF_H_
+
+#include <fsfw/devicehandlers/DeviceCommunicationIF.h>
+#include <fsfw/objectmanager/SystemObject.h>
+#include <linux/gpio/GpioIF.h>
+#include <linux/spi/spiDefinitions.h>
+#include <returnvalues/classIds.h>
+
+#include <vector>
+#include <unordered_map>
+
+/**
+ * @brief   Encapsulates access to linux SPI driver for FSFW objects
+ * @details
+ * Right now, only full-duplex SPI is supported.
+ * @author  R. Mueller
+ */
+class SpiComIF: public DeviceCommunicationIF, public SystemObject {
+public:
+    static constexpr uint8_t spiRetvalId = CLASS_ID::LINUX_SPI_COM_IF;
+    static constexpr ReturnValue_t OPENING_FILE_FAILED =
+            HasReturnvaluesIF::makeReturnCode(spiRetvalId, 0);
+    /* Full duplex (ioctl) transfer failure */
+    static constexpr ReturnValue_t FULL_DUPLEX_TRANSFER_FAILED =
+            HasReturnvaluesIF::makeReturnCode(spiRetvalId, 1);
+    /* Half duplex (read/write) transfer failure */
+    static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED =
+            HasReturnvaluesIF::makeReturnCode(spiRetvalId, 2);
+
+    SpiComIF(object_id_t objectId, GpioIF* gpioComIF);
+
+    ReturnValue_t initializeInterface(CookieIF * cookie) override;
+    ReturnValue_t sendMessage(CookieIF *cookie,const uint8_t *sendData,
+            size_t sendLen) override;
+    ReturnValue_t getSendSuccess(CookieIF *cookie) override;
+    ReturnValue_t requestReceiveMessage(CookieIF *cookie,
+            size_t requestLen) override;
+    ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
+            size_t *size) override;
+private:
+
+    struct SpiInstance {
+        std::vector<uint8_t> replyBuffer;
+    };
+
+    GpioIF* gpioComIF = nullptr;
+
+    MutexIF* spiMutex = nullptr;
+    MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING;
+    uint32_t timeoutMs = 20;
+
+    using SpiDeviceMap = std::unordered_map<address_t, SpiInstance>;
+    using SpiDeviceMapIter = SpiDeviceMap::iterator;
+
+    SpiDeviceMap spiDeviceMap;
+
+
+    ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer);
+    void setSpiSpeedAndMode(int spiFd, spi::SpiMode mode, uint32_t speed);
+};
+
+#endif /* LINUX_SPI_SPICOMIF_H_ */

linux/spi/SpiCookie.cpp

@@ -0,0 +1,99 @@
+#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),
+        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):
+        SpiCookie(spiAddress, gpio::NO_GPIO, spiDev, maxSize, spiMode, spiSpeed) {
+}
+
+void SpiCookie::getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed,
+        UncommonParameters* parameters) const {
+    spiMode = this->spiMode;
+    spiSpeed = this->spiSpeed;
+
+    if(parameters != nullptr) {
+        parameters->threeWireSpi = uncommonParameters.threeWireSpi;
+        parameters->lsbFirst = uncommonParameters.lsbFirst;
+        parameters->noCs = uncommonParameters.noCs;
+        parameters->bitsPerWord = uncommonParameters.bitsPerWord;
+        parameters->csHigh = uncommonParameters.csHigh;
+    }
+}
+
+gpioId_t SpiCookie::getChipSelectPin() const {
+    return chipSelectPin;
+}
+
+size_t SpiCookie::getMaxBufferSize() const {
+    return maxSize;
+}
+
+address_t SpiCookie::getSpiAddress() const {
+    return spiAddress;
+}
+
+std::string SpiCookie::getSpiDevice() const {
+    return spiDevice;
+}
+
+void SpiCookie::setThreeWireSpi(bool enable) {
+    uncommonParameters.threeWireSpi = enable;
+}
+
+void SpiCookie::setLsbFirst(bool enable) {
+    uncommonParameters.lsbFirst = enable;
+}
+
+void SpiCookie::setNoCs(bool enable) {
+    uncommonParameters.noCs = enable;
+}
+
+void SpiCookie::setBitsPerWord(uint8_t bitsPerWord) {
+    uncommonParameters.bitsPerWord = bitsPerWord;
+}
+
+void SpiCookie::setCsHigh(bool enable) {
+    uncommonParameters.csHigh = enable;
+}
+
+void SpiCookie::activateCsDeselect(bool deselectCs, uint16_t delayUsecs) {
+    spiTransferStruct.cs_change = deselectCs;
+    spiTransferStruct.delay_usecs = delayUsecs;
+}
+
+void SpiCookie::assignReadBuffer(uint8_t* rx) {
+    if(rx != nullptr) {
+        spiTransferStruct.rx_buf = reinterpret_cast<__u64>(rx);
+    }
+}
+
+void SpiCookie::assignWriteBuffer(const uint8_t* tx) {
+    if(tx != nullptr) {
+        spiTransferStruct.tx_buf = reinterpret_cast<__u64>(tx);
+    }
+}
+
+spi_ioc_transfer* SpiCookie::getTransferStructHandle() {
+    return &spiTransferStruct;
+}
+
+void SpiCookie::setFullOrHalfDuplex(bool halfDuplex) {
+    this->halfDuplex = halfDuplex;
+}
+
+bool SpiCookie::isFullDuplex() const {
+    return not this->halfDuplex;
+}
+
+void SpiCookie::assignTransferSize(size_t transferSize) {
+    spiTransferStruct.len = transferSize;
+}
+
+size_t SpiCookie::getCurrentTransferSize() const {
+    return spiTransferStruct.len;
+}

linux/spi/SpiCookie.h

@@ -0,0 +1,113 @@
+#ifndef LINUX_SPI_SPICOOKIE_H_
+#define LINUX_SPI_SPICOOKIE_H_
+
+#include "spiDefinitions.h"
+#include <fsfw/devicehandlers/CookieIF.h>
+#include <linux/gpio/gpioDefinitions.h>
+#include <linux/spi/spidev.h>
+
+class SpiCookie: public CookieIF {
+public:
+
+    /**
+     * Each SPI device will have a corresponding cookie. The cookie is used by the communication
+     * interface and contains device specific information like the largest expected size to be
+     * sent and received and the GPIO pin used to toggle the SPI slave select pin.
+     * @param spiAddress
+     * @param chipSelect    Chip select. gpio::NO_GPIO can be used for hardware slave selects.
+     * @param spiDev
+     * @param maxSize
+     */
+    SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev,
+            const size_t maxReplySize, spi::SpiMode 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);
+
+    address_t getSpiAddress() const;
+    std::string getSpiDevice() const;
+    gpioId_t getChipSelectPin() const;
+    size_t getMaxBufferSize() const;
+    /**
+     * True if SPI transfers should be performed in full duplex mode
+     * @return
+     */
+    bool isFullDuplex() const;
+
+    /**
+     * Set transfer type to full duplex or half duplex. Full duplex is the default setting,
+     * ressembling common SPI hardware implementation with shift registers, where read and writes
+     * happen simultaneosly.
+     * @param fullDuplex
+     */
+    void setFullOrHalfDuplex(bool halfDuplex);
+
+    /**
+     * This needs to be called to specify where the SPI driver writes to or reads from.
+     * @param readLocation
+     * @param writeLocation
+     */
+    void assignReadBuffer(uint8_t* rx);
+    void assignWriteBuffer(const uint8_t* tx);
+    /**
+     * Assign size for the next transfer.
+     * @param transferSize
+     */
+    void assignTransferSize(size_t transferSize);
+    size_t getCurrentTransferSize() const;
+
+    struct UncommonParameters {
+        uint8_t bitsPerWord = 8;
+        bool noCs = false;
+        bool csHigh = false;
+        bool threeWireSpi = false;
+        /* MSB first is more common */
+        bool lsbFirst = false;
+    };
+
+    /**
+     * Can be used to explicitely disable hardware chip select.
+     * Some drivers like the Raspberry Pi Linux driver will not use hardware chip select by default
+     * (see https://www.raspberrypi.org/documentation/hardware/raspberrypi/spi/README.md)
+     * @param enable
+     */
+    void setNoCs(bool enable);
+    void setThreeWireSpi(bool enable);
+    void setLsbFirst(bool enable);
+    void setCsHigh(bool enable);
+    void setBitsPerWord(uint8_t bitsPerWord);
+
+    void getSpiParameters(spi::SpiMode& spiMode, uint32_t& spiSpeed,
+            UncommonParameters* parameters = nullptr) const;
+
+    /**
+     * See spidev.h cs_change and delay_usecs
+     * @param deselectCs
+     * @param delayUsecs
+     */
+    void activateCsDeselect(bool deselectCs, uint16_t delayUsecs);
+
+    spi_ioc_transfer* getTransferStructHandle();
+private:
+    size_t currentTransferSize = 0;
+
+    address_t spiAddress;
+    gpioId_t chipSelectPin;
+    std::string spiDevice;
+
+    const size_t maxSize;
+    spi::SpiMode spiMode;
+    uint32_t spiSpeed;
+    bool halfDuplex = false;
+
+    struct spi_ioc_transfer spiTransferStruct = {};
+    UncommonParameters uncommonParameters;
+};
+
+
+
+#endif /* LINUX_SPI_SPICOOKIE_H_ */

linux/spi/spiDefinitions.h

@@ -0,0 +1,15 @@
+#ifndef LINUX_SPI_SPIDEFINITONS_H_
+#define LINUX_SPI_SPIDEFINITONS_H_
+
+namespace spi {
+
+enum SpiMode {
+    MODE_0,
+    MODE_1,
+    MODE_2,
+    MODE_3
+};
+
+}
+
+#endif /* LINUX_SPI_SPIDEFINITONS_H_ */

linux/utility/CMakeLists.txt

@@ -0,0 +1,7 @@
+target_sources(${TARGET_NAME} PUBLIC
+    Utility.cpp
+)
+
+
+
+

linux/utility/Utility.cpp

@@ -0,0 +1,52 @@
+#include "Utility.h"
+
+void utility::handleIoctlError(const char* const customPrintout) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    if(customPrintout != nullptr) {
+        sif::warning << customPrintout << std::endl;
+    }
+    sif::warning << "handleIoctlError: Error code " << errno << ", "<< strerror(errno) <<
+            std::endl;
+#else
+    if(customPrintout != nullptr) {
+        sif::printWarning("%s\n", customPrintout);
+    }
+    sif::printWarning("handleIoctlError: Error code %d, %s\n", errno, strerror(errno));
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+
+}
+
+utility::UnixFileHelper::UnixFileHelper(std::string device, int* fileDescriptor, int flags,
+        std::string diagnosticPrefix):
+        fileDescriptor(fileDescriptor) {
+    if(fileDescriptor == nullptr) {
+        return;
+    }
+    *fileDescriptor = open(device.c_str(), 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 <<
+                "." << std::endl;
+        sif::warning << "Error description: " << strerror(errno) << std::endl;
+#else
+        sif::printError("%sOpening device failed with error code %d.\n", diagnosticPrefix);
+        sif::printWarning("Error description: %s\n", strerror(errno));
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        openStatus = OPEN_FILE_FAILED;
+    }
+}
+
+utility::UnixFileHelper::~UnixFileHelper() {
+    if(fileDescriptor != nullptr) {
+        close(*fileDescriptor);
+    }
+}
+
+ReturnValue_t utility::UnixFileHelper::getOpenResult() const {
+    return openStatus;
+}
+

linux/utility/Utility.h

@@ -0,0 +1,36 @@
+#ifndef LINUX_UTILITY_UTILITY_H_
+#define LINUX_UTILITY_UTILITY_H_
+
+#include <cerrno>
+#include <cstring>
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+
+namespace utility {
+
+void handleIoctlError(const char* const customPrintout);
+
+class UnixFileHelper {
+public:
+    static constexpr int READ_WRITE_FLAG = O_RDWR;
+    static constexpr int READ_ONLY_FLAG = O_RDONLY;
+    static constexpr int NON_BLOCKING_IO_FLAG = O_NONBLOCK;
+
+    static constexpr ReturnValue_t OPEN_FILE_FAILED = 1;
+
+    UnixFileHelper(std::string device, int* fileDescriptor, int flags,
+            std::string diagnosticPrefix = "");
+
+    virtual~ UnixFileHelper();
+
+    ReturnValue_t getOpenResult() const;
+private:
+    int* fileDescriptor = nullptr;
+    ReturnValue_t openStatus = HasReturnvaluesIF::RETURN_OK;
+};
+
+}
+
+#endif /* LINUX_UTILITY_UTILITY_H_ */

misc/eclipse/Host/eive-mingw-debug-cmake.launch

@@ -4,7 +4,7 @@
     <listAttribute key="org.eclipse.cdt.dsf.gdb.AUTO_SOLIB_LIST"/>
     <stringAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_NAME" value="gdb"/>
     <booleanAttribute key="org.eclipse.cdt.dsf.gdb.DEBUG_ON_FORK" value="false"/>
-    <booleanAttribute key="org.eclipse.cdt.dsf.gdb.EXTERNAL_CONSOLE" value="false"/>
+    <booleanAttribute key="org.eclipse.cdt.dsf.gdb.EXTERNAL_CONSOLE" value="true"/>
     <stringAttribute key="org.eclipse.cdt.dsf.gdb.GDB_INIT" value=".gdbinit"/>
     <booleanAttribute key="org.eclipse.cdt.dsf.gdb.NON_STOP" value="false"/>
     <booleanAttribute key="org.eclipse.cdt.dsf.gdb.REVERSE" value="false"/>
@@ -18,7 +18,7 @@
     <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_START_MODE" value="run"/>
     <booleanAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN" value="true"/>
     <stringAttribute key="org.eclipse.cdt.launch.DEBUGGER_STOP_AT_MAIN_SYMBOL" value="main"/>
-    <stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="Debug/eive_obsw"/>
+    <stringAttribute key="org.eclipse.cdt.launch.PROGRAM_NAME" value="Debug-Host/eive_obsw.exe"/>
     <stringAttribute key="org.eclipse.cdt.launch.PROJECT_ATTR" value="eive_obsw"/>
     <booleanAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_AUTO_ATTR" value="false"/>
     <stringAttribute key="org.eclipse.cdt.launch.PROJECT_BUILD_CONFIG_ID_ATTR" value="cdt.managedbuild.toolchain.gnu.mingw.base.1455833186.1840876443"/>

mission/core/GenericFactory.cpp

@@ -11,6 +11,7 @@
 #include <fsfw/pus/CService200ModeCommanding.h>
 #include <fsfw/pus/Service17Test.h>
 #include <fsfw/pus/Service1TelecommandVerification.h>
+#include <fsfw/pus/Service20ParameterManagement.h>
 #include <fsfw/pus/Service3Housekeeping.h>
 #include <fsfw/pus/Service2DeviceAccess.h>
 #include <fsfw/pus/Service5EventReporting.h>
@@ -78,11 +79,13 @@ void ObjectFactory::produceGenericObjects() {
             apid::EIVE_OBSW, pus::PUS_SERVICE_9);
     new Service17Test(objects::PUS_SERVICE_17_TEST, apid::EIVE_OBSW,
             pus::PUS_SERVICE_17);
+    new Service20ParameterManagement(objects::PUS_SERVICE_20_PARAMETERS, apid::EIVE_OBSW,
+            pus::PUS_SERVICE_20);
     new CService200ModeCommanding(objects::PUS_SERVICE_200_MODE_MGMT,
             apid::EIVE_OBSW, pus::PUS_SERVICE_200);
 
     /* Test Device Handler */
 #if OBSW_ADD_TEST_CODE == 1
-    	new TestTask(objects::TEST_TASK);
+    new TestTask(objects::TEST_TASK);
 #endif
 }

mission/devices/MGMHandlerLIS3MDL.cpp

@@ -1,19 +1,20 @@
+#include <fsfw/datapool/PoolReadHelper.h>
 #include "MGMHandlerLIS3MDL.h"
 
 
 MGMHandlerLIS3MDL::MGMHandlerLIS3MDL(object_id_t objectId,
-		object_id_t deviceCommunication, CookieIF* comCookie):
-		DeviceHandlerBase(objectId, deviceCommunication, comCookie),
-		dataset(this) {
+        object_id_t deviceCommunication, CookieIF* comCookie):
+        DeviceHandlerBase(objectId, deviceCommunication, comCookie),
+        dataset(this) {
 #if OBSW_VERBOSE_LEVEL >= 1
-	debugDivider = new PeriodicOperationDivider(10);
+    debugDivider = new PeriodicOperationDivider(10);
 #endif
-	// Set to default values right away.
-	registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
-	registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
-	registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
-	registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
-	registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
+    /* Set to default values right away. */
+    registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
+    registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
+    registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
+    registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
+    registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
 
 }
 
@@ -22,392 +23,435 @@ MGMHandlerLIS3MDL::~MGMHandlerLIS3MDL() {
 
 
 void MGMHandlerLIS3MDL::doStartUp() {
-	switch (internalState) {
-	case(InternalState::STATE_NONE):
-		internalState = InternalState::STATE_FIRST_CONTACT;
-		break;
-
-	case(InternalState::STATE_FIRST_CONTACT):
-		internalState = InternalState::STATE_SETUP;
-		break;
-
-	case(InternalState::STATE_SETUP):
-		internalState = InternalState::STATE_CHECK_REGISTERS;
-		break;
-
-	case(InternalState::STATE_CHECK_REGISTERS): {
-		// Set up cached registers which will be used to configure the MGM.
-		if(commandExecuted) {
-			commandExecuted = false;
-			setMode(_MODE_TO_ON);
-		}
-		break;
-	}
-	default:
-		break;
-	}
+    switch (internalState) {
+    case(InternalState::STATE_NONE): {
+        internalState = InternalState::STATE_FIRST_CONTACT;
+        break;
+    }
+    case(InternalState::STATE_FIRST_CONTACT): {
+        /* Will be set by checking device ID (WHO AM I register) */
+        if(commandExecuted) {
+            commandExecuted = false;
+        }
+        internalState = InternalState::STATE_SETUP;
+        break;
+    }
+    case(InternalState::STATE_SETUP): {
+        internalState = InternalState::STATE_CHECK_REGISTERS;
+        break;
+    }
+    case(InternalState::STATE_CHECK_REGISTERS): {
+        /* 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 */
+            setMode(_MODE_TO_ON);
+        }
+        break;
+    }
+    default:
+        break;
+    }
 
 }
 
 void MGMHandlerLIS3MDL::doShutDown() {
-	setMode(_MODE_POWER_DOWN);
+    setMode(_MODE_POWER_DOWN);
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::buildTransitionDeviceCommand(
-		DeviceCommandId_t *id) {
-	switch (internalState) {
-	case(InternalState::STATE_NONE):
-	case(InternalState::STATE_NORMAL): {
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	case(InternalState::STATE_FIRST_CONTACT): {
-		*id = MGMLIS3MDL::IDENTIFY_DEVICE;
-		break;
-	}
-	case(InternalState::STATE_SETUP): {
-		*id = MGMLIS3MDL::SETUP_MGM;
-		break;
-	}
-	case(InternalState::STATE_CHECK_REGISTERS): {
-		*id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
-		break;
-	}
-	default:
-		// might be a configuration error.
-		sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown "
-				<< "internal state!" << std::endl;
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	return buildCommandFromCommand(*id, NULL, 0);
+        DeviceCommandId_t *id) {
+    switch (internalState) {
+    case(InternalState::STATE_NONE):
+    case(InternalState::STATE_NORMAL): {
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(InternalState::STATE_FIRST_CONTACT): {
+        *id = MGMLIS3MDL::IDENTIFY_DEVICE;
+        break;
+    }
+    case(InternalState::STATE_SETUP): {
+        *id = MGMLIS3MDL::SETUP_MGM;
+        break;
+    }
+    case(InternalState::STATE_CHECK_REGISTERS): {
+        *id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+        break;
+    }
+    default: {
+        /* might be a configuration error. */
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" <<
+                std::endl;
+#else
+        sif::printWarning("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n");
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+
+    }
+    return buildCommandFromCommand(*id, NULL, 0);
 }
 
 uint8_t MGMHandlerLIS3MDL::readCommand(uint8_t command, bool continuousCom) {
-	command |= (1 << MGMLIS3MDL::RW_BIT);
-	if (continuousCom == true) {
-		command |= (1 << MGMLIS3MDL::MS_BIT);
-	}
-	return command;
+    command |= (1 << MGMLIS3MDL::RW_BIT);
+    if (continuousCom == true) {
+        command |= (1 << MGMLIS3MDL::MS_BIT);
+    }
+    return command;
 }
 
 uint8_t MGMHandlerLIS3MDL::writeCommand(uint8_t command, bool continuousCom) {
-	command &= ~(1 << MGMLIS3MDL::RW_BIT);
-	if (continuousCom == true) {
-		command |= (1 << MGMLIS3MDL::MS_BIT);
-	}
-	return command;
+    command &= ~(1 << MGMLIS3MDL::RW_BIT);
+    if (continuousCom == true) {
+        command |= (1 << MGMLIS3MDL::MS_BIT);
+    }
+    return command;
 }
 
 void MGMHandlerLIS3MDL::setupMgm() {
 
-	registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
-	registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
-	registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
-	registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
-	registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
+    registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT;
+    registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT;
+    registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT;
+    registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT;
+    registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT;
 
-	prepareCtrlRegisterWrite();
+    prepareCtrlRegisterWrite();
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::buildNormalDeviceCommand(
-		DeviceCommandId_t *id) {
-	// Data/config register will be read in an alternating manner.
-	if(communicationStep == CommunicationStep::DATA) {
-		*id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
-		communicationStep = CommunicationStep::TEMPERATURE;
-		return buildCommandFromCommand(*id, NULL, 0);
-	}
-	else {
-		*id = MGMLIS3MDL::READ_TEMPERATURE;
-		communicationStep = CommunicationStep::DATA;
-		return buildCommandFromCommand(*id, NULL, 0);
-	}
+        DeviceCommandId_t *id) {
+    // Data/config register will be read in an alternating manner.
+    if(communicationStep == CommunicationStep::DATA) {
+        *id = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+        communicationStep = CommunicationStep::TEMPERATURE;
+        return buildCommandFromCommand(*id, NULL, 0);
+    }
+    else {
+        *id = MGMLIS3MDL::READ_TEMPERATURE;
+        communicationStep = CommunicationStep::DATA;
+        return buildCommandFromCommand(*id, NULL, 0);
+    }
 
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::buildCommandFromCommand(
-		DeviceCommandId_t deviceCommand, const uint8_t *commandData,
-		size_t commandDataLen) {
-	switch(deviceCommand) {
-	case(MGMLIS3MDL::READ_CONFIG_AND_DATA): {
-		std::memset(commandBuffer, 0, sizeof(commandBuffer));
-		commandBuffer[0] = readCommand(MGMLIS3MDL::CTRL_REG1, true);
+        DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+        size_t commandDataLen) {
+    switch(deviceCommand) {
+    case(MGMLIS3MDL::READ_CONFIG_AND_DATA): {
+        std::memset(commandBuffer, 0, sizeof(commandBuffer));
+        commandBuffer[0] = readCommand(MGMLIS3MDL::CTRL_REG1, true);
 
-		rawPacket = commandBuffer;
-		rawPacketLen = MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1;
-		return RETURN_OK;
-	}
-	case(MGMLIS3MDL::READ_TEMPERATURE): {
-		std::memset(commandBuffer, 0, 3);
-		commandBuffer[0] = readCommand(MGMLIS3MDL::TEMP_LOWBYTE, true);
+        rawPacket = commandBuffer;
+        rawPacketLen = MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1;
+        return RETURN_OK;
+    }
+    case(MGMLIS3MDL::READ_TEMPERATURE): {
+        std::memset(commandBuffer, 0, 3);
+        commandBuffer[0] = readCommand(MGMLIS3MDL::TEMP_LOWBYTE, true);
 
-		rawPacket = commandBuffer;
-		rawPacketLen = 3;
-		return RETURN_OK;
-	}
-	case(MGMLIS3MDL::IDENTIFY_DEVICE): {
-		return identifyDevice();
-	}
-	case(MGMLIS3MDL::TEMP_SENSOR_ENABLE): {
-		return enableTemperatureSensor(commandData, commandDataLen);
-	}
-	case(MGMLIS3MDL::SETUP_MGM): {
-		setupMgm();
-		return HasReturnvaluesIF::RETURN_OK;
-	}
-	case(MGMLIS3MDL::ACCURACY_OP_MODE_SET): {
-		return setOperatingMode(commandData, commandDataLen);
-	}
-	default:
-		return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
-	}
-	return HasReturnvaluesIF::RETURN_FAILED;
+        rawPacket = commandBuffer;
+        rawPacketLen = 3;
+        return RETURN_OK;
+    }
+    case(MGMLIS3MDL::IDENTIFY_DEVICE): {
+        return identifyDevice();
+    }
+    case(MGMLIS3MDL::TEMP_SENSOR_ENABLE): {
+        return enableTemperatureSensor(commandData, commandDataLen);
+    }
+    case(MGMLIS3MDL::SETUP_MGM): {
+        setupMgm();
+        return HasReturnvaluesIF::RETURN_OK;
+    }
+    case(MGMLIS3MDL::ACCURACY_OP_MODE_SET): {
+        return setOperatingMode(commandData, commandDataLen);
+    }
+    default:
+        return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
+    }
+    return HasReturnvaluesIF::RETURN_FAILED;
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::identifyDevice() {
-	uint32_t size = 2;
-	commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR);
-	commandBuffer[1] = 0x00;
+    uint32_t size = 2;
+    commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR);
+    commandBuffer[1] = 0x00;
 
-	rawPacket = commandBuffer;
-	rawPacketLen = size;
+    rawPacket = commandBuffer;
+    rawPacketLen = size;
 
-	return RETURN_OK;
+    return RETURN_OK;
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::scanForReply(const uint8_t *start,
-		size_t len, DeviceCommandId_t *foundId, size_t *foundLen) {
-	*foundLen = len;
-	if (len == MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1) {
-		*foundLen = len;
-		*foundId = MGMLIS3MDL::READ_CONFIG_AND_DATA;
-		// Check validity by checking config registers
-		if (start[1] != registers[0] or start[2] != registers[1] or
-				start[3] != registers[2] or start[4] != registers[3] or
-				start[5] != registers[4]) {
-			return DeviceHandlerIF::INVALID_DATA;
-		}
-		if(mode == _MODE_START_UP) {
-			commandExecuted = true;
-		}
+        size_t len, DeviceCommandId_t *foundId, size_t *foundLen) {
+    *foundLen = len;
+    if (len == MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1) {
+        *foundLen = len;
+        *foundId = MGMLIS3MDL::READ_CONFIG_AND_DATA;
+        // Check validity by checking config registers
+        if (start[1] != registers[0] or start[2] != registers[1] or
+                start[3] != registers[2] or start[4] != registers[3] or
+                start[5] != registers[4]) {
+#if OBSW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+            sif::warning << "MGMHandlerLIS3MDL::scanForReply: Invalid registers!" << std::endl;
+#else
+            sif::printWarning("MGMHandlerLIS3MDL::scanForReply: Invalid registers!\n");
+#endif
+#endif
+            return DeviceHandlerIF::INVALID_DATA;
+        }
+        if(mode == _MODE_START_UP) {
+            commandExecuted = true;
+        }
 
-	}
-	else if(len == MGMLIS3MDL::TEMPERATURE_REPLY_LEN) {
-		*foundLen = len;
-		*foundId = MGMLIS3MDL::READ_TEMPERATURE;
-	}
-	else if (len == MGMLIS3MDL::SETUP_REPLY_LEN) {
-		*foundLen = len;
-		*foundId = MGMLIS3MDL::SETUP_MGM;
-	}
-	else if (len == SINGLE_COMMAND_ANSWER_LEN) {
-		*foundLen = len;
-		*foundId = getPendingCommand();
-	}
-	else {
-		return DeviceHandlerIF::INVALID_DATA;
-	}
+    }
+    else if(len == MGMLIS3MDL::TEMPERATURE_REPLY_LEN) {
+        *foundLen = len;
+        *foundId = MGMLIS3MDL::READ_TEMPERATURE;
+    }
+    else if (len == MGMLIS3MDL::SETUP_REPLY_LEN) {
+        *foundLen = len;
+        *foundId = MGMLIS3MDL::SETUP_MGM;
+    }
+    else if (len == SINGLE_COMMAND_ANSWER_LEN) {
+        *foundLen = len;
+        *foundId = getPendingCommand();
+        if(*foundId == MGMLIS3MDL::IDENTIFY_DEVICE) {
+            if(start[1] != MGMLIS3MDL::DEVICE_ID) {
+#if OBSW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+                sif::warning << "MGMHandlerLIS3MDL::scanForReply: Invalid registers!" << std::endl;
+#else
+                sif::printWarning("MGMHandlerLIS3MDL::scanForReply: Invalid registers!\n");
+#endif
+#endif
+                return DeviceHandlerIF::INVALID_DATA;
+            }
 
-	// Data with SPI Interface has always this answer
-	if (start[0] == 0b11111111) {
-		return RETURN_OK;
-	}
-	else {
-		return DeviceHandlerIF::INVALID_DATA;
-	}
+            if(mode == _MODE_START_UP) {
+                commandExecuted = true;
+            }
+        }
+    }
+    else {
+        return DeviceHandlerIF::INVALID_DATA;
+    }
+
+    /* Data with SPI Interface always has this answer */
+    if (start[0] == 0b11111111) {
+        return RETURN_OK;
+    }
+    else {
+        return DeviceHandlerIF::INVALID_DATA;
+    }
 
 }
 ReturnValue_t MGMHandlerLIS3MDL::interpretDeviceReply(DeviceCommandId_t id,
-		const uint8_t *packet) {
+        const uint8_t *packet) {
 
-	switch (id) {
-	case MGMLIS3MDL::IDENTIFY_DEVICE: {
-		break;
-	}
-	case MGMLIS3MDL::SETUP_MGM: {
-		break;
-	}
-	case MGMLIS3MDL::READ_CONFIG_AND_DATA: {
-		// TODO: Store configuration and sensor values in new local datasets.
+    switch (id) {
+    case MGMLIS3MDL::IDENTIFY_DEVICE: {
+        break;
+    }
+    case MGMLIS3MDL::SETUP_MGM: {
+        break;
+    }
+    case MGMLIS3MDL::READ_CONFIG_AND_DATA: {
+        // TODO: Store configuration in new local datasets.
 
-		uint8_t scale = getFullScale(registers[2]);
-		float sensitivityFactor = getSensitivityFactor(scale);
+        uint8_t scale = getFullScale(registers[2]);
+        float sensitivityFactor = getSensitivityFactor(scale);
 
-		int16_t mgmMeasurementRawX = packet[MGMLIS3MDL::X_HIGHBYTE_IDX] << 8
-				| packet[MGMLIS3MDL::X_LOWBYTE_IDX] ;
-		int16_t mgmMeasurementRawY = packet[MGMLIS3MDL::Y_HIGHBYTE_IDX] << 8
-				| packet[MGMLIS3MDL::Y_LOWBYTE_IDX] ;
-		int16_t mgmMeasurementRawZ = packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8
-				| packet[MGMLIS3MDL::Z_LOWBYTE_IDX] ;
+        int16_t mgmMeasurementRawX = packet[MGMLIS3MDL::X_HIGHBYTE_IDX] << 8
+                | packet[MGMLIS3MDL::X_LOWBYTE_IDX] ;
+        int16_t mgmMeasurementRawY = packet[MGMLIS3MDL::Y_HIGHBYTE_IDX] << 8
+                | packet[MGMLIS3MDL::Y_LOWBYTE_IDX] ;
+        int16_t mgmMeasurementRawZ = packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8
+                | packet[MGMLIS3MDL::Z_LOWBYTE_IDX] ;
 
-		// Target value in microtesla
-		float mgmX = static_cast<float>(mgmMeasurementRawX) * sensitivityFactor
-				*  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
-		float mgmY = static_cast<float>(mgmMeasurementRawY) * sensitivityFactor
-				*  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
-		float mgmZ = static_cast<float>(mgmMeasurementRawZ) * sensitivityFactor
-				*  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+        /* Target value in microtesla */
+        float mgmX = static_cast<float>(mgmMeasurementRawX) * sensitivityFactor
+                *  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+        float mgmY = static_cast<float>(mgmMeasurementRawY) * sensitivityFactor
+                *  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
+        float mgmZ = static_cast<float>(mgmMeasurementRawZ) * sensitivityFactor
+                *  MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR;
 
 #if OBSW_VERBOSE_LEVEL >= 1
-		if(debugDivider->checkAndIncrement()) {
-			sif::info << "MGMHandlerLIS3: Magnetic field strength in"
-					" microtesla:" << std::endl;
-			// Set terminal to utf-8 if there is an issue with micro printout.
-			sif::info << "X: " << mgmX << " \xC2\xB5T" << std::endl;
-			sif::info << "Y: " << mgmY << " \xC2\xB5T" << std::endl;
-			sif::info << "Z: " << mgmZ << " \xC2\xB5T" << std::endl;
-		}
+        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;
+            sif::info << "X: " << mgmX << " \xC2\xB5T" << std::endl;
+            sif::info << "Y: " << mgmY << " \xC2\xB5T" << std::endl;
+            sif::info << "Z: " << mgmZ << " \xC2\xB5T" << std::endl;
+#else
+            sif::printInfo("MGMHandlerLIS3: Magnetic field strength in microtesla:\n");
+            sif::printInfo("X: %f " "\xC2\xB5" "T\n", mgmX);
+            sif::printInfo("Y: %f " "\xC2\xB5" "T\n", mgmY);
+            sif::printInfo("Z: %f " "\xC2\xB5" "T\n", mgmZ);
 #endif
-		ReturnValue_t result = dataset.read();
-		if(result == HasReturnvaluesIF::RETURN_OK) {
-		    dataset.fieldStrengthX = mgmX;
-		    dataset.fieldStrengthY = mgmY;
-		    dataset.fieldStrengthZ = mgmZ;
-		    dataset.setValidity(true, true);
-		    dataset.commit();
-		}
-		break;
-	}
+        }
+#endif
+        PoolReadHelper readHelper(&dataset);
+        if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) {
+            dataset.fieldStrengthX = mgmX;
+            dataset.fieldStrengthY = mgmY;
+            dataset.fieldStrengthZ = mgmZ;
+            dataset.setValidity(true, true);
+        }
+        break;
+    }
 
-	case MGMLIS3MDL::READ_TEMPERATURE: {
-		int16_t tempValueRaw = packet[2] << 8 | packet[1];
-		float tempValue = 25.0 + ((static_cast<float>(tempValueRaw)) / 8.0);
+    case MGMLIS3MDL::READ_TEMPERATURE: {
+        int16_t tempValueRaw = packet[2] << 8 | packet[1];
+        float tempValue = 25.0 + ((static_cast<float>(tempValueRaw)) / 8.0);
 #if OBSW_VERBOSE_LEVEL >= 1
-		if(debugDivider->check()) {
-			// Set terminal to utf-8 if there is an issue with micro printout.
-			sif::info << "MGMHandlerLIS3: Temperature: " << tempValue<< " °C"
-					<< std::endl;
-		}
+        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 << " \xC2\xB0" << "C" <<
+                    std::endl;
+#else
+            sif::printInfo("MGMHandlerLIS3: Temperature: %f" "\xC2\xB0" "C\n");
+#endif
+        }
 #endif
         ReturnValue_t result = dataset.read();
         if(result == HasReturnvaluesIF::RETURN_OK) {
             dataset.temperature = tempValue;
             dataset.commit();
         }
-		break;
-	}
+        break;
+    }
 
-	default: {
-		return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
-	}
+    default: {
+        return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY;
+    }
 
-	}
-	return RETURN_OK;
+    }
+    return RETURN_OK;
 }
 
 uint8_t MGMHandlerLIS3MDL::getFullScale(uint8_t ctrlRegister2) {
-	bool FS0 = false;
-	bool FS1 = false;
-	if ((ctrlRegister2 >> 5) == 1)
-		FS0 = true;
-	if ((ctrlRegister2 >> 6) == 1)
-		FS1 = true;
-	if ((FS0 == true) && (FS1 == true))
-		return 16;
-	else if ((FS0 == false) && (FS1 == true))
-		return 12;
-	else if ((FS0 == true) && (FS1 == false))
-		return 8;
-	else
-		return 4;
+    bool FS0 = false;
+    bool FS1 = false;
+    if ((ctrlRegister2 >> 5) == 1)
+        FS0 = true;
+    if ((ctrlRegister2 >> 6) == 1)
+        FS1 = true;
+    if ((FS0 == true) && (FS1 == true))
+        return 16;
+    else if ((FS0 == false) && (FS1 == true))
+        return 12;
+    else if ((FS0 == true) && (FS1 == false))
+        return 8;
+    else
+        return 4;
 }
 
 float MGMHandlerLIS3MDL::getSensitivityFactor(uint8_t scale) {
-	return (float) scale / (INT16_MAX);
+    return (float) scale / (INT16_MAX);
 }
 
 
 ReturnValue_t MGMHandlerLIS3MDL::enableTemperatureSensor(
-		const uint8_t *commandData, size_t commandDataLen) {
-	triggerEvent(CHANGE_OF_SETUP_PARAMETER);
-	uint32_t size = 2;
-	commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1);
-	if (commandDataLen > 1) {
-		return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
-	}
-	switch (*commandData) {
-	case (MGMLIS3MDL::ON): {
-		commandBuffer[1] = registers[0] | (1 << 7);
-		break;
-	}
-	case (MGMLIS3MDL::OFF): {
-		commandBuffer[1] = registers[0] & ~(1 << 7);
-		break;
-	}
-	default:
-		return INVALID_COMMAND_PARAMETER;
-	}
-	registers[0] = commandBuffer[1];
+        const uint8_t *commandData, size_t commandDataLen) {
+    triggerEvent(CHANGE_OF_SETUP_PARAMETER);
+    uint32_t size = 2;
+    commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1);
+    if (commandDataLen > 1) {
+        return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
+    }
+    switch (*commandData) {
+    case (MGMLIS3MDL::ON): {
+        commandBuffer[1] = registers[0] | (1 << 7);
+        break;
+    }
+    case (MGMLIS3MDL::OFF): {
+        commandBuffer[1] = registers[0] & ~(1 << 7);
+        break;
+    }
+    default:
+        return INVALID_COMMAND_PARAMETER;
+    }
+    registers[0] = commandBuffer[1];
 
-	rawPacket = commandBuffer;
-	rawPacketLen = size;
+    rawPacket = commandBuffer;
+    rawPacketLen = size;
 
-	return RETURN_OK;
+    return RETURN_OK;
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::setOperatingMode(const uint8_t *commandData,
-		size_t commandDataLen) {
-	triggerEvent(CHANGE_OF_SETUP_PARAMETER);
-	if (commandDataLen != 1) {
-		return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
-	}
+        size_t commandDataLen) {
+    triggerEvent(CHANGE_OF_SETUP_PARAMETER);
+    if (commandDataLen != 1) {
+        return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS;
+    }
 
-	switch (commandData[0]) {
-	case MGMLIS3MDL::LOW:
-		registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) & (~(1 << MGMLIS3MDL::OM0));
-		registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) & (~(1 << MGMLIS3MDL::OMZ0));
-		break;
-	case MGMLIS3MDL::MEDIUM:
-		registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) | (1 << MGMLIS3MDL::OM0);
-		registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) | (1 << MGMLIS3MDL::OMZ0);
-		break;
+    switch (commandData[0]) {
+    case MGMLIS3MDL::LOW:
+        registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) & (~(1 << MGMLIS3MDL::OM0));
+        registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) & (~(1 << MGMLIS3MDL::OMZ0));
+        break;
+    case MGMLIS3MDL::MEDIUM:
+        registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) | (1 << MGMLIS3MDL::OM0);
+        registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) | (1 << MGMLIS3MDL::OMZ0);
+        break;
 
-	case MGMLIS3MDL::HIGH:
-		registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) & (~(1 << MGMLIS3MDL::OM0));
-		registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) & (~(1 << MGMLIS3MDL::OMZ0));
-		break;
+    case MGMLIS3MDL::HIGH:
+        registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) & (~(1 << MGMLIS3MDL::OM0));
+        registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) & (~(1 << MGMLIS3MDL::OMZ0));
+        break;
 
-	case MGMLIS3MDL::ULTRA:
-		registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) | (1 << MGMLIS3MDL::OM0);
-		registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) | (1 << MGMLIS3MDL::OMZ0);
-		break;
-	default:
-		break;
-	}
+    case MGMLIS3MDL::ULTRA:
+        registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) | (1 << MGMLIS3MDL::OM0);
+        registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) | (1 << MGMLIS3MDL::OMZ0);
+        break;
+    default:
+        break;
+    }
 
-	return prepareCtrlRegisterWrite();
+    return prepareCtrlRegisterWrite();
 }
 
 void MGMHandlerLIS3MDL::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);
-	insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1);
-	insertInCommandAndReplyMap(MGMLIS3MDL::TEMP_SENSOR_ENABLE, 1);
-	insertInCommandAndReplyMap(MGMLIS3MDL::ACCURACY_OP_MODE_SET, 1);
+    /*
+     * 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);
+    insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1);
+    insertInCommandAndReplyMap(MGMLIS3MDL::TEMP_SENSOR_ENABLE, 1);
+    insertInCommandAndReplyMap(MGMLIS3MDL::ACCURACY_OP_MODE_SET, 1);
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::prepareCtrlRegisterWrite() {
+    commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true);
 
-	commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true);
+    for (size_t i = 0; i < MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) {
+        commandBuffer[i + 1] = registers[i];
+    }
+    rawPacket = commandBuffer;
+    rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1;
 
-	for (size_t i = 0; i < MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) {
-		commandBuffer[i + 1] = registers[i];
-	}
-	rawPacket = commandBuffer;
-	rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1;
-
-	// We dont have to check if this is working because we just did it
-	return RETURN_OK;
+    /* We dont have to check if this is working because we just did it */
+    return RETURN_OK;
 }
 
 void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
@@ -415,22 +459,25 @@ void MGMHandlerLIS3MDL::doTransition(Mode_t modeFrom, Submode_t subModeFrom) {
 }
 
 uint32_t MGMHandlerLIS3MDL::getTransitionDelayMs(Mode_t from, Mode_t to) {
-	return 5000;
+    return 30000;
 }
 
 void MGMHandlerLIS3MDL::modeChanged(void) {
-	internalState = InternalState::STATE_NONE;
+    internalState = InternalState::STATE_NONE;
 }
 
 ReturnValue_t MGMHandlerLIS3MDL::initializeLocalDataPool(
-		localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
-	localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_X,
-			new PoolEntry<float>({0.0}));
-	localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Y,
-			new PoolEntry<float>({0.0}));
-	localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Z,
-			new PoolEntry<float>({0.0}));
-	localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS,
-			new PoolEntry<float>({0.0}));
-	return HasReturnvaluesIF::RETURN_OK;
+        localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) {
+    localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_X,
+            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Y,
+            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Z,
+            new PoolEntry<float>({0.0}));
+    localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS,
+            new PoolEntry<float>({0.0}));
+    return HasReturnvaluesIF::RETURN_OK;
+}
+
+void MGMHandlerLIS3MDL::performOperationHook() {
 }

mission/devices/MGMHandlerLIS3MDL.h

@@ -18,149 +18,151 @@
  */
 class MGMHandlerLIS3MDL: public DeviceHandlerBase {
 public:
-	enum class CommunicationStep {
-		DATA,
-		TEMPERATURE
-	};
+    enum class CommunicationStep {
+        DATA,
+        TEMPERATURE
+    };
 
-	static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL;
-	static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL;
-	//Notifies a command to change the setup parameters
-	static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW);
+    static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL;
+    static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL;
+    //Notifies a command to change the setup parameters
+    static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW);
 
-	MGMHandlerLIS3MDL(uint32_t objectId, object_id_t deviceCommunication,
-			CookieIF* comCookie);
-	virtual ~MGMHandlerLIS3MDL();
+    MGMHandlerLIS3MDL(uint32_t objectId, object_id_t deviceCommunication,
+            CookieIF* comCookie);
+    virtual ~MGMHandlerLIS3MDL();
 
 protected:
 
-	/** DeviceHandlerBase overrides */
-	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;
-	ReturnValue_t buildCommandFromCommand(
-			DeviceCommandId_t deviceCommand, const uint8_t *commandData,
-			size_t commandDataLen) override;
-	ReturnValue_t buildTransitionDeviceCommand(
-			DeviceCommandId_t *id) override;
-	ReturnValue_t buildNormalDeviceCommand(
-			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,
-			const uint8_t *packet) override;
-	void fillCommandAndReplyMap() override;
-	void modeChanged(void) override;
-	ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
-			LocalDataPoolManager &poolManager) override;
+    /** DeviceHandlerBase overrides */
+    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;
+    ReturnValue_t buildCommandFromCommand(
+            DeviceCommandId_t deviceCommand, const uint8_t *commandData,
+            size_t commandDataLen) override;
+    ReturnValue_t buildTransitionDeviceCommand(
+            DeviceCommandId_t *id) override;
+    ReturnValue_t buildNormalDeviceCommand(
+            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,
+            const uint8_t *packet) override;
+    void fillCommandAndReplyMap() override;
+    void modeChanged(void) override;
+    ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap,
+            LocalDataPoolManager &poolManager) override;
 
 private:
-	MGMLIS3MDL::MgmPrimaryDataset dataset;
+    MGMLIS3MDL::MgmPrimaryDataset dataset;
 
-	/*------------------------------------------------------------------------*/
-	/* Device specific commands and variables */
-	/*------------------------------------------------------------------------*/
-	/**
-	 * Sets the read bit for the command
-	 * @param single command to set the read-bit at
-	 * @param boolean to select a continuous read bit, default = false
-	 */
-	uint8_t readCommand(uint8_t command, bool continuousCom = false);
+    /*------------------------------------------------------------------------*/
+    /* Device specific commands and variables */
+    /*------------------------------------------------------------------------*/
+    /**
+     * Sets the read bit for the command
+     * @param single command to set the read-bit at
+     * @param boolean to select a continuous read bit, default = false
+     */
+    uint8_t readCommand(uint8_t command, bool continuousCom = false);
 
-	/**
-	 * Sets the write bit for the command
-	 * @param single command to set the write-bit at
-	 * @param boolean to select a continuous write bit, default = false
-	 */
-	uint8_t writeCommand(uint8_t command, bool continuousCom = false);
+    /**
+     * Sets the write bit for the command
+     * @param single command to set the write-bit at
+     * @param boolean to select a continuous write bit, default = false
+     */
+    uint8_t writeCommand(uint8_t command, bool continuousCom = false);
 
-	/**
-	 * This Method gets the full scale for the measurement range
-	 * e.g.: +- 4 gauss. See p.25 datasheet.
-	 *  @return The ReturnValue does not contain the sign of the value
-	 */
-	uint8_t getFullScale(uint8_t ctrlReg2);
+    /**
+     * This Method gets the full scale for the measurement range
+     * e.g.: +- 4 gauss. See p.25 datasheet.
+     *  @return The ReturnValue does not contain the sign of the value
+     */
+    uint8_t getFullScale(uint8_t ctrlReg2);
 
-	/**
-	 * The 16 bit value needs to be divided by the full range of a 16bit value
-	 * and then multiplied with the current scale of the MGM.
-	 * This factor returns the factor required to achieve this with
-	 * one multiplication.
-	 *
-	 * @param scale is the return value of the getFulscale Method
-	 * @return Multiplication factor to get the sensor value from raw data.
-	 */
-	float getSensitivityFactor(uint8_t scale);
+    /**
+     * The 16 bit value needs to be divided by the full range of a 16bit value
+     * and then multiplied with the current scale of the MGM.
+     * This factor returns the factor required to achieve this with
+     * one multiplication.
+     *
+     * @param scale is the return value of the getFulscale Method
+     * @return Multiplication factor to get the sensor value from raw data.
+     */
+    float getSensitivityFactor(uint8_t scale);
 
-	/**
-	 * This Command detects the device ID
-	 */
-	ReturnValue_t identifyDevice();
+    /**
+     * This Command detects the device ID
+     */
+    ReturnValue_t identifyDevice();
 
-	virtual void setupMgm();
+    virtual void setupMgm();
 
-	/*------------------------------------------------------------------------*/
-	/* Non normal commands */
-	/*------------------------------------------------------------------------*/
-	/**
-	 * Enables/Disables the integrated Temperaturesensor
-	 * @param commandData On or Off
-	 * @param length of the commandData: has to be 1
-	 */
-	virtual ReturnValue_t enableTemperatureSensor(const uint8_t *commandData,
-			size_t commandDataLen);
+    /*------------------------------------------------------------------------*/
+    /* Non normal commands */
+    /*------------------------------------------------------------------------*/
+    /**
+     * Enables/Disables the integrated Temperaturesensor
+     * @param commandData On or Off
+     * @param length of the commandData: has to be 1
+     */
+    virtual ReturnValue_t enableTemperatureSensor(const uint8_t *commandData,
+            size_t commandDataLen);
 
-	/**
-	 * Sets the accuracy of the measurement of the axis. The noise is changing.
-	 * @param commandData LOW, MEDIUM, HIGH, ULTRA
-	 * @param length of the command, has to be 1
-	 */
-	virtual ReturnValue_t setOperatingMode(const uint8_t *commandData,
-			size_t commandDataLen);
+    /**
+     * Sets the accuracy of the measurement of the axis. The noise is changing.
+     * @param commandData LOW, MEDIUM, HIGH, ULTRA
+     * @param length of the command, has to be 1
+     */
+    virtual ReturnValue_t setOperatingMode(const uint8_t *commandData,
+            size_t commandDataLen);
 
 
-	//Length a sindgle command SPI answer
-	static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2;
+    //Length a sindgle command SPI answer
+    static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2;
 
-	//Single SPIcommand 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
-	uint8_t commandBuffer[MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1];
+    //Single SPIcommand 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
+    uint8_t commandBuffer[MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1];
 
-	/**
-	 * We want to save the registers we set, so we dont have to read the
-	 * registers when we want to change something.
-	 * --> everytime we change set a register we have to save it
-	 */
-	uint8_t registers[MGMLIS3MDL::NR_OF_CTRL_REGISTERS];
+    /**
+     * We want to save the registers we set, so we dont have to read the
+     * registers when we want to change something.
+     * --> everytime we change set a register we have to save it
+     */
+    uint8_t registers[MGMLIS3MDL::NR_OF_CTRL_REGISTERS];
 
-	uint8_t statusRegister = 0;
+    uint8_t statusRegister = 0;
 
-	/**
-	 * We always update all registers together, so this method updates
-	 * the rawpacket and rawpacketLen, so we just manipulate the local
-	 * saved register
-	 *
-	 */
-	ReturnValue_t prepareCtrlRegisterWrite();
+    /**
+     * We always update all registers together, so this method updates
+     * the rawpacket and rawpacketLen, so we just manipulate the local
+     * saved register
+     *
+     */
+    ReturnValue_t prepareCtrlRegisterWrite();
 
-	enum class InternalState {
-		STATE_NONE,
-		STATE_FIRST_CONTACT,
-		STATE_SETUP,
-		STATE_CHECK_REGISTERS,
-		STATE_NORMAL
-	};
+    enum class InternalState {
+        STATE_NONE,
+        STATE_FIRST_CONTACT,
+        STATE_SETUP,
+        STATE_CHECK_REGISTERS,
+        STATE_NORMAL
+    };
 
-	InternalState internalState = InternalState::STATE_NONE;
-	CommunicationStep communicationStep = CommunicationStep::DATA;
-	bool commandExecuted = false;
+    InternalState internalState = InternalState::STATE_NONE;
+    CommunicationStep communicationStep = CommunicationStep::DATA;
+    bool commandExecuted = false;
 
 #if OBSW_VERBOSE_LEVEL >= 1
-	PeriodicOperationDivider* debugDivider;
+    PeriodicOperationDivider* debugDivider;
 #endif
 
+    void performOperationHook() override;
+
 };
 
 #endif /* MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ */

mission/devices/PCDUHandler.cpp

@@ -1,9 +1,12 @@
 #include "PCDUHandler.h"
-#include <fsfwconfig/objects/systemObjectList.h>
+#include <OBSWConfig.h>
+#include <objects/systemObjectList.h>
+
 #include <mission/devices/devicedefinitions/GomSpacePackets.h>
+
 #include <fsfw/ipc/QueueFactory.h>
 #include <fsfw/housekeeping/HousekeepingSnapshot.h>
-#include <fsfwconfig/OBSWConfig.h>
+
 
 PCDUHandler::PCDUHandler(object_id_t setObjectId, size_t cmdQueueSize) :
         SystemObject(setObjectId), poolManager(this, nullptr), pdu2HkTableDataset(this), pdu1HkTableDataset(

mission/devices/PCDUHandler.h

@@ -1,6 +1,7 @@
 #ifndef MISSION_DEVICES_PCDUHANDLER_H_
 #define MISSION_DEVICES_PCDUHANDLER_H_
 
+#include <devices/powerSwitcherList.h>
 #include <mission/devices/devicedefinitions/GomspaceDefinitions.h>
 #include <mission/devices/GomspaceDeviceHandler.h>
 #include <fsfw/timemanager/CCSDSTime.h>
@@ -9,7 +10,7 @@
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/tasks/ExecutableObjectIF.h>
 #include <fsfw/objectmanager/SystemObject.h>
-#include <fsfwconfig/devices/powerSwitcherList.h>
+
 
 /**
  * @brief	The PCDUHandler provides a compact interface to handle all devices related to the

mission/devices/PDU2Handler.cpp

@@ -1,6 +1,6 @@
 #include "PDU2Handler.h"
 #include <mission/devices/devicedefinitions/GomSpacePackets.h>
-#include <fsfwconfig/OBSWConfig.h>
+#include <OBSWConfig.h>
 
 PDU2Handler::PDU2Handler(object_id_t objectId, object_id_t comIF, CookieIF * comCookie) :
         GomspaceDeviceHandler(objectId, comIF, comCookie, PDU::MAX_CONFIGTABLE_ADDRESS,

mission/devices/Tmp1075Handler.cpp

@@ -1,6 +1,6 @@
 #include <mission/devices/Tmp1075Handler.h>
 #include <mission/devices/devicedefinitions/Tmp1075Definitions.h>
-#include <fsfwconfig/OBSWConfig.h>
+#include <OBSWConfig.h>
 
 Tmp1075Handler::Tmp1075Handler(object_id_t objectId, object_id_t comIF,
 		CookieIF * comCookie) :

mission/devices/devicedefinitions/MGMHandlerLIS3Definitions.h

@@ -24,11 +24,11 @@ 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
+/* Number of all control registers */
 static const uint8_t NR_OF_CTRL_REGISTERS = 5;
-//Number of registers in the MGM
+/* Number of registers in the MGM */
 static const uint8_t NR_OF_REGISTERS = 19;
-//Total number of adresses for all registers
+/* 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;
@@ -37,47 +37,47 @@ static const uint8_t SETUP_REPLY_LEN = 6;
 /*------------------------------------------------------------------------*/
 /* Register adresses */
 /*------------------------------------------------------------------------*/
-// Register adress returns identifier of device with default 0b00111101
+/* 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
+/* Register adress to access register 1 */
 static const uint8_t CTRL_REG1 = 0b00100000;
-//Register adress to access register 2
+/* Register adress to access register 2 */
 static const uint8_t CTRL_REG2 = 0b00100001;
-//Register adress to access register 3
+/* Register adress to access register 3 */
 static const uint8_t CTRL_REG3 = 0b00100010;
-//Register adress to access register 4
+/* Register adress to access register 4 */
 static const uint8_t CTRL_REG4 = 0b00100011;
-//Register adress to access register 5
+/* Register adress to access register 5 */
 static const uint8_t CTRL_REG5 = 0b00100100;
 
-//Register adress to access status register
+/* 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
+/* 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
+/* 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
+/* 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
+/* 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
+/* 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
+/* 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
+/* 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
+/* Register adress to access high byte of temperature sensor */
 static const uint8_t TEMP_HIGHBYTE = 0b00101111;
 
 /*------------------------------------------------------------------------*/

mission/utility/InitMission.h

@@ -0,0 +1,22 @@
+#ifndef MISSION_UTILITY_INITMISSION_H_
+#define MISSION_UTILITY_INITMISSION_H_
+
+#include <fsfw/objectmanager/SystemObjectIF.h>
+#include <fsfw/serviceinterface/ServiceInterface.h>
+
+namespace initmission {
+
+void printAddObjectError(const char* name, object_id_t objectId) {
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+    sif::error << "InitMission::printAddError: Adding object " << name << " with object ID 0x"
+            << std::hex << std::setfill('0') << std::setw(8) << objectId
+            << " failed!" << std::dec << std::endl;
+#else
+    sif::printError("InitMission::printAddError: Adding object %s with object ID 0x%08x failed!\n" ,
+            name, objectId);
+#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */
+}
+
+}
+
+#endif /* MISSION_UTILITY_INITMISSION_H_ */

test/testtasks/TestTask.cpp

@@ -1,4 +1,6 @@
-#include <test/testtasks/TestTask.h>
+#include "TestTask.h"
+#include <OBSWConfig.h>
+
 #include <fsfw/objectmanager/frameworkObjects.h>
 #include <fsfw/serviceinterface/ServiceInterfaceStream.h>
 #include <fsfw/objectmanager/ObjectManagerIF.h>
@@ -8,65 +10,101 @@
 #include <array>
 #include <cstring>
 
-bool TestTask::oneShotAction = true;
-MutexIF* TestTask::testLock = nullptr;
 
 TestTask::TestTask(object_id_t objectId_):
-	SystemObject(objectId_), testMode(testModes::A) {
-	if(testLock == nullptr) {
-		testLock = MutexFactory::instance()->createMutex();
-	}
-	IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
+SystemObject(objectId_), testMode(testModes::A) {
+    IPCStore = objectManager->get<StorageManagerIF>(objects::IPC_STORE);
 }
 
 TestTask::~TestTask() {
 }
 
 ReturnValue_t TestTask::performOperation(uint8_t operationCode) {
-	ReturnValue_t result = RETURN_OK;
-	sif::info << "Hallo EIVE!" << std::endl;
-	testLock ->lockMutex(MutexIF::TimeoutType::WAITING, 20);
-	if(oneShotAction) {
-		// Add code here which should only be run once
-		performOneShotAction();
-		oneShotAction = false;
-	}
-	testLock->unlockMutex();
+    ReturnValue_t result = RETURN_OK;
 
-	// Add code here which should only be run once per performOperation
-	performPeriodicAction();
+    if(oneShotAction) {
+        /* Add code here which should only be run once */
+        performOneShotAction();
+        oneShotAction = false;
+    }
 
-	// Add code here which should only be run on alternating cycles.
-	if(testMode == testModes::A) {
-		performActionA();
-		testMode = testModes::B;
-	}
-	else if(testMode == testModes::B) {
-		performActionB();
-		testMode = testModes::A;
-	}
-	return result;
+    /* Add code here which should only be run once per performOperation */
+    performPeriodicAction();
+
+    /* Add code here which should only be run on alternating cycles. */
+    if(testMode == testModes::A) {
+        performActionA();
+        testMode = testModes::B;
+    }
+    else if(testMode == testModes::B) {
+        performActionB();
+        testMode = testModes::A;
+    }
+    return result;
 }
 
+#include <etl/vector.h>
+#include <lwgps/lwgps.h>
+
+/**
+ * @brief           Dummy data from GPS receiver. Will be replaced witgh hyperion data later.
+ */
+const char
+gps_rx_data[] = ""
+        "$GPRMC,183729,A,3907.356,N,12102.482,W,000.0,360.0,080301,015.5,E*6F\r\n"
+        "$GPRMB,A,,,,,,,,,,,,V*71\r\n"
+        "$GPGGA,183730,3907.356,N,12102.482,W,1,05,1.6,646.4,M,-24.1,M,,*75\r\n"
+        "$GPGSA,A,3,02,,,07,,09,24,26,,,,,1.6,1.6,1.0*3D\r\n"
+        "$GPGSV,2,1,08,02,43,088,38,04,42,145,00,05,11,291,00,07,60,043,35*71\r\n"
+        "$GPGSV,2,2,08,08,02,145,00,09,46,303,47,24,16,178,32,26,18,231,43*77\r\n"
+        "$PGRME,22.0,M,52.9,M,51.0,M*14\r\n"
+        "$GPGLL,3907.360,N,12102.481,W,183730,A*33\r\n"
+        "$PGRMZ,2062,f,3*2D\r\n"
+        "$PGRMM,WGS84*06\r\n"
+        "$GPBOD,,T,,M,,*47\r\n"
+        "$GPRTE,1,1,c,0*07\r\n"
+        "$GPRMC,183731,A,3907.482,N,12102.436,W,000.0,360.0,080301,015.5,E*67\r\n"
+        "$GPRMB,A,,,,,,,,,,,,V*71\r\n";
+
 ReturnValue_t TestTask::performOneShotAction() {
-	// Everything here will only be performed once.
+#if OBSW_ADD_TEST_CODE == 1
+    performLwgpsTest();
+#endif
     return HasReturnvaluesIF::RETURN_OK;
 }
 
 
 ReturnValue_t TestTask::performPeriodicAction() {
-	ReturnValue_t result = RETURN_OK;
-	return result;
+    ReturnValue_t result = RETURN_OK;
+    return result;
 }
 
 ReturnValue_t TestTask::performActionA() {
-	ReturnValue_t result = RETURN_OK;
-	// Add periodically executed code here
-	return result;
+    ReturnValue_t result = RETURN_OK;
+    /* Add periodically executed code here */
+    return result;
 }
 
 ReturnValue_t TestTask::performActionB() {
-	ReturnValue_t result = RETURN_OK;
-	// Add periodically executed code here
-	return result;
+    ReturnValue_t result = RETURN_OK;
+    /* Add periodically executed code here */
+    return result;
+}
+
+void TestTask::performLwgpsTest() {
+    /* Everything here will only be performed once. */
+    etl::vector<uint8_t, 30> testVec;
+
+    lwgps_t gpsStruct;
+    sif::info << "Size of GPS struct: " << sizeof(gpsStruct) << std::endl;
+    lwgps_init(&gpsStruct);
+
+    /* Process all input data */
+    lwgps_process(&gpsStruct, gps_rx_data, strlen(gps_rx_data));
+
+    /* Print messages */
+    printf("Valid status: %d\r\n", gpsStruct.is_valid);
+    printf("Latitude: %f degrees\r\n", gpsStruct.latitude);
+    printf("Longitude: %f degrees\r\n", gpsStruct.longitude);
+    printf("Altitude: %f meters\r\n", gpsStruct.altitude);
 }

test/testtasks/TestTask.h

@@ -48,9 +48,10 @@ protected:
 private:
 	// Actually, to be really thread-safe, a mutex should be used as well
 	// Let's keep it simple for now.
-	static bool oneShotAction;
-	static MutexIF* testLock;
+	bool oneShotAction = true;
 	StorageManagerIF* IPCStore;
+
+	void performLwgpsTest();
 };
 
 

thirdparty/libcsp/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.13)
+
+set(LIB_CSP_NAME libcsp)
+
+add_library(${LIB_CSP_NAME})
+
+add_subdirectory(src)
+add_subdirectory(include)
+
+target_include_directories(${LIB_CSP_NAME} PRIVATE
+	${CMAKE_CURRENT_SOURCE_DIR}
+)

thirdparty/libcsp/bindings/python/libcsp/__init__.py

@@ -0,0 +1,6 @@
+import sys
+
+if sys.version_info >= (3, 0):
+    from libcsp_py3 import *
+else:
+    from libcsp_py2 import *

thirdparty/libcsp/doc/example.rst

@@ -0,0 +1,123 @@
+Client and server example
+=========================
+
+The following examples show the initialization of the protocol stack and examples of client/server code.
+
+Initialization Sequence
+-----------------------
+
+This code initializes the CSP buffer system, device drivers and router core. The example uses the CAN interface function csp_can_tx but the initialization is similar for other interfaces. The loopback interface does not require any explicit initialization.
+
+.. code-block:: c
+
+   #include <csp/csp.h>
+   #include <csp/interfaces/csp_if_can.h>
+   
+   /* CAN configuration struct for SocketCAN interface "can0" */
+   struct csp_can_config can_conf = {.ifc = "can0"};
+   
+   /* Init buffer system with 10 packets of maximum 320 bytes each */
+   csp_buffer_init(10, 320);
+   
+   /* Init CSP with address 1 */
+   csp_init(1);
+   
+   /* Init the CAN interface with hardware filtering */
+   csp_can_init(CSP_CAN_MASKED, &can_conf)
+   
+   /* Setup default route to CAN interface */
+   csp_route_set(CSP_DEFAULT_ROUTE, &csp_can_tx, CSP_HOST_MAC);
+   
+   /* Start router task with 500 word stack, OS task priority 1 */
+   csp_route_start_task(500, 1);
+
+Server
+------
+
+This example shows how to create a server task that listens for incoming connections. CSP should be initialized before starting this task. Note the use of `csp_service_handler()` as the default branch in the port switch case. The service handler will automatically reply to ICMP-like requests, such as pings and buffer status requests.
+
+.. code-block:: c
+
+   void csp_task(void *parameters) {
+       /* Create socket without any socket options */
+       csp_socket_t *sock = csp_socket(CSP_SO_NONE);
+   
+       /* Bind all ports to socket */
+       csp_bind(sock, CSP_ANY);
+   
+       /* Create 10 connections backlog queue */
+       csp_listen(sock, 10);
+   
+       /* Pointer to current connection and packet */
+       csp_conn_t *conn;
+       csp_packet_t *packet;
+   
+       /* Process incoming connections */
+       while (1) {
+           /* Wait for connection, 10000 ms timeout */    
+           if ((conn = csp_accept(sock, 10000)) == NULL)
+               continue;
+   
+           /* Read packets. Timout is 1000 ms */
+           while ((packet = csp_read(conn, 1000)) != NULL) {
+               switch (csp_conn_dport(conn)) {
+                   case MY_PORT:
+                       /* Process packet here */
+                   default:
+                       /* Let the service handler reply pings, buffer use, etc. */
+                       csp_service_handler(conn, packet);
+                       break;
+               }
+           }
+   
+           /* Close current connection, and handle next */
+           csp_close(conn);
+       }
+   }
+
+Client
+------
+
+This example shows how to allocate a packet buffer, connect to another host and send the packet. CSP should be initialized before calling this function. RDP, XTEA, HMAC and CRC checksums can be enabled per connection, by setting the connection option to a bitwise OR of any combination of `CSP_O_RDP`, `CSP_O_XTEA`, `CSP_O_HMAC` and `CSP_O_CRC`.
+
+.. code-block:: c
+
+   int send_packet(void) {
+   
+       /* Get packet buffer for data */
+       csp_packet_t *packet = csp_buffer_get(data_size);
+       if (packet == NULL) {
+           /* Could not get buffer element */
+           printf("Failed to get buffer element\\n");
+           return -1;
+       }
+   
+       /* Connect to host HOST, port PORT with regular UDP-like protocol and 1000 ms timeout */
+       csp_conn_t *conn = csp_connect(CSP_PRIO_NORM, HOST, PORT, 1000, CSP_O_NONE);
+       if (conn == NULL) {
+           /* Connect failed */
+           printf("Connection failed\\n");
+           /* Remember to free packet buffer */
+           csp_buffer_free(packet);
+           return -1;
+       }
+   
+       /* Copy message to packet */
+       char *msg = "HELLO";
+       strcpy(packet->data, msg);
+   
+       /* Set packet length */
+       packet->length = strlen(msg);
+   
+       /* Send packet */
+       if (!csp_send(conn, packet, 1000)) {
+           /* Send failed */
+           printf("Send failed\\n");
+           csp_buffer_free(packet);
+       }
+   
+       /* Close connection */
+       csp_close(conn);
+   
+       return 0
+   }

thirdparty/libcsp/doc/history.rst

@@ -0,0 +1,17 @@
+History
+=======
+
+The idea was developed by a group of students from Aalborg University in 2008. In 2009 the main developer started working for GomSpace, and CSP became integrated into the GomSpace products. The protocol is based on a 32-bit header containing both transport, network and MAC-layer information. It's implementation is designed for, but not limited to, embedded systems such as the 8-bit AVR microprocessor and the 32-bit ARM and AVR from Atmel. The implementation is written in C and is currently ported to run on FreeRTOS and POSIX and pthreads based operating systems like Linux and BSD. The three letter acronym CSP was originally an abbreviation for CAN Space Protocol because the first MAC-layer driver was written for CAN-bus. Now the physical layer has extended to include spacelink, I2C and RS232, the name was therefore extended to the more general CubeSat Space Protocol without changing the abbreviation.
+
+Satellites using CSP
+--------------------
+
+This is the known list of satellites or organisations that uses CSP.
+
+ * GomSpace GATOSS GOMX-1
+ * AAUSAT-3
+ * EgyCubeSat
+ * EuroLuna
+ * NUTS
+ * Hawaiian Space Flight Laboratory
+ * GomSpace GOMX-3

thirdparty/libcsp/doc/interfaces.rst

@@ -0,0 +1,95 @@
+CSP Interfaces
+==============
+
+This is an example of how to implement a new layer-2 interface in CSP. The example is going to show how to create a `csp_if_fifo`, using a set of [named pipes](http://en.wikipedia.org/wiki/Named_pipe). The complete interface example code can be found in `examples/fifo.c`. For an example of a fragmenting interface, see the CAN interface in `src/interfaces/csp_if_can.c`.
+
+CSP interfaces are declared in a `csp_iface_t` structure, which sets the interface nexthop function and name. A maximum transmission unit can also be set, which forces CSP to drop outgoing packets above a certain size. The fifo interface is defined as:
+
+.. code-block:: c
+
+   #include <csp/csp.h>
+   #include <csp/csp_interface.h>
+   
+   csp_iface_t csp_if_fifo = {
+       .name = "fifo",
+       .nexthop = csp_fifo_tx,
+       .mtu = BUF_SIZE,
+   };
+
+Outgoing traffic
+----------------
+
+The nexthop function takes a pointer to a CSP packet and a timeout as parameters. All outgoing packets that are routed to the interface are passed to this function:
+
+.. code-block:: c
+
+   int csp_fifo_tx(csp_packet_t *packet, uint32_t timeout) {
+       write(tx_channel, &packet->length, packet->length + sizeof(uint32_t) + sizeof(uint16_t));
+       csp_buffer_free(packet);
+       return 1;
+   }
+
+In the fifo interface, we simply transmit the header, length field and data using a write to the fifo. CSP does not dictate the wire format, so other interfaces may decide to e.g. ignore the length field if the physical layer provides start/stop flags. 
+
+_Important notice: If the transmission succeeds, the interface must free the packet and return 1. If transmission fails, the nexthop function should return 0 and not free the packet, to allow retransmissions by the caller._
+
+Incoming traffic
+----------------
+
+The interface also needs to receive incoming packets and pass it to the CSP protocol stack. In the fifo interface, this is handled by a thread that blocks on the incoming fifo and waits for packets:
+
+.. code-block:: c
+
+   void * fifo_rx(void * parameters) {
+       csp_packet_t *buf = csp_buffer_get(BUF_SIZE);
+       /* Wait for packet on fifo */
+       while (read(rx_channel, &buf->length, BUF_SIZE) > 0) {
+           csp_qfifo_write(buf, &csp_if_fifo, NULL);
+           buf = csp_buffer_get(BUF_SIZE);
+       }
+   }
+
+A new CSP buffer is preallocated with csp_buffer_get(). When data is received, the packet is passed to CSP using `csp_qfifo_write()` and a new buffer is allocated for the next packet. In addition to the received packet, `csp_qfifo_write()` takes two additional arguments:
+
+.. code-block:: c
+
+   void csp_qfifo_write(csp_packet_t *packet, csp_iface_t *interface, CSP_BASE_TYPE *pxTaskWoken);
+
+The calling interface must be passed in `interface` to avoid routing loops. Furthermore, `pxTaskWoken` must be set to a non-NULL value if the packet is received in an interrupt service routine. If the packet is received in task context, NULL must be passed. 'pxTaskWoken' only applies to FreeRTOS systems, and POSIX system should always set the value to NULL.
+
+`csp_qfifo_write` will either accept the packet or free the packet buffer, so the interface must never free the packet after passing it to CSP.
+
+Initialization
+--------------
+
+In order to initialize the interface, and make it available to the router, use the following function found in `csp/csp_interface.h`:
+
+.. code-block:: c
+
+   csp_route_add_if(&csp_if_fifo);
+
+This actually happens automatically if you try to call `csp_route_add()` with an interface that is unknown to the router. This may however be removed in the future, in order to ensure that all interfaces are initialised before configuring the routing table. The reason is, that some products released in the future may ship with an empty routing table, which is then configured by a routing protocol rather than a static configuration.
+
+In order to setup a manual static route, use the following example where the default route is set to the fifo interface:
+
+.. code-block:: c
+
+   csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC);
+
+All outgoing traffic except loopback, is now passed to the fifo interface's nexthop function. 
+
+Building the example
+--------------------
+
+The fifo examples can be compiled with:
+
+.. code-block:: bash
+
+   % gcc csp_if_fifo.c -o csp_if_fifo -I<CSP PATH>/include -L<CSP PATH>/build -lcsp -lpthread -lrt
+
+The two named pipes are created with:
+
+.. code-block:: bash
+
+   % mkfifo server_to_client client_to_server
+

thirdparty/libcsp/doc/libcsp.rst

@@ -0,0 +1,21 @@
+.. CSP Documentation master file.
+
+.. _libcsp:
+   
+**********************
+CubeSat Space Protocol
+**********************
+
+.. toctree::
+   :maxdepth: 3
+   
+   ../README
+   history
+   structure
+   interfaces
+   memory
+   protocolstack
+   topology
+   mtu
+   example
+   

thirdparty/libcsp/doc/memory.rst

@@ -0,0 +1,28 @@
+How CSP uses memory
+===================
+
+CSP has been written for small microprocessor systems. The way memory is handled is therefore a tradeoff between the amount used and the code efficiency. This section tries to give some answers to what the memory is used for and how it it used. The primary memory blocks in use by CSP is:
+
+ * Routing table
+ * Ports table
+ * Connection table
+ * Buffer pool
+ * Interface list
+
+Tables
+------
+The reason for using tables for the routes, ports and connections is speed. When a new packet arrives the core of CSP needs to do a quick lookup in the connection so see if it can find an existing connection to which the packet matches. If this is not found, it will take a lookup in the ports table to see if there are any applications listening on the incoming port number. Another argument of using tables are pre-allocation. The linker will reserve an area of the memory for which the routes and connections can be stored. This avoid an expensive `malloc()` call during initialization of CSP, and practically costs zero CPU instructions. The downside of using tables are the wasted memory used by unallocated ports and connections. For the routing table the argumentation is the same, pre-allocation is better than calling `malloc()`.
+
+Buffer Pool
+-----------
+
+The buffer handling system can be compiled for either static allocation or a one-time dynamic allocation of the main memory block. After this, the buffer system is entirely self-contained. All allocated elements are of the same size, so the buffer size must be chosen to be able to handle the maximum possible packet length. The buffer pool uses a queue to store pointers to free buffer elements. First of all, this gives a very quick method to get the next free element since the dequeue is an O(1) operation. Furthermore, since the queue is a protected operating system primitive, it can be accessed from both task-context and interrupt-context. The `csp_buffer_get` version is for task-context and `csp_buffer_get_isr` is for interrupt-context. Using fixed size buffer elements that are preallocated is again a question of speed and safety.
+
+
+A basic concept of the buffer system is called Zero-Copy. This means that from userspace to the kernel-driver, the buffer is never copied from one buffer to another. This is a big deal for a small microprocessor, where a call to `memcpy()` can be very expensive. In practice when data is inserted into a packet, it is shifted a certain number of bytes in order to allow for a packet header to be prepended at the lower layers. This also means that there is a strict contract between the layers, which data can be modified and where. The buffer object is normally casted to a `csp_packet_t`, but when its given to an interface on the MAC layer it's casted to a `csp_i2c_frame_t` for example.
+
+Interface list
+--------------
+
+The interface list is a simple single-ended linked list of references to the interface specification structures. These structures are static const and allocated by the linker. The pointer to this data is inserted into the list one time during setup of the interface. Each entry in the routing table has a direct pointer to the interface element, thereby avoiding list lookup, but the list is needed in order for the dynamic route configuration to know which interfaces are available.
+

thirdparty/libcsp/doc/mtu.rst

@@ -0,0 +1,19 @@
+Maximum Transfer Unit
+=====================
+
+There are two things limiting the MTU of CSP.
+
+  1. The pre-allocated buffer pool’s allocation size
+  2. The link layer protocol.
+
+So let’s assume that you have made a protocol called KISS with a MTU of 256. The 256 is the total amount of data that you can put into the CSP-packet. However, you need to take the overhead of the link layer into account. Typically this could consist of a length field and/or a start/stop flag. So the actual frame size on the link layer would for example be 256 bytes of data + 2 bytes sync flag + 2 bytes length field.
+
+This requires a buffer allocation of at lest 256 + 2 + 2. However, the CSP packet itself has some reserved bytes in the beginning of the packet (which you can see in csp.h) - so the recommended buffer allocation size is MAX MTU + 16 bytes. In this case the max MTU would be 256.
+
+If you try to pass data which is longer than the MTU, the chance is that you will also make a buffer overflow in the CSP buffer pool. However, lets assume that you have two interfaces one with an MTU of 200 bytes and another with an MTU of 100 bytes. In this case you might successfully transfer 150 bytes over the first interface, but the packet will be rejected once it comes to the second interface.
+
+If you want to increase your MTU of a specific link layer, it is up to the link layer protocol to implement its own fragmentation protocol. A good example is CAN-bus which only allows a frame size of 8 bytes. libcsp have a small protocol for this called the “CAN fragmentation protocol" or CFP for short. This allows data of much larger size to be transferred over the CAN bus.
+
+Okay, but what if you want to transfer 1000 bytes, and the network maximum MTU is 256? Well, since CSP does not include streaming sockets, only packet’s. Somebody will have to split that data up into chunks. It might be that you application have special knowledge about the datatype you are transmitting, and that it makes sense to split the 1000 byte content into 10 chunks of 100 byte status messages. This, application layer delimitation might be good if you have a situation with packet loss, because your receiver could still make good usage of the partially delivered chunks.
+
+But, what if you just want 1000 bytes transmitted, and you don’t care about the fragmentation unit, and also don’t want the hassle of writing the fragmentation code yourself? - In this case, libcsp now features a new (still experimental) feature called SFP (small fragmentation protocol) designed to work on the application layer. For this purpose you will not use csp_send and csp_recv, but csp_sfp_send and csp_sfp_recv. This will split your data into chunks of a certain size, enumerate them and transfer over a given connection. If a chunk is missing the SFP client will abort the reception, because SFP does not provide retransmission. If you wish to also have retransmission and orderly delivery you will have to open an RDP connection and send your SFP message to that connection.

thirdparty/libcsp/doc/protocolstack.rst

@@ -0,0 +1,54 @@
+The Protocol Stack
+==================
+
+The CSP protocol stack includes functionality on all layers of the TCP/IP model:
+
+Layer 1: Drivers
+----------------
+
+Lib CSP is not designed for any specific processor or hardware peripheral, but yet these drivers are required in order to work. The intention of LibCSP is not to provide CAN, I2C or UART drivers for all platforms, however some drivers has been included for some platforms. If you do not find your platform supported, it is quite simple to add a driver that conforms to the CSP interfaces. For example the I2C driver just requires three functions: `init`, `send` and `recv`. For good stability and performance interrupt driven drivers are preferred in favor of polled drivers. Where applicable also DMA usage is recommended.
+
+Layer 2: MAC interfaces
+-----------------------
+
+CSP has interfaces for I2C, CAN, RS232 (KISS) and Loopback. The layer 2 protocol software defines a frame-format that is suitable for the media. CSP can be easily extended with implementations for even more links. For example a radio-link and IP-networks. The file `csp_interface.h` declares the rx and tx functions needed in order to define a network interface in CSP. During initialisation of CSP each interface will be inserted into a linked list of interfaces that is available to the router. In cases where link-layer addresses are required, such as I2C, the routing table supports specifying next-hop link-layer address directly. This avoids the need to implement an address resolution protocol to translate CSP addresses to I2C addresses.
+
+Layer 3: Network Router
+-----------------------
+
+The router core is the backbone of the CSP implementation. The router works by looking at a 32-bit CSP header which contains the delivery and source address together with port numbers for the connection. Each router supports both local delivery and forwarding of frames to another destination. Frames will never exit the router on the same interface that they arrives at, this concept is called split horizon, and helps prevent routing loops.
+
+The main purpose of the router is to accept incoming packets and deliver them to the right message queue. Therefore, in order to listen on a port-number on the network, a task must create a socket and call the accept() call. This will make the task block and wait for incoming traffic, just like a web-server or similar. When an incoming connection is opened, the task is woken. Depending on the task-priority, the task can even preempt another task and start execution immediately.
+
+There is no routing protocol for automatic route discovery, all routing tables are pre-programmed into the subsystems. The table itself contains a separate route to each of the possible 32 nodes in the network and the additional default route. This means that the overall topology must be decided before putting sub-systems together, as explained in the `topology.md` file. However CSP has an extension on port zero CMP (CSP management protocol), which allows for over-the-network routing table configuration. This has the advantage that default routes could be changed if for example the primary radio fails, and the secondary should be used instead.
+
+Layer 4: Transport Layer
+------------------------
+
+LibCSP implements two different Transport Layer protocols, they are called UDP (unreliable datagram protocol) and RDP (reliable datagram protocol). The name UDP has not been chosen to be an exact replica of the UDP (user datagram protocol) known from the TCP/IP model, but they have certain similarities.
+
+The most important thing to notice is that CSP is entirely a datagram service. There is no stream based service like TCP. A datagram is defined a block of data with a specified size and structure. This block enters the transport layer as a single datagram and exits the transport layer in the other end as a single datagram. CSP preserves this structure all the way to the physical layer for I2C, KISS and Loopback interfaces are used. The CAN-bus interface has to fragment the datagram into CAN-frames of 8 bytes, however only a fully completed datagram will arrive at the receiver.
+
+UDP
+^^^
+
+UDP uses a simple transmission model without implicit hand-shaking dialogues for guaranteeing reliability, ordering, or data integrity. Thus, UDP provides an unreliable service and datagrams may arrive out of order, appear duplicated, or go missing without notice. UDP assumes that error checking and correction is either not necessary or performed in the application, avoiding the overhead of such processing at the network interface level. Time-sensitive applications often use UDP because dropping packets is preferable to waiting for delayed packets, which may not be an option in a real-time system.
+
+UDP is very practical to implement request/reply based communication where a single packet forms the request and a single packet forms the reply. In this case a typical request and wait protocol is used between the client and server, which will simply return an error if a reply is not received within a specified time limit. An error would normally lead to a retransmission of the request from the user or operator which sent the request.
+
+While UDP is very simple, it also has some limitations. Normally a human in the loop is a good thing when operating the satellite over UDP. But when it comes to larger file transfers, the human becomes the bottleneck. When a high-speed file transfer is initiated data acknowledgment should be done automatically in order to speed up the transfer. This is where the RDP protocol can help.
+
+RDP
+^^^
+CSP provides a transport layer extension called RDP (reliable datagram protocol) which is an implementation of RFC908 and RFC1151. RDP provides a few additional features:
+
+ * Three-way handshake
+ * Flow Control
+ * Data-buffering
+ * Packet re-ordering
+ * Retransmission
+ * Windowing
+ * Extended Acknowledgment
+
+For more information on this, please refer to RFC908.
+

thirdparty/libcsp/doc/structure.rst

@@ -0,0 +1,27 @@
+Structure
+=========
+The Cubesat Space Protocol library is structured as shown in the following table:
+
+=============================  =========================================================================
+**Folder**                     **Description**
+=============================  =========================================================================
+libcsp/include/csp             Main include files
+libcsp/include/csp/arch        Architecture include files
+libcsp/include/csp/interfaces  Interface include files
+libcsp/include/csp/drivers     Drivers include files
+libcsp/src                     Main modules for CSP: io, router, connections, services 
+libcsp/src/interfaces          Interface modules for CAN, I2C, KISS, LOOP and ZMQHUB
+libcsp/src/drivers/can         Driver for CAN                                      
+libcsp/src/drivers/usart       Driver for USART                                      
+libcsp/src/arch/freertos       FreeRTOS architecture module
+libcsp/src/arch/macosx         Mac OS X architecture module
+libcsp/src/arch/posix          Posix architecture module
+libcsp/src/arch/windows        Windows architecture module
+libcsp/src/rtable              Routing table module
+libcsp/transport               Transport module, UDP and RDP
+libcsp/crypto                  Crypto module
+libcsp/utils                   Utilities
+libcsp/bindings/python         Python wrapper for libcsp                                       
+libcsp/examples                CSP examples (source code)                                      
+libasf/doc                     The doc folder contains the source code for this documentation
+=============================  =========================================================================

thirdparty/libcsp/doc/topology.rst

@@ -0,0 +1,26 @@
+Network Topology
+================
+
+CSP uses a network oriented terminology similar to what is known from the Internet and the TCP/IP model. A CSP network can be configured for several different topologies. The most common topology is to create two segments, one for the Satellite and one for the Ground-Station. 
+
+.. code-block:: none
+
+                 I2C BUS
+     _______________________________
+    /       |       |       |       \
+   +---+   +---+   +---+   +---+   +---+
+   |OBC|   |COM|   |EPS|   |PL1|   |PL2|       Nodes 0 - 7 (Space segment)
+   +---+   +---+   +---+   +---+   +---+
+             ^
+             |  Radio
+             v
+           +---+           +----+
+           |TNC|  -------  | PC |              Nodes 8 - 15 (Ground segment)
+           +---+    USB    +----+
+   
+           Node 9          Node 10
+
+The address range, from 0 to 15, has been segmented into two equal size segments. This allows for easy routing in the network. All addresses starting with binary 1 is on the ground-segment, and all addresses starting with 0 is on the space segment. From CSP v1.0 the address space has been increased to 32 addresses, 0 to 31. But for legacy purposes, the old 0 to 15 is still used in most products.
+
+The network is configured using static routes initialised at boot-up of each sub-system. This means that the basic routing table must be assigned compile-time of each subsystem. However each node supports assigning an individual route to every single node in the network and can be changed run-time. This means that the network topology can be easily reconfigured after startup.
+

thirdparty/libcsp/examples/csp_if_fifo.c

@@ -0,0 +1,165 @@
+/*
+Cubesat Space Protocol - A small network-layer protocol designed for Cubesats
+Copyright (C) 2012 GomSpace ApS (http://www.gomspace.com)
+Copyright (C) 2012 AAUSAT3 Project (http://aausat3.space.aau.dk)
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <pthread.h>
+#include <stdint.h>
+
+#include <csp/csp.h>
+#include <csp/csp_interface.h>
+
+#define TYPE_SERVER 1
+#define TYPE_CLIENT 2
+#define PORT        10
+#define BUF_SIZE    250
+
+pthread_t rx_thread;
+int rx_channel, tx_channel;
+
+int csp_fifo_tx(csp_iface_t *ifc, csp_packet_t *packet, uint32_t timeout);
+
+csp_iface_t csp_if_fifo = {
+    .name = "fifo",
+    .nexthop = csp_fifo_tx,
+    .mtu = BUF_SIZE,
+};
+
+int csp_fifo_tx(csp_iface_t *ifc, csp_packet_t *packet, uint32_t timeout) {
+    /* Write packet to fifo */
+    if (write(tx_channel, &packet->length, packet->length + sizeof(uint32_t) + sizeof(uint16_t)) < 0)
+        printf("Failed to write frame\r\n");
+    csp_buffer_free(packet);
+    return CSP_ERR_NONE;
+}
+
+void * fifo_rx(void * parameters) {
+    csp_packet_t *buf = csp_buffer_get(BUF_SIZE);
+    /* Wait for packet on fifo */
+    while (read(rx_channel, &buf->length, BUF_SIZE) > 0) {
+        csp_qfifo_write(buf, &csp_if_fifo, NULL);
+        buf = csp_buffer_get(BUF_SIZE);
+    }
+
+    return NULL;
+}
+
+int main(int argc, char **argv) {
+
+    int me, other, type;
+    const char *message = "Testing CSP";
+    const char *rx_channel_name;
+    const char *tx_channel_name;
+    csp_socket_t *sock;
+    csp_conn_t *conn;
+    csp_packet_t *packet;
+
+    /* Run as either server or client */
+    if (argc != 2) {
+        printf("usage: %s <server/client>\r\n", argv[0]);
+        return -1;
+    }
+
+    /* Set type */
+    if (strcmp(argv[1], "server") == 0) {
+        me = 1;
+        other = 2;
+        tx_channel_name = "server_to_client";
+        rx_channel_name = "client_to_server";
+        type = TYPE_SERVER;
+    } else if (strcmp(argv[1], "client") == 0) {
+        me = 2;
+        other = 1;
+        tx_channel_name = "client_to_server";
+        rx_channel_name = "server_to_client";
+        type = TYPE_CLIENT;
+    } else {
+        printf("Invalid type. Must be either 'server' or 'client'\r\n");
+        return -1;
+    }
+
+    /* Init CSP and CSP buffer system */
+    if (csp_init(me) != CSP_ERR_NONE || csp_buffer_init(10, 300) != CSP_ERR_NONE) {
+        printf("Failed to init CSP\r\n");
+        return -1;
+    }
+
+    tx_channel = open(tx_channel_name, O_RDWR);
+    if (tx_channel < 0) {
+        printf("Failed to open TX channel\r\n");
+        return -1;
+    }
+
+    rx_channel = open(rx_channel_name, O_RDWR);
+    if (rx_channel < 0) {
+        printf("Failed to open RX channel\r\n");
+        return -1;
+    }
+
+    /* Start fifo RX task */
+	pthread_create(&rx_thread, NULL, fifo_rx, NULL);
+
+    /* Set default route and start router */
+    csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC);
+    csp_route_start_task(0, 0);
+
+    /* Create socket and listen for incoming connections */
+    if (type == TYPE_SERVER) {
+        sock = csp_socket(CSP_SO_NONE);
+        csp_bind(sock, PORT);
+        csp_listen(sock, 5);
+    }
+
+    /* Super loop */
+    while (1) {
+        if (type == TYPE_SERVER) {
+            /* Process incoming packet */
+            conn = csp_accept(sock, 1000);
+            if (conn) {
+                packet = csp_read(conn, 0);
+                if (packet)
+                    printf("Received: %s\r\n", packet->data);
+                csp_buffer_free(packet);
+                csp_close(conn);
+            }
+        } else {
+            /* Send a new packet */
+            packet = csp_buffer_get(strlen(message));
+            if (packet) {
+                strcpy((char *) packet->data, message);
+                packet->length = strlen(message);
+
+                conn = csp_connect(CSP_PRIO_NORM, other, PORT, 1000, CSP_O_NONE);
+                printf("Sending: %s\r\n", message);
+                if (!conn || !csp_send(conn, packet, 1000))
+                    return -1;
+                csp_close(conn);
+            }
+            sleep(1);
+        }
+    }
+
+    close(rx_channel);
+    close(tx_channel);
+
+    return 0;
+}

thirdparty/libcsp/examples/csp_if_fifo_windows.c

@@ -0,0 +1,225 @@
+#include <Windows.h>
+#include <process.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <stddef.h>
+#undef interface
+
+#include <csp/csp.h>
+#include <csp/csp_interface.h>
+
+#define PIPE_BUFSIZE 1024
+
+#define TYPE_SERVER 1
+#define TYPE_CLIENT 2
+#define PORT        10
+#define BUF_SIZE    250
+
+
+static LPCTSTR pipeName = TEXT("\\\\.\\pipe\\CSP_Pipe");
+
+static HANDLE pipe = INVALID_HANDLE_VALUE;
+
+unsigned WINAPI fifo_rx(void *);
+unsigned WINAPI pipe_listener(void *);
+
+void printError(void);
+
+int csp_fifo_tx(csp_packet_t *packet, uint32_t timeout);
+
+csp_iface_t csp_if_fifo = {
+    .name = "fifo",
+    .nexthop = csp_fifo_tx,
+    .mtu = BUF_SIZE,
+};
+
+int csp_fifo_tx(csp_packet_t *packet, uint32_t timeout) {
+    printf("csp_fifo_tx tid: %lu\n", GetCurrentThreadId());
+    DWORD expectedSent = packet->length + sizeof(uint32_t) + sizeof(uint16_t);
+    DWORD actualSent;
+    /* Write packet to fifo */
+    if( !WriteFile(pipe, &packet->length, expectedSent, &actualSent, NULL)
+            || actualSent != expectedSent ) {
+        printError();
+    }
+
+    csp_buffer_free(packet);
+    return CSP_ERR_NONE;
+}
+
+
+int main(int argc, char *argv[]) {
+    int me, other, type;
+    char *message = "Testing CSP";
+    csp_socket_t *sock = NULL;
+    csp_conn_t *conn = NULL;
+    csp_packet_t *packet = NULL;
+
+    /* Run as either server or client */
+    if (argc != 2) {
+        printf("usage: server <server/client>\r\n");
+        return -1;
+    }
+
+    /* Set type */
+    if (strcmp(argv[1], "server") == 0) {
+        me = 1;
+        other = 2;
+        type = TYPE_SERVER;
+    } else if (strcmp(argv[1], "client") == 0) {
+        me = 2;
+        other = 1;
+        type = TYPE_CLIENT;
+    } else {
+        printf("Invalid type. Must be either 'server' or 'client'\r\n");
+        return -1;
+    }
+
+    /* Init CSP and CSP buffer system */
+    if (csp_init(me) != CSP_ERR_NONE || csp_buffer_init(10, 300) != CSP_ERR_NONE) {
+        printf("Failed to init CSP\r\n");
+        return -1;
+    }
+
+    if( type == TYPE_SERVER ) {
+        _beginthreadex(NULL, 0, pipe_listener, NULL, 0, 0);    
+    } else {
+        pipe = CreateFile(
+            pipeName,
+            GENERIC_READ | GENERIC_WRITE,
+            0,
+            NULL,
+            OPEN_EXISTING,
+            0,
+            NULL);
+        if( pipe == INVALID_HANDLE_VALUE ) {
+            printError();
+            return -1;
+        }
+    }
+
+    /* Set default route and start router */
+    csp_route_set(CSP_DEFAULT_ROUTE, &csp_if_fifo, CSP_NODE_MAC);
+    csp_route_start_task(0, 0);
+
+    /* Create socket and listen for incoming connections */
+    if (type == TYPE_SERVER) {
+        sock = csp_socket(CSP_SO_NONE);
+        csp_bind(sock, PORT);
+        csp_listen(sock, 5);
+    }
+
+    /* Super loop */
+    while (1) {
+        if (type == TYPE_SERVER) {
+            /* Process incoming packet */
+            conn = csp_accept(sock, 1000);
+            if (conn) {
+                packet = csp_read(conn, 0);
+                if (packet)
+                    printf("Received: %s\r\n", packet->data);
+                csp_buffer_free(packet);
+                csp_close(conn);
+            }
+        } else {
+            /* Send a new packet */
+            packet = csp_buffer_get(strlen(message));
+            if (packet) {
+                strcpy((char *) packet->data, message);
+                packet->length = strlen(message);
+                
+                conn = csp_connect(CSP_PRIO_NORM, other, PORT, 1000, CSP_O_NONE);
+                printf("Sending: %s\r\n", message);
+                if (!conn || !csp_send(conn, packet, 1000))
+                    return -1;
+                csp_close(conn);
+                Sleep(1000);
+            }
+        }
+    }
+
+    return 0;
+}
+
+void printError(void) {
+    LPTSTR messageBuffer = NULL;
+    DWORD errorCode = GetLastError();
+    DWORD formatMessageRet;
+    formatMessageRet = FormatMessage(
+        FORMAT_MESSAGE_ALLOCATE_BUFFER |
+        FORMAT_MESSAGE_FROM_SYSTEM |
+        FORMAT_MESSAGE_IGNORE_INSERTS,
+        NULL,
+        errorCode,
+        MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+        (LPTSTR)&messageBuffer,
+        0,
+        NULL);
+
+    if( !formatMessageRet ) {
+        wprintf(L"FormatMessage error, code: %lu\n", GetLastError());
+        return;
+    }
+    
+    printf("%s\n", messageBuffer);
+    LocalFree(messageBuffer);
+}
+
+unsigned WINAPI pipe_listener(void *parameters) {
+    while(1) {
+        HANDLE pipe =  CreateNamedPipe(
+                        pipeName,
+                        PIPE_ACCESS_DUPLEX,
+                        PIPE_TYPE_MESSAGE | PIPE_READMODE_MESSAGE | PIPE_WAIT,
+                        PIPE_UNLIMITED_INSTANCES,
+                        PIPE_BUFSIZE,
+                        PIPE_BUFSIZE,
+                        0,
+                        NULL);
+        BOOL clientConnected;
+        if( pipe == INVALID_HANDLE_VALUE ) {
+            printf("Error creating named pipe. Code %lu\n", GetLastError());
+            return -1;
+        }
+
+        // True if client connects *after* server called ConnectNamedPipe 
+        // or *between* CreateNamedPipe and ConnectNamedPipe
+        clientConnected = 
+            ConnectNamedPipe(pipe, NULL) ? TRUE : GetLastError()==ERROR_PIPE_CONNECTED;
+        printf("Client connected!\n");
+
+        if( !clientConnected ) {
+            printf("Failure while listening for clients. Code %lu\n", GetLastError());
+            CloseHandle(pipe);
+            return -1;
+        }
+        printf("Create client thread\n");
+        _beginthreadex(NULL, 0, fifo_rx, (PVOID)pipe, 0, 0);    
+    }
+
+    return 0;
+}
+
+unsigned WINAPI fifo_rx(void *handle) {
+    printf("fifo_rx tid: %lu\n", GetCurrentThreadId());
+    HANDLE pipe = (HANDLE) handle;
+    csp_packet_t *buf = csp_buffer_get(BUF_SIZE);
+    DWORD bytesRead;
+    BOOL readSuccess;
+
+    while(1) {
+        readSuccess = 
+            ReadFile(pipe, &buf->length, BUF_SIZE, &bytesRead, NULL);
+        if( !readSuccess || bytesRead == 0 ) {
+            csp_buffer_free(buf);
+            printError();
+            break;
+        }
+        csp_qfifo_write(buf, &csp_if_fifo, NULL);
+        buf = csp_buffer_get(BUF_SIZE);
+    }
+    printf("Closing pipe to client\n");
+    CloseHandle(pipe);
+
+    return 0;
+}

thirdparty/libcsp/examples/kiss.c

@@ -0,0 +1,151 @@
+/**
+ * Build this example on linux with:
+ * ./waf configure --enable-examples --enable-if-kiss --with-driver-usart=linux --enable-crc32 clean build
+ */
+
+#include <stdio.h>
+#include <csp/csp.h>
+#include <csp/interfaces/csp_if_kiss.h>
+
+#include <csp/drivers/usart.h>
+#include <csp/arch/csp_thread.h>
+
+#define PORT 10
+#define MY_ADDRESS 1
+
+#define SERVER_TIDX 0
+#define CLIENT_TIDX 1
+#define USART_HANDLE 0
+
+CSP_DEFINE_TASK(task_server) {
+    int running = 1;
+    csp_socket_t *socket = csp_socket(CSP_SO_NONE);
+    csp_conn_t *conn;
+    csp_packet_t *packet;
+    csp_packet_t *response;
+
+    response = csp_buffer_get(sizeof(csp_packet_t) + 2);
+    if( response == NULL ) {
+        fprintf(stderr, "Could not allocate memory for response packet!\n");
+        return CSP_TASK_RETURN;
+    }
+    response->data[0] = 'O';
+    response->data[1] = 'K';
+    response->length = 2;
+
+    csp_bind(socket, CSP_ANY);
+    csp_listen(socket, 5);
+
+    printf("Server task started\r\n");
+
+    while(running) {
+        if( (conn = csp_accept(socket, 10000)) == NULL ) {
+            continue;
+        }
+
+        while( (packet = csp_read(conn, 100)) != NULL ) {
+            switch( csp_conn_dport(conn) ) {
+                case PORT:
+                    if( packet->data[0] == 'q' )
+                        running = 0;
+                    csp_buffer_free(packet);
+                    csp_send(conn, response, 1000);
+                    break;
+                default:
+                    csp_service_handler(conn, packet);
+                    break;
+            }
+        }
+
+        csp_close(conn);
+    }
+
+    csp_buffer_free(response);
+
+    return CSP_TASK_RETURN;
+}
+
+CSP_DEFINE_TASK(task_client) {
+
+    char outbuf = 'q';
+    char inbuf[3] = {0};
+    int pingResult;
+
+    for(int i = 50; i <= 200; i+= 50) {
+        pingResult = csp_ping(MY_ADDRESS, 1000, 100, CSP_O_NONE);
+        printf("Ping with payload of %d bytes, took %d ms\n", i, pingResult);
+        csp_sleep_ms(1000);
+    }
+    csp_ps(MY_ADDRESS, 1000);
+    csp_sleep_ms(1000);
+    csp_memfree(MY_ADDRESS, 1000);
+    csp_sleep_ms(1000);
+    csp_buf_free(MY_ADDRESS, 1000);
+    csp_sleep_ms(1000);
+    csp_uptime(MY_ADDRESS, 1000);
+    csp_sleep_ms(1000);
+
+    csp_transaction(0, MY_ADDRESS, PORT, 1000, &outbuf, 1, inbuf, 2);
+    printf("Quit response from server: %s\n", inbuf);
+
+    return CSP_TASK_RETURN;
+}
+
+int main(int argc, char **argv) {
+    csp_debug_toggle_level(CSP_PACKET);
+    csp_debug_toggle_level(CSP_INFO);
+
+    csp_buffer_init(10, 300);
+    csp_init(MY_ADDRESS);
+
+    struct usart_conf conf;
+
+#if defined(CSP_WINDOWS)
+    conf.device = argc != 2 ? "COM4" : argv[1];
+    conf.baudrate = CBR_9600;
+    conf.databits = 8;
+    conf.paritysetting = NOPARITY;
+    conf.stopbits = ONESTOPBIT;
+    conf.checkparity = FALSE;
+#elif defined(CSP_POSIX)
+    conf.device = argc != 2 ? "/dev/ttyUSB0" : argv[1];
+    conf.baudrate = 500000;
+#elif defined(CSP_MACOSX)
+    conf.device = argc != 2 ? "/dev/tty.usbserial-FTSM9EGE" : argv[1];
+    conf.baudrate = 115200;
+#endif
+
+	/* Run USART init */
+	usart_init(&conf);
+
+    /* Setup CSP interface */
+	static csp_iface_t csp_if_kiss;
+	static csp_kiss_handle_t csp_kiss_driver;
+	csp_kiss_init(&csp_if_kiss, &csp_kiss_driver, usart_putc, usart_insert, "KISS");
+		
+	/* Setup callback from USART RX to KISS RS */
+	void my_usart_rx(uint8_t * buf, int len, void * pxTaskWoken) {
+		csp_kiss_rx(&csp_if_kiss, buf, len, pxTaskWoken);
+	}
+	usart_set_callback(my_usart_rx);
+
+    csp_route_set(MY_ADDRESS, &csp_if_kiss, CSP_NODE_MAC);
+    csp_route_start_task(0, 0);
+
+    csp_conn_print_table();
+    csp_route_print_table();
+    csp_route_print_interfaces();
+
+    csp_thread_handle_t handle_server;
+    csp_thread_create(task_server, "SERVER", 1000, NULL, 0, &handle_server);
+    csp_thread_handle_t handle_client;
+    csp_thread_create(task_client, "CLIENT", 1000, NULL, 0, &handle_client);
+
+    /* Wait for program to terminate (ctrl + c) */
+    while(1) {
+    	csp_sleep_ms(1000000);
+    }
+
+    return 0;
+
+}

thirdparty/libcsp/examples/python_bindings_example_client.py

@@ -0,0 +1,42 @@
+#!/usr/bin/python
+
+# libcsp must be build with at least these options to run this example client:
+# ./waf distclean configure build --enable-bindings --enable-crc32 --enable-rdp --enable-if-zmq --with-driver-usart=linux --enable-if-kiss --enable-xtea --enable-if-can --enable-can-socketcan --enable-hmac --enable-examples
+
+# Can be run from root of libcsp like this:
+# LD_LIBRARY_PATH=build PYTHONPATH=bindings/python:build python examples/python_bindings_example_client.py
+#
+
+import os
+import time
+import libcsp as csp
+
+
+if __name__ == "__main__":
+
+    csp.buffer_init(10, 300)
+    csp.init(28)
+    csp.zmqhub_init(28, "localhost")
+    csp.rtable_set(27, 5, "ZMQHUB")
+    csp.route_start_task()
+
+    ## allow router task startup
+    time.sleep(1)
+
+    ## cmp_ident
+    (rc, host, model, rev, date, time) = csp.cmp_ident(27)
+    if rc == csp.CSP_ERR_NONE:
+        print (host, model, rev, date, time)
+    else:
+        print ("error in cmp_ident, rc=%i" % (rc))
+
+    ## transaction
+    outbuf = bytearray().fromhex('01')
+    inbuf = bytearray(1)
+    print ("using csp_transaction to send a single byte")
+    if csp.transaction(0, 27, 10, 1000, outbuf, inbuf) < 1:
+        print ("csp_transaction failed")
+    else:
+        print ("got reply, data=" + ''.join('{:02x}'.format(x) for x in inbuf))
+
+

thirdparty/libcsp/examples/python_bindings_example_client_can.py

@@ -0,0 +1,30 @@
+#!/usr/bin/python
+
+# libcsp must be build with at least these options to run this example client:
+# ./waf distclean configure build --enable-bindings --enable-crc32 --enable-rdp --enable-if-zmq --with-driver-usart=linux --enable-if-kiss --enable-xtea --enable-if-can --enable-can-socketcan --enable-hmac --enable-examples
+
+# Can be run from root of libcsp like this:
+# LD_LIBRARY_PATH=build PYTHONPATH=bindings/python:build python examples/python_bindings_example_client.py
+#
+
+import os
+import time
+import libcsp as csp
+
+
+if __name__ == "__main__":
+
+    csp.buffer_init(10, 300)
+    csp.init(28)
+    csp.can_socketcan_init("can0")
+    csp.rtable_set(4, 5, "CAN")
+    csp.route_start_task()
+
+    ## allow router task startup
+    time.sleep(1)
+
+
+    node = 4
+    if csp.ping(node) < 0:
+        print ("Unable to ping node %d"%(node))
+

thirdparty/libcsp/examples/python_bindings_example_server.py

@@ -0,0 +1,72 @@
+#!/usr/bin/python
+
+# libcsp must be build with at least these options to run this example server:
+# ./waf distclean configure build --enable-bindings --enable-crc32 --enable-rdp --enable-if-zmq --with-driver-usart=linux --enable-if-kiss --enable-xtea --enable-if-can --enable-can-socketcan --enable-hmac --enable-examples
+
+# Can be run from root of libcsp like this:
+# LD_LIBRARY_PATH=build PYTHONPATH=bindings/python:build python examples/python_bindings_example_server.py
+#
+
+import os
+import time
+import sys
+import libcsp as csp
+import subprocess
+
+if __name__ == "__main__":
+
+    # start a zmqproxy to transport messages to and from the client
+    zmqp = subprocess.Popen('build/zmqproxy')
+
+    # init csp
+    csp.buffer_init(10, 300)
+    csp.init(27)
+    csp.zmqhub_init(27, "localhost")
+    csp.rtable_set(28, 5, "ZMQHUB")
+    csp.route_start_task()
+
+    # set identity
+    csp.set_hostname("test_service")
+    csp.set_model("bindings")
+    csp.set_revision("1.2.3")
+
+    # and read it back
+    print (csp.get_hostname())
+    print (csp.get_model())
+    print (csp.get_revision())
+
+    # start listening for packets...
+    sock = csp.socket()
+    csp.bind(sock, csp.CSP_ANY)
+    csp.listen(sock)
+    while True:
+        conn = csp.accept(sock)
+        if not conn:
+            continue
+
+        print ("connection: source=%i:%i, dest=%i:%i" % (csp.conn_src(conn),
+                                                        csp.conn_sport(conn),
+                                                        csp.conn_dst(conn),
+                                                        csp.conn_dport(conn)))
+
+        while True:
+            packet = csp.read(conn)
+            if not packet:
+                break
+
+            if csp.conn_dport(conn) == 10:
+                data = bytearray(csp.packet_get_data(packet))
+                length = csp.packet_get_length(packet)
+                print ("got packet, len=" + str(length) + ", data=" + ''.join('{:02x}'.format(x) for x in data))
+
+                data[0] = data[0] + 1
+                reply_packet = csp.buffer_get(1)
+                if reply_packet:
+                    csp.packet_set_data(reply_packet, data)
+                    csp.sendto_reply(packet, reply_packet, csp.CSP_O_NONE)
+
+                csp.buffer_free(packet)
+            else:
+                csp.service_handler(conn, packet)
+        csp.close(conn)
+