diff --git a/_bin/linux/debug/fsfw-example-linux.elf b/_bin/linux/debug/fsfw-example-linux.elf
index 86183cd..5528b41 100755
Binary files a/_bin/linux/debug/fsfw-example-linux.elf and b/_bin/linux/debug/fsfw-example-linux.elf differ
diff --git a/_obj/linux/debug/bsp_linux/core/InitMission.o b/_obj/linux/debug/bsp_linux/core/InitMission.o
index 81ea0ae..385f6e7 100644
Binary files a/_obj/linux/debug/bsp_linux/core/InitMission.o and b/_obj/linux/debug/bsp_linux/core/InitMission.o differ
diff --git a/_obj/linux/debug/bsp_linux/fsfwconfig/datapool/dataPoolInit.o b/_obj/linux/debug/bsp_linux/fsfwconfig/datapool/dataPoolInit.o
index 5868052..fc3bf6f 100644
Binary files a/_obj/linux/debug/bsp_linux/fsfwconfig/datapool/dataPoolInit.o and b/_obj/linux/debug/bsp_linux/fsfwconfig/datapool/dataPoolInit.o differ
diff --git a/_obj/linux/debug/bsp_linux/fsfwconfig/pollingsequence/pollingSequenceControllerFunction.o b/_obj/linux/debug/bsp_linux/fsfwconfig/pollingsequence/pollingSequenceControllerFunction.o
index c6b60ac..d22355e 100644
Binary files a/_obj/linux/debug/bsp_linux/fsfwconfig/pollingsequence/pollingSequenceControllerFunction.o and b/_obj/linux/debug/bsp_linux/fsfwconfig/pollingsequence/pollingSequenceControllerFunction.o differ
diff --git a/_obj/linux/debug/bsp_linux/main.o b/_obj/linux/debug/bsp_linux/main.o
index 63b787f..bdb5990 100644
Binary files a/_obj/linux/debug/bsp_linux/main.o and b/_obj/linux/debug/bsp_linux/main.o differ
diff --git a/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerBase.o b/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerBase.o
index ec9fe48..828254d 100644
Binary files a/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerBase.o and b/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerBase.o differ
diff --git a/_obj/linux/debug/mission/Controller/ArduinoTCSTemperatureSensor.o b/_obj/linux/debug/mission/Controller/ArduinoTCSTemperatureSensor.o
index 8fb2311..ee57b35 100644
Binary files a/_obj/linux/debug/mission/Controller/ArduinoTCSTemperatureSensor.o and b/_obj/linux/debug/mission/Controller/ArduinoTCSTemperatureSensor.o differ
diff --git a/_obj/linux/debug/mission/Controller/ThermalController.o b/_obj/linux/debug/mission/Controller/ThermalController.o
index 817fec5..455f7da 100644
Binary files a/_obj/linux/debug/mission/Controller/ThermalController.o and b/_obj/linux/debug/mission/Controller/ThermalController.o differ
diff --git a/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o b/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o
index 173b154..91d2a51 100644
Binary files a/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o and b/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o differ
diff --git a/bsp_linux/core/InitMission.cpp b/bsp_linux/core/InitMission.cpp
index f2284e9..c59ff73 100644
--- a/bsp_linux/core/InitMission.cpp
+++ b/bsp_linux/core/InitMission.cpp
@@ -115,7 +115,7 @@ void InitMission::createTasks(){
 	 */
 	FixedTimeslotTaskIF* arduinoTask = TaskFactory::instance()->
 			createFixedTimeslotTask("ARDUINO_TASK",40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 2, nullptr);
+					PeriodicTaskIF::MINIMUM_STACK_SIZE, 3.2, nullptr);
 	result = pollingSequenceArduinoFunction(arduinoTask);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
 		sif::error << "InitMission::createTasks:ArduinoPST initialization failed!"
@@ -124,7 +124,7 @@ void InitMission::createTasks(){
 	
 	PeriodicTaskIF* controllerTask = TaskFactory::instance()->
 			createPeriodicTask("CONTROLLER_TASK",40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 2, nullptr);
+					PeriodicTaskIF::MINIMUM_STACK_SIZE, 3.2, nullptr);
 	//result = pollingSequenceControllerFunction(controllerTask);
 	result = controllerTask->addComponent(objects::THERMAL_CONTROLLER);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -161,7 +161,8 @@ void InitMission::createTasks(){
 	udpPollingTask->startTask();
 	//serializeTask->startTask();
 	arduinoTask->startTask();
-	//controllerTask->startTask();
+	TaskFactory::delayTask(6400);
+	controllerTask->startTask();
 
 	//payloadTask->startTask();
 	eventTask->startTask();
diff --git a/bsp_linux/fsfwconfig/datapool/dataPoolInit.cpp b/bsp_linux/fsfwconfig/datapool/dataPoolInit.cpp
index 78b4d47..e32c7a7 100644
--- a/bsp_linux/fsfwconfig/datapool/dataPoolInit.cpp
+++ b/bsp_linux/fsfwconfig/datapool/dataPoolInit.cpp
@@ -16,14 +16,20 @@ void dataPoolInit(std::map<uint32_t, PoolEntryIF*>* pool_map) {
 	pool_map->insert(
 			std::pair<uint32_t, PoolEntryIF*>(datapool::Temperature_Timestamp,
 					new PoolEntry<unsigned int>(Temperature_Timestamp, 36)));
-	float TEMP_SENSOR_CH1[1] = {0};
+
+	uint8_t TempValidity_SENSOR_CH1[1] = {0};
 	pool_map->insert(
-			std::pair<uint32_t, PoolEntryIF*>(datapool::TEMP_SENSOR_CH1,
-					new PoolEntry<float>(TEMP_SENSOR_CH1, 1)));
-	float TEMP_SENSOR_CH2[1] = {0};
+			std::pair<uint32_t, PoolEntryIF*>(datapool::TempValidity_SENSOR_CH1,
+					new PoolEntry<uint8_t>(TempValidity_SENSOR_CH1, 1)));
+	uint8_t TempValidity_SENSOR_CH2[1] = {0};
 	pool_map->insert(
-			std::pair<uint32_t, PoolEntryIF*>(datapool::TEMP_SENSOR_CH2,
-					new PoolEntry<float>(TEMP_SENSOR_CH2, 1)));
+			std::pair<uint32_t, PoolEntryIF*>(datapool::TempValidity_SENSOR_CH2,
+					new PoolEntry<uint8_t>(TempValidity_SENSOR_CH2, 1)));
+
+	float Temperature_COMPONENT_1[1] = {0};
+	pool_map->insert(
+			std::pair<uint32_t, PoolEntryIF*>(datapool::Temperature_COMPONENT_1,
+					new PoolEntry<float>(Temperature_COMPONENT_1, 1)));
 	int8_t TargetState_COMPONENT_1[1] = {0};
 	pool_map->insert(
 			std::pair<uint32_t, PoolEntryIF*>(datapool::TargetState_COMPONENT_1,
@@ -36,6 +42,10 @@ void dataPoolInit(std::map<uint32_t, PoolEntryIF*>* pool_map) {
 	pool_map->insert(
 			std::pair<uint32_t, PoolEntryIF*>(datapool::HeaterRequest_COMPONENT_1,
 					new PoolEntry<uint8_t>(HeaterRequest_COMPONENT_1, 1)));
+	float Temperature_COMPONENT_2[1] = {0};
+	pool_map->insert(
+			std::pair<uint32_t, PoolEntryIF*>(datapool::Temperature_COMPONENT_2,
+					new PoolEntry<float>(Temperature_COMPONENT_2, 1)));
 	int8_t TargetState_COMPONENT_2[1] = {0};
 	pool_map->insert(
 			std::pair<uint32_t, PoolEntryIF*>(datapool::TargetState_COMPONENT_2,
diff --git a/bsp_linux/fsfwconfig/datapool/dataPoolInit.h b/bsp_linux/fsfwconfig/datapool/dataPoolInit.h
index 58397c6..8bc2266 100644
--- a/bsp_linux/fsfwconfig/datapool/dataPoolInit.h
+++ b/bsp_linux/fsfwconfig/datapool/dataPoolInit.h
@@ -14,15 +14,19 @@ namespace datapool {
 		// The IDs of the variables of the DH and CONTROLLER are here defined.
 		Temperature_value = 0x100001,
 		Temperature_Timestamp = 0x100002,
-		TEMP_SENSOR_CH1 = 0x10003,
-		TEMP_SENSOR_CH2 = 0x10004,
-		TargetState_COMPONENT_1 = 0x10005,
-		CurrentState_COMPONENT_1 = 0x100006,
-		HeaterRequest_COMPONENT_1 = 0x100007,
-		TargetState_COMPONENT_2 = 0x100008,
-		CurrentState_COMPONENT_2 = 0x100009,
-		HeaterRequest_COMPONENT_2 = 0x100010
+
+		TempValidity_SENSOR_CH1 = 0x10003,
+		TempValidity_SENSOR_CH2 = 0x10004,
 		
+		Temperature_COMPONENT_1 = 0x10005,
+		TargetState_COMPONENT_1 = 0x10006,
+		CurrentState_COMPONENT_1 = 0x100007,
+		HeaterRequest_COMPONENT_1 = 0x100008,
+		Temperature_COMPONENT_2 = 0x10009,
+		TargetState_COMPONENT_2 = 0x100010,
+		CurrentState_COMPONENT_2 = 0x100011,
+		HeaterRequest_COMPONENT_2 = 0x100012
+
 	};
 }
 
diff --git a/fsfw/devicehandlers/DeviceHandlerBase.cpp b/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 6fba38d..f83732d 100644
--- a/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -67,7 +67,6 @@ ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
 	this->pstStep = counter;
 
 	if (getComAction() == SEND_WRITE) {
-		sif::debug<<"DH_base: performOp1"<<std::endl;
 		cookieInfo.state = COOKIE_UNUSED;
 		readCommandQueue();
 		doStateMachine();
@@ -84,22 +83,17 @@ ReturnValue_t DeviceHandlerBase::performOperation(uint8_t counter) {
 	switch (getComAction()) {
 	case SEND_WRITE:
 		if ((cookieInfo.state == COOKIE_UNUSED)) {
-			sif::debug<<"DH_base: performOp2"<<std::endl;
 			buildInternalCommand();
 		}
-		sif::debug<<"DH_base: performOp3"<<std::endl;
 		doSendWrite();
 		break;
 	case GET_WRITE:
-		sif::debug<<"DH_base: performOp4"<<std::endl;
 		doGetWrite();
 		break;
 	case SEND_READ:
-		sif::debug<<"DH_base: performOp5"<<std::endl;
 		doSendRead();
 		break;
 	case GET_READ:
-		sif::debug<<"DH_base: performOp6"<<std::endl;
 		doGetRead();
 		cookieInfo.state = COOKIE_UNUSED;
 		break;
@@ -665,19 +659,15 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 	// scanForReply routines.
 	uint32_t remainingLength = receivedDataLen;
 	for (uint32_t count = 0; count < receivedDataLen; count++) {
-		sif::debug<<"DH_base: debug0"<<std::endl;
 		result = scanForReply(receivedData, remainingLength, &foundId,
 				&foundLen);
-		sif::debug<<"DH_base: debug01"<<std::endl;
 		switch (result) {
 		case RETURN_OK:
 			handleReply(receivedData, foundId, foundLen);
 			break;
 		case APERIODIC_REPLY: {
-			sif::debug<<"DH_base: debug1"<<std::endl;
 			result = interpretDeviceReply(foundId, receivedData);
 			if (result != RETURN_OK) {
-				sif::debug<<" DH_base: debug2"<<std::endl;
 				replyRawReplyIfnotWiretapped(receivedData, foundLen);
 				triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result,
 						foundId);
@@ -689,7 +679,6 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 		case IGNORE_FULL_PACKET:
 			return;
 		default:
-			sif::debug<<"DH_base: debug02"<<std::endl;
 			//We need to wait for timeout.. don't know what command failed and who sent it.
 			replyRawReplyIfnotWiretapped(receivedData, foundLen);
 			triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen);
@@ -698,9 +687,7 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 		receivedData += foundLen;
 		if (remainingLength > foundLen) {
 			remainingLength -= foundLen;
-			sif::debug<<"DH_base: debug3"<<std::endl;
 		} else {
-			sif::debug<<"DH_base: debug4"<<std::endl;
 			return;
 		}
 	}
diff --git a/mission/Controller/ArduinoTCSTemperatureSensor.cpp b/mission/Controller/ArduinoTCSTemperatureSensor.cpp
index 8b63331..b90e4e5 100644
--- a/mission/Controller/ArduinoTCSTemperatureSensor.cpp
+++ b/mission/Controller/ArduinoTCSTemperatureSensor.cpp
@@ -9,43 +9,42 @@ ArduinoTCSTemperatureSensor::ArduinoTCSTemperatureSensor(object_id_t setObjectid
 		float *inputTemperature, PoolVariableIF *poolVariable,
 		uint8_t vectorIndex, Parameters parameters, uint32_t datapoolId,
 		DataSet *outputSet, ThermalModuleIF *thermalModule) :
-			AbstractTemperatureSensor(setObjectid, thermalModule), parameters(
-					parameters), inputTemperature(inputTemperature), poolVariable(
-					poolVariable), outputTemperature(datapoolId, outputSet,
-					PoolVariableIF::VAR_WRITE) {
+		AbstractTemperatureSensor(setObjectid, thermalModule), parameters(parameters),
+		inputTemperature(*inputTemperature), poolVariable(poolVariable),
+		outputValidity(datapoolId, outputSet, PoolVariableIF::VAR_READ_WRITE)
+		/*DataPool::poolIdAndPositionToPid(poolVariable->getDataPoolId(), vectorIndex)*/
+		/*outputTemperature(datapoolId, outputSet, PoolVariableIF::VAR_WRITE)*/ {
 }
 
 ArduinoTCSTemperatureSensor::~ArduinoTCSTemperatureSensor() {}
 
 void ArduinoTCSTemperatureSensor::setInvalid() {
-		outputTemperature = INVALID_TEMPERATURE;
-		outputTemperature.setValid(false);
-}
-
-float ArduinoTCSTemperatureSensor::calculateOutputTemperature(float inputTemperature) {
-	return inputTemperature; // [°C]
+	inputTemperature = INVALID_TEMPERATURE;
+	outputValidity.setValid(false);
 }
 
 void ArduinoTCSTemperatureSensor::doChildOperation() {
-	if (!poolVariable->isValid() || !healthHelper.healthTable->isHealthy(getObjectId())) {
+	getTemperature();
+	// Loop with isValid doesn't work
+	/*if (!poolVariable->isValid() || !healthHelper.healthTable->isHealthy(getObjectId())) {
 		setInvalid();
 		return;
-	}
-	outputTemperature = calculateOutputTemperature(*inputTemperature);
-	if (outputTemperature<parameters.lowerLimit || outputTemperature>parameters.upperLimit){
-		outputTemperature.setValid(PoolVariableIF::INVALID);
-		outputTemperature = INVALID_TEMPERATURE;
+	}*/
+	if (inputTemperature<parameters.lowerLimit || inputTemperature>parameters.upperLimit){
+		outputValidity.setValid(PoolVariableIF::INVALID);
+		inputTemperature = INVALID_TEMPERATURE;
 	} else {
-		outputTemperature.setValid(PoolVariableIF::VALID);
+		outputValidity.setValid(PoolVariableIF::VALID);
 	}
 }
 
 float ArduinoTCSTemperatureSensor::getTemperature() {
-	return outputTemperature;
+	return inputTemperature;
 }
 
 bool ArduinoTCSTemperatureSensor::isValid() {
-	return outputTemperature.isValid();
+	// To modify?
+	return outputValidity.isValid();
 }
 
 void ArduinoTCSTemperatureSensor::resetOldState() {}
diff --git a/mission/Controller/ArduinoTCSTemperatureSensor.h b/mission/Controller/ArduinoTCSTemperatureSensor.h
index a9e46aa..913f49d 100644
--- a/mission/Controller/ArduinoTCSTemperatureSensor.h
+++ b/mission/Controller/ArduinoTCSTemperatureSensor.h
@@ -19,12 +19,13 @@ private:
 protected:
 
 	Parameters parameters;
-	float *inputTemperature;
+	float inputTemperature;
+	//PoolVariable<float> *inputTemperature;
 	PoolVariableIF *poolVariable;
-	PoolVariable<float> outputTemperature;
+	//float outputTemperature;
+	PoolVariable<uint8_t> outputValidity;
 
-	virtual float calculateOutputTemperature(float inputTemperature);
-	void doChildOperation() override;
+	void doChildOperation();
 
 public:
 	ArduinoTCSTemperatureSensor(object_id_t setObjectid,
diff --git a/mission/Controller/ThermalController.cpp b/mission/Controller/ThermalController.cpp
index 0554bb6..e56ad3f 100644
--- a/mission/Controller/ThermalController.cpp
+++ b/mission/Controller/ThermalController.cpp
@@ -11,15 +11,15 @@
 ThermalController::ThermalController(object_id_t objectId, object_id_t powerSwitcher, size_t commandQueueDepth):
 		ControllerBase(objectId, 0, commandQueueDepth),
 		powerSwitcherId(powerSwitcher), TempValueVec(datapool::Temperature_value, &SensorData, PoolVariableIF::VAR_READ),
-        TEMPERATURE_SENSOR_CH1(objects::TCS_SENSOR_CH1, &TempValueVec[0], &TempValueVec, 0, TEMPERATURE_SENSOR_CONFIG, datapool::TEMP_SENSOR_CH1, &SensorData, NULL),
-        TEMPERATURE_SENSOR_CH2(objects::TCS_SENSOR_CH2, &TempValueVec[1], &TempValueVec, 1, TEMPERATURE_SENSOR_CONFIG, datapool::TEMP_SENSOR_CH2, &SensorData, NULL),
+        TEMPERATURE_SENSOR_CH1(objects::TCS_SENSOR_CH1, &TempValueVec[0], &TempValueVec, 0, TEMPERATURE_SENSOR_CONFIG, datapool::TempValidity_SENSOR_CH1, &SensorData, NULL),
+        TEMPERATURE_SENSOR_CH2(objects::TCS_SENSOR_CH2, &TempValueVec[1], &TempValueVec, 1, TEMPERATURE_SENSOR_CONFIG, datapool::TempValidity_SENSOR_CH2, &SensorData, NULL),
         HEATER_1(objects::TCS_HEATER_1, objects::TCS_SWITCH_1),
         REDUNDANT_HEATER_1(objects::TCS_REDUNDANT_HEATER_1, objects::TCS_SWITCH_1R),
         HEATER_2(objects::TCS_HEATER_2, objects::TCS_SWITCH_2),
         REDUNDANT_HEATER_2(objects::TCS_REDUNDANT_HEATER_2, objects::TCS_SWITCH_2R),
-		TEMPERATURE_COMPONENT_1(objects::TCS_COMPONENT_1, COMPONENT_DOMAIN_ID, datapool::TEMP_SENSOR_CH1, datapool::TargetState_COMPONENT_1, datapool::CurrentState_COMPONENT_1,
+		TEMPERATURE_COMPONENT_1(objects::TCS_COMPONENT_1, COMPONENT_DOMAIN_ID, datapool::Temperature_COMPONENT_1, datapool::TargetState_COMPONENT_1, datapool::CurrentState_COMPONENT_1,
 				datapool::HeaterRequest_COMPONENT_1, &ComponentData, &TEMPERATURE_SENSOR_CH1, NULL, NULL, &HEATER_1, &REDUNDANT_HEATER_1, NULL, COMPONENT1_CONFIG, ThermalComponentIF::SAFE),
-		TEMPERATURE_COMPONENT_2(objects::TCS_COMPONENT_2, COMPONENT_DOMAIN_ID + 1, datapool::TEMP_SENSOR_CH2, datapool::TargetState_COMPONENT_2, datapool::CurrentState_COMPONENT_2,
+		TEMPERATURE_COMPONENT_2(objects::TCS_COMPONENT_2, COMPONENT_DOMAIN_ID + 1, datapool::Temperature_COMPONENT_2, datapool::TargetState_COMPONENT_2, datapool::CurrentState_COMPONENT_2,
 				datapool::HeaterRequest_COMPONENT_2, &ComponentData, &TEMPERATURE_SENSOR_CH2, NULL, NULL, &HEATER_2, &REDUNDANT_HEATER_2, NULL, COMPONENT2_CONFIG, ThermalComponentIF::SAFE)
 		{
         sensors.push_back(TEMPERATURE_SENSOR_CH1);
@@ -103,6 +103,8 @@ ReturnValue_t ThermalController::performOperation(uint8_t opCode) {
 	}
 
 	SensorData.read();
+	printf("\nTemperature: %f", TempValueVec[0]);
+	printf("\nTemperature: %f\n", TempValueVec[1]);
 	for (std::list<ArduinoTCSTemperatureSensor>::iterator iter = sensors.begin(); iter != sensors.end(); iter++) {
 		iter->performOperation(0);
 	}
@@ -117,7 +119,7 @@ ReturnValue_t ThermalController::performOperation(uint8_t opCode) {
 		//request = iter->performOperation(0, ThermalComponentIF::SAFE, true, false);
 	}
 	ComponentData.commit();
-	sif::debug<<"DEBUG_TCS: Component ops"<<std::endl;
+	sif::debug<<"DEBUG_TCS: Component ops"<<std::endl;//*/
 
 	return HasReturnvaluesIF::RETURN_OK;	
 }
diff --git a/mission/DeviceHandler/ArduinoDeviceHandler.cpp b/mission/DeviceHandler/ArduinoDeviceHandler.cpp
index c90c48f..d533f43 100644
--- a/mission/DeviceHandler/ArduinoDeviceHandler.cpp
+++ b/mission/DeviceHandler/ArduinoDeviceHandler.cpp
@@ -103,22 +103,21 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 		memcpy(&TempValueVec[i], &Temp_ch.temperature, 4);
 		memcpy(&TempTimeVec[i], &Temp_ch.Timestamp, 4);
 		// The data are here printed (useful for tests).
-		printf("\n\nStart: %7s", Temp_ch.start_string);
+		/*printf("\n\nStart: %7s", Temp_ch.start_string);
 		printf("\nTyp: %u", Temp_ch.Typ);
 		printf("\nSPCChNumber: %u", Temp_ch.SPCChNumber);
 		printf("\nValue_Cnt: %u", Temp_ch.Value_Cnt);
 		printf("\nTemperature: %f", Temp_ch.temperature);
 		printf("\nTimestamp: %u", Temp_ch.Timestamp);
 		printf("\nEnd: %7s", Temp_ch.end_string);
-		vecTemp.emplace_back(Temp_ch);
+		vecTemp.emplace_back(Temp_ch);*/
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
 	/* The environment and orientation data reading and printing
 	 * are currently commented out because they are not needed.
 	 * Anyway they are available and they can be used if necessary.
 	 */
-
-	printf(
+	/*printf(
 			"\n\n***********************************************************************************************\n");
 	printf("ENVIRONMENTAL parameters are: ");
 	for (int j = 0; j < 9; j++) {