diff --git a/README.md b/README.md
index b60e1ab..7c021bf 100644
--- a/README.md
+++ b/README.md
@@ -18,3 +18,5 @@ In conclusion it is firstly necessary to know the name of the computer serial po
 This name should be substituted in line 52 of mission/DeviceHandler/ArduinoComIF.cpp as: 
 int serial_port = open("WRITE_SERIAL_PORT_NAME", O_RDWR);
 Finally the command described here above must be inserted in the terminal.
+
+RMK! ==> Now the port should be just renamed with the correct name in the code, there is no need anymore of write the open command in the terminal.
diff --git a/_bin/linux/debug/fsfw-example-linux.elf b/_bin/linux/debug/fsfw-example-linux.elf
index 008bf88..85938ae 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 1dce137..2a57db5 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/pollingsequence/PollingSequenceArduinoFunction.o b/_obj/linux/debug/bsp_linux/fsfwconfig/pollingsequence/PollingSequenceArduinoFunction.o
index d05c0af..73604f9 100644
Binary files a/_obj/linux/debug/bsp_linux/fsfwconfig/pollingsequence/PollingSequenceArduinoFunction.o and b/_obj/linux/debug/bsp_linux/fsfwconfig/pollingsequence/PollingSequenceArduinoFunction.o differ
diff --git a/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerFailureIsolation.o b/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerFailureIsolation.o
index b7dd142..8173579 100644
Binary files a/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerFailureIsolation.o and b/_obj/linux/debug/fsfw/devicehandlers/DeviceHandlerFailureIsolation.o differ
diff --git a/_obj/linux/debug/mission/Controller/ThermalController.o b/_obj/linux/debug/mission/Controller/ThermalController.o
index 4b8de08..e78f056 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/ArduinoComIF.o b/_obj/linux/debug/mission/DeviceHandler/ArduinoComIF.o
index ba52c52..7973b27 100644
Binary files a/_obj/linux/debug/mission/DeviceHandler/ArduinoComIF.o and b/_obj/linux/debug/mission/DeviceHandler/ArduinoComIF.o differ
diff --git a/_obj/linux/debug/mission/DeviceHandler/ArduinoCookie.o b/_obj/linux/debug/mission/DeviceHandler/ArduinoCookie.o
index ae4abf2..bcbefa2 100644
Binary files a/_obj/linux/debug/mission/DeviceHandler/ArduinoCookie.o and b/_obj/linux/debug/mission/DeviceHandler/ArduinoCookie.o differ
diff --git a/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o b/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o
index fabb565..b0410ab 100644
Binary files a/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o and b/_obj/linux/debug/mission/DeviceHandler/ArduinoDeviceHandler.o differ
diff --git a/_obj/linux/debug/mission/core/GenericFactory.o b/_obj/linux/debug/mission/core/GenericFactory.o
index 6f74ba1..7b2bb48 100644
Binary files a/_obj/linux/debug/mission/core/GenericFactory.o and b/_obj/linux/debug/mission/core/GenericFactory.o differ
diff --git a/bsp_linux/core/InitMission.cpp b/bsp_linux/core/InitMission.cpp
index 0c388b6..8a5f840 100644
--- a/bsp_linux/core/InitMission.cpp
+++ b/bsp_linux/core/InitMission.cpp
@@ -111,7 +111,7 @@ void InitMission::createTasks(){
 	
 	FixedTimeslotTaskIF* arduinoTask = TaskFactory::instance()->
 			createFixedTimeslotTask("ARDUINO_TASK",40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8, nullptr);
+					PeriodicTaskIF::MINIMUM_STACK_SIZE, 0.8/*6.4*/, nullptr);
 	result = pollingSequenceArduinoFunction(arduinoTask);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
 		sif::error << "InitMission::createTasks:ArduinoPST initialization failed!"
@@ -120,7 +120,7 @@ void InitMission::createTasks(){
 	
 	PeriodicTaskIF* controllerTask = TaskFactory::instance()->
 			createPeriodicTask("CONTROLLER_TASK",40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 3, nullptr);
+					PeriodicTaskIF::MINIMUM_STACK_SIZE, 2, nullptr);
 	//result = pollingSequenceControllerFunction(controllerTask);
 	result = controllerTask->addComponent(objects::THERMAL_CONTROLLER);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -150,8 +150,8 @@ void InitMission::createTasks(){
 #endif
 
 	//Main thread sleep
-	sif::debug << "Starting Tasks in 3.2 seconds" << std::endl;
-	TaskFactory::delayTask(1600);
+	sif::debug << "Starting Tasks in 2 seconds" << std::endl;
+	TaskFactory::delayTask(2400);
 	distributerTask->startTask();
 	udpBridgeTask->startTask();
 	udpPollingTask->startTask();
diff --git a/bsp_linux/fsfwconfig/pollingsequence/PollingSequenceArduinoFunction.cpp b/bsp_linux/fsfwconfig/pollingsequence/PollingSequenceArduinoFunction.cpp
index 1674f79..0f76d93 100644
--- a/bsp_linux/fsfwconfig/pollingsequence/PollingSequenceArduinoFunction.cpp
+++ b/bsp_linux/fsfwconfig/pollingsequence/PollingSequenceArduinoFunction.cpp
@@ -17,9 +17,9 @@ ReturnValue_t pollingSequenceArduinoFunction(
 	uint32_t length = thisSequence->getPeriodMs();
 
 	thisSequence->addSlot(objects::ARDUINO_DEVICE_HANDLER, length * 0, 0);
-	thisSequence->addSlot(objects::ARDUINO_DEVICE_HANDLER, length * 0.2, 1);
-	thisSequence->addSlot(objects::ARDUINO_DEVICE_HANDLER, length * 0.3, 2);
-	thisSequence->addSlot(objects::ARDUINO_DEVICE_HANDLER, length * 0.5, 3);
+	thisSequence->addSlot(objects::ARDUINO_DEVICE_HANDLER, length * 0.1, 1);
+	thisSequence->addSlot(objects::ARDUINO_DEVICE_HANDLER, length * 0.15, 2);
+	thisSequence->addSlot(objects::ARDUINO_DEVICE_HANDLER, length * 0.75, 3);
 
 	if (thisSequence->checkSequence() == HasReturnvaluesIF::RETURN_OK) {
 		return HasReturnvaluesIF::RETURN_OK;
diff --git a/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp b/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
index 3620782..495390b 100644
--- a/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
+++ b/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp
@@ -44,6 +44,8 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event)
 		break;
 	case DeviceHandlerIF::DEVICE_INTERPRETING_REPLY_FAILED:
 	case DeviceHandlerIF::DEVICE_READING_REPLY_FAILED:
+		sif::debug<<"DH_FDIR: event received REPLY"<<std::endl;
+		break;
 	case DeviceHandlerIF::DEVICE_UNREQUESTED_REPLY:
 	case DeviceHandlerIF::DEVICE_UNKNOWN_REPLY: //Some DH's generate generic reply-ids.
 	case DeviceHandlerIF::DEVICE_BUILDING_COMMAND_FAILED:
diff --git a/mission/Controller/tcs_data_config.h b/mission/Controller/tcs_data_config.h
index c14a425..34ab500 100644
--- a/mission/Controller/tcs_data_config.h
+++ b/mission/Controller/tcs_data_config.h
@@ -5,7 +5,7 @@
 #ifndef TCS_DATA_CONFIG_H_
 #define TCS_DATA_CONFIG_H_
 
-#define TEMPERATURE_SENSOR_CONFIG ArduinoTCSTemperatureSensor::Parameters({-999.0,999.0})
+#define TEMPERATURE_SENSOR_CONFIG ArduinoTCSTemperatureSensor::Parameters({-1000.0,1000.0})
 
 #define HEATER_ON_DEFAULT 3
 #define HEATER_HYSTERESIS_DEFAULT 3
diff --git a/mission/DeviceHandler/ArduinoComIF.cpp b/mission/DeviceHandler/ArduinoComIF.cpp
index e6d446f..8ec4ec8 100644
--- a/mission/DeviceHandler/ArduinoComIF.cpp
+++ b/mission/DeviceHandler/ArduinoComIF.cpp
@@ -1,7 +1,7 @@
 /*
  * ArduinoComIF.cpp
  *
- * Created on: 02/06/2021
+ * Last Modify: 20/09/2021
  * Author: Marco Modè
  *
  */
@@ -76,8 +76,8 @@ ReturnValue_t ArduinoComIF::initializeInterface(CookieIF *cookie) {
 	tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
 	tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
 
-	tty.c_cc[VTIME] = 16; // Wait for up to 16 ds (Arduino measurement cycle time), returning as soon as any data is received.
-	tty.c_cc[VMIN] = 255; // limit number of bytes which can be read in one time by Linux serial port
+	tty.c_cc[VTIME] = 0; // No blocking time (Arduino complete measurement cycle time is 16 ds)
+	tty.c_cc[VMIN] = 255; // Limit number of bytes which can be read in one time by Linux serial port
 
 	// Set in/out baud rate to be 115200 bps (same of Arduino)
 	cfsetispeed(&tty, B115200);
@@ -116,36 +116,50 @@ ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie,
 		uint8_t **buffer, size_t *size) {
 	ArduinoCookie *Cookie = dynamic_cast<ArduinoCookie*>(cookie);
 
-	// The buffer array to store the data read are initialized.
-	// The whole data packet to be read is 2580 bytes, but the limit for one reading (VMAX) is 255 bytes.
-	// Therefore the reading is divided in 11 separate stages which employ 11 different buffer array.
-	// At the end these 11 arrays are concatenated in the main buffer array read_buf.
-	uint8_t read_buf[2580]; // 2580 bytes from SPC serial output
-	uint8_t read_buf1[255]; // 255 bytes
-	uint8_t read_buf2[255]; // 255 bytes
-	uint8_t read_buf3[255]; // 255 bytes
-	uint8_t read_buf4[255]; // 255 bytes
-	uint8_t read_buf5[255]; // 255 bytes
-	uint8_t read_buf6[255]; // 255 bytes
-	uint8_t read_buf7[255]; // 255 bytes
-	uint8_t read_buf8[255]; // 255 bytes
-	uint8_t read_buf9[255]; // 255 bytes
-	uint8_t read_buf10[255]; // 255 bytes
-	uint8_t read_buf11[30]; // 30 bytes
+	/* Afterward, the buffer array to store the data read are initialized.
+	 * The whole data packet to be read is 2580 bytes, the limit for one reading (VMAX) is 255 bytes.
+	 * In order to avoid problems during the acquisition of data in the framework, the buffer is set to
+	 * be 3 times the needed dimension: 7740 bytes.
+	 * Exploiting then some control loop, the beginning of a full and complete buffer is identified.
+	 * Since the acquisition limit for one reading is of 255 bytes, as mentioned above,
+	 * the reading is divided in 31 separate stages which employ 31 different buffer array.
+	 * At the end these 31 arrays are concatenated in the main buffer array read_buf.
+	 */
+	uint8_t read_buf[7740]; // 7740 bytes from SPC serial output
+	// Intermediate buffers (limit for one single read call is 255 bytes)
+	uint8_t read_buf1[255]; uint8_t read_buf2[255]; uint8_t read_buf3[255];
+	uint8_t read_buf4[255]; uint8_t read_buf5[255]; uint8_t read_buf6[255];
+	uint8_t read_buf7[255]; uint8_t read_buf8[255]; uint8_t read_buf9[255];
+	uint8_t read_buf10[255]; uint8_t read_buf11[255]; uint8_t read_buf12[255];
+	uint8_t read_buf13[255]; uint8_t read_buf14[255]; uint8_t read_buf15[255];
+	uint8_t read_buf16[255]; uint8_t read_buf17[255]; uint8_t read_buf18[255];
+	uint8_t read_buf19[255]; uint8_t read_buf20[255]; uint8_t read_buf21[255];
+	uint8_t read_buf22[255]; uint8_t read_buf23[255]; uint8_t read_buf24[255];
+	uint8_t read_buf25[255]; uint8_t read_buf26[255]; uint8_t read_buf27[255];
+	uint8_t read_buf28[255]; uint8_t read_buf29[255]; uint8_t read_buf30[255];
+	uint8_t read_buf31[90];
 
 	// The buffer elements are initially set to 0.
 	memset(&read_buf, '\0', sizeof(read_buf));
-	memset(&read_buf1, '\0', sizeof(read_buf1));
-	memset(&read_buf2, '\0', sizeof(read_buf2));
-	memset(&read_buf3, '\0', sizeof(read_buf3));
-	memset(&read_buf4, '\0', sizeof(read_buf4));
-	memset(&read_buf5, '\0', sizeof(read_buf5));
-	memset(&read_buf6, '\0', sizeof(read_buf6));
-	memset(&read_buf7, '\0', sizeof(read_buf7));
-	memset(&read_buf8, '\0', sizeof(read_buf8));
-	memset(&read_buf9, '\0', sizeof(read_buf9));
-	memset(&read_buf10, '\0', sizeof(read_buf10));
-	memset(&read_buf11, '\0', sizeof(read_buf11));
+	memset(&read_buf1, '\0', 255); memset(&read_buf2, '\0', 255); memset(&read_buf3, '\0', 255);
+	memset(&read_buf4, '\0', 255); memset(&read_buf5, '\0', 255); memset(&read_buf6, '\0', 255);
+	memset(&read_buf7, '\0', 255); memset(&read_buf8, '\0', 255); memset(&read_buf9, '\0', 255);
+	memset(&read_buf10, '\0', 255); memset(&read_buf11, '\0', 255); memset(&read_buf12, '\0', 255);
+	memset(&read_buf13, '\0', 255); memset(&read_buf14, '\0', 255); memset(&read_buf15, '\0', 255);
+	memset(&read_buf16, '\0', 255); memset(&read_buf17, '\0', 255); memset(&read_buf18, '\0', 255);
+	memset(&read_buf19, '\0', 255); memset(&read_buf20, '\0', 255); memset(&read_buf21, '\0', 255);
+	memset(&read_buf22, '\0', 255); memset(&read_buf23, '\0', 255); memset(&read_buf24, '\0', 255);
+	memset(&read_buf25, '\0', 255); memset(&read_buf26, '\0', 255); memset(&read_buf27, '\0', 255);
+	memset(&read_buf28, '\0', 255); memset(&read_buf29, '\0', 255); memset(&read_buf30, '\0', 255);
+	memset(&read_buf31, '\0', 90);
+	/*memset(&read_buf, '\0', sizeof(read_buf));
+	memset(&read_buf1, '\0', 255); memset(&read_buf2, '\0', 255); memset(&read_buf3, '\0', 255);
+	memset(&read_buf4, '\0', 255); memset(&read_buf5, '\0', 255); memset(&read_buf6, '\0', 255);
+	memset(&read_buf7, '\0', 255); memset(&read_buf8, '\0', 255); memset(&read_buf9, '\0', 255);
+	memset(&read_buf10, '\0', 255); memset(&read_buf11, '\0', 255); memset(&read_buf12, '\0', 255);
+	memset(&read_buf13, '\0', 255); memset(&read_buf14, '\0', 255); memset(&read_buf15, '\0', 255);
+	memset(&read_buf16, '\0', 255); memset(&read_buf17, '\0', 255); memset(&read_buf18, '\0', 255);
+	memset(&read_buf19, '\0', 255); memset(&read_buf20, '\0', 255); memset(&read_buf21, '\0', 60);*/
 
 	// Read bytes. The behaviour of read() (e.g. does it block?, how long does it block for?)
 	// depends on the configuration settings above, specifically VMIN and VTIME.
@@ -160,9 +174,61 @@ ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie,
 	int num_bytes9 = read(Cookie->Serial_port_number, &read_buf9, sizeof(read_buf9));
 	int num_bytes10 = read(Cookie->Serial_port_number, &read_buf10, sizeof(read_buf10));
 	int num_bytes11 = read(Cookie->Serial_port_number, &read_buf11, sizeof(read_buf11));
+	int num_bytes12 = read(Cookie->Serial_port_number, &read_buf12, sizeof(read_buf12));
+	int num_bytes13 = read(Cookie->Serial_port_number, &read_buf13, sizeof(read_buf13));
+	int num_bytes14 = read(Cookie->Serial_port_number, &read_buf14, sizeof(read_buf14));
+	int num_bytes15 = read(Cookie->Serial_port_number, &read_buf15, sizeof(read_buf15));
+	int num_bytes16 = read(Cookie->Serial_port_number, &read_buf16, sizeof(read_buf16));
+	int num_bytes17 = read(Cookie->Serial_port_number, &read_buf17, sizeof(read_buf17));
+	int num_bytes18 = read(Cookie->Serial_port_number, &read_buf18, sizeof(read_buf18));
+	int num_bytes19 = read(Cookie->Serial_port_number, &read_buf19, sizeof(read_buf19));
+	int num_bytes20 = read(Cookie->Serial_port_number, &read_buf20, sizeof(read_buf20));
+	int num_bytes21 = read(Cookie->Serial_port_number, &read_buf21, sizeof(read_buf21));
+	int num_bytes22 = read(Cookie->Serial_port_number, &read_buf22, sizeof(read_buf22));
+	int num_bytes23 = read(Cookie->Serial_port_number, &read_buf23, sizeof(read_buf23));
+	int num_bytes24 = read(Cookie->Serial_port_number, &read_buf24, sizeof(read_buf24));
+	int num_bytes25 = read(Cookie->Serial_port_number, &read_buf25, sizeof(read_buf25));
+	int num_bytes26 = read(Cookie->Serial_port_number, &read_buf26, sizeof(read_buf26));
+	int num_bytes27 = read(Cookie->Serial_port_number, &read_buf27, sizeof(read_buf27));
+	int num_bytes28 = read(Cookie->Serial_port_number, &read_buf28, sizeof(read_buf28));
+	int num_bytes29 = read(Cookie->Serial_port_number, &read_buf29, sizeof(read_buf29));
+	int num_bytes30 = read(Cookie->Serial_port_number, &read_buf30, sizeof(read_buf30));
+	int num_bytes31 = read(Cookie->Serial_port_number, &read_buf31, sizeof(read_buf31));
 	int num_bytes = num_bytes1 + num_bytes2 + num_bytes3 + num_bytes4
 			+ num_bytes5 + num_bytes6 + num_bytes7 + num_bytes8
-			+ num_bytes9 + num_bytes10 + num_bytes11;
+			+ num_bytes9 + num_bytes10 + num_bytes11 + num_bytes12
+			+ num_bytes13 + num_bytes14 + num_bytes15 + num_bytes16
+			+ num_bytes17 + num_bytes18 + num_bytes19 + num_bytes20
+			+ num_bytes21 + num_bytes22 + num_bytes23 + num_bytes24
+			+ num_bytes25 + num_bytes26 + num_bytes27 + num_bytes28
+			+ num_bytes29 + num_bytes30 + num_bytes31;
+	/*int num_bytes1 = read(Cookie->Serial_port_number, &read_buf1, sizeof(read_buf1));
+	int num_bytes2 = read(Cookie->Serial_port_number, &read_buf2, sizeof(read_buf2));
+	int num_bytes3 = read(Cookie->Serial_port_number, &read_buf3, sizeof(read_buf3));
+	int num_bytes4 = read(Cookie->Serial_port_number, &read_buf4, sizeof(read_buf4));
+	int num_bytes5 = read(Cookie->Serial_port_number, &read_buf5, sizeof(read_buf5));
+	int num_bytes6 = read(Cookie->Serial_port_number, &read_buf6, sizeof(read_buf6));
+	int num_bytes7 = read(Cookie->Serial_port_number, &read_buf7, sizeof(read_buf7));
+	int num_bytes8 = read(Cookie->Serial_port_number, &read_buf8, sizeof(read_buf8));
+	int num_bytes9 = read(Cookie->Serial_port_number, &read_buf9, sizeof(read_buf9));
+	int num_bytes10 = read(Cookie->Serial_port_number, &read_buf10, sizeof(read_buf10));
+	int num_bytes11 = read(Cookie->Serial_port_number, &read_buf11, sizeof(read_buf11));
+	int num_bytes12 = read(Cookie->Serial_port_number, &read_buf12, sizeof(read_buf12));
+	int num_bytes13 = read(Cookie->Serial_port_number, &read_buf13, sizeof(read_buf13));
+	int num_bytes14 = read(Cookie->Serial_port_number, &read_buf14, sizeof(read_buf14));
+	int num_bytes15 = read(Cookie->Serial_port_number, &read_buf15, sizeof(read_buf15));
+	int num_bytes16 = read(Cookie->Serial_port_number, &read_buf16, sizeof(read_buf16));
+	int num_bytes17 = read(Cookie->Serial_port_number, &read_buf17, sizeof(read_buf17));
+	int num_bytes18 = read(Cookie->Serial_port_number, &read_buf18, sizeof(read_buf18));
+	int num_bytes19 = read(Cookie->Serial_port_number, &read_buf19, sizeof(read_buf19));
+	int num_bytes20 = read(Cookie->Serial_port_number, &read_buf20, sizeof(read_buf20));
+	int num_bytes21 = read(Cookie->Serial_port_number, &read_buf21, sizeof(read_buf21));
+	int num_bytes = num_bytes1 + num_bytes2 + num_bytes3 + num_bytes4
+			+ num_bytes5 + num_bytes6 + num_bytes7 + num_bytes8
+			+ num_bytes9 + num_bytes10 + num_bytes11 + num_bytes12
+			+ num_bytes13 + num_bytes14 + num_bytes15 + num_bytes16
+			+ num_bytes17 + num_bytes18 + num_bytes19 + num_bytes20
+			+ num_bytes21;*/
 
 	// The 11 buffer arrays are here concatenated in one single vector.
 	std::copy(read_buf1, read_buf1 + 255, read_buf);
@@ -175,7 +241,48 @@ ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie,
 	std::copy(read_buf8, read_buf8 + 255, read_buf + 7 * 255);
 	std::copy(read_buf9, read_buf9 + 255, read_buf + 8 * 255);
 	std::copy(read_buf10, read_buf10 + 255, read_buf + 9 * 255);
-	std::copy(read_buf11, read_buf11 + 30, read_buf + 10 * 255);
+	std::copy(read_buf11, read_buf11 + 255, read_buf + 10 * 255);
+	std::copy(read_buf12, read_buf12 + 255, read_buf + 11 * 255);
+	std::copy(read_buf13, read_buf13 + 255, read_buf + 12 * 255);
+	std::copy(read_buf14, read_buf14 + 255, read_buf + 13 * 255);
+	std::copy(read_buf15, read_buf15 + 255, read_buf + 14 * 255);
+	std::copy(read_buf16, read_buf16 + 255, read_buf + 15 * 255);
+	std::copy(read_buf17, read_buf17 + 255, read_buf + 16 * 255);
+	std::copy(read_buf18, read_buf18 + 255, read_buf + 17 * 255);
+	std::copy(read_buf19, read_buf19 + 255, read_buf + 18 * 255);
+	std::copy(read_buf20, read_buf20 + 255, read_buf + 19 * 255);
+	std::copy(read_buf21, read_buf21 + 255, read_buf + 20 * 255);
+	std::copy(read_buf22, read_buf22 + 255, read_buf + 21 * 255);
+	std::copy(read_buf23, read_buf23 + 255, read_buf + 22 * 255);
+	std::copy(read_buf24, read_buf24 + 255, read_buf + 23 * 255);
+	std::copy(read_buf25, read_buf25 + 255, read_buf + 24 * 255);
+	std::copy(read_buf26, read_buf26 + 255, read_buf + 25 * 255);
+	std::copy(read_buf27, read_buf27 + 255, read_buf + 26 * 255);
+	std::copy(read_buf28, read_buf28 + 255, read_buf + 27 * 255);
+	std::copy(read_buf29, read_buf29 + 255, read_buf + 28 * 255);
+	std::copy(read_buf30, read_buf30 + 255, read_buf + 29 * 255);
+	std::copy(read_buf31, read_buf31 + 90, read_buf + 30 * 255);
+	/*std::copy(read_buf1, read_buf1 + 255, read_buf);
+	std::copy(read_buf2, read_buf2 + 255, read_buf + 255);
+	std::copy(read_buf3, read_buf3 + 255, read_buf + 2 * 255);
+	std::copy(read_buf4, read_buf4 + 255, read_buf + 3 * 255);
+	std::copy(read_buf5, read_buf5 + 255, read_buf + 4 * 255);
+	std::copy(read_buf6, read_buf6 + 255, read_buf + 5 * 255);
+	std::copy(read_buf7, read_buf7 + 255, read_buf + 6 * 255);
+	std::copy(read_buf8, read_buf8 + 255, read_buf + 7 * 255);
+	std::copy(read_buf9, read_buf9 + 255, read_buf + 8 * 255);
+	std::copy(read_buf10, read_buf10 + 255, read_buf + 9 * 255);
+	std::copy(read_buf11, read_buf11 + 255, read_buf + 10 * 255);
+	std::copy(read_buf12, read_buf12 + 255, read_buf + 11 * 255);
+	std::copy(read_buf13, read_buf13 + 255, read_buf + 12 * 255);
+	std::copy(read_buf14, read_buf14 + 255, read_buf + 13 * 255);
+	std::copy(read_buf15, read_buf15 + 255, read_buf + 14 * 255);
+	std::copy(read_buf16, read_buf16 + 255, read_buf + 15 * 255);
+	std::copy(read_buf17, read_buf17 + 255, read_buf + 16 * 255);
+	std::copy(read_buf18, read_buf18 + 255, read_buf + 17 * 255);
+	std::copy(read_buf19, read_buf19 + 255, read_buf + 18 * 255);
+	std::copy(read_buf20, read_buf20 + 255, read_buf + 19 * 255);
+	std::copy(read_buf21, read_buf21 + 60, read_buf + 20 * 255);*/
 
 	// num_bytes is the number of bytes read (n=0: no bytes received, n=-1: error).
 	if (num_bytes < 0) {
@@ -185,9 +292,48 @@ ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie,
 
 	printf("Read %i bytes.\n", num_bytes);
 
+	// Loop to define the adress of the buffer start
+	int first_index = 0;
+	while (first_index < 7740) {
+		if (read_buf[first_index] == 91){
+			if (read_buf[first_index + 3] == 83) {
+				if (read_buf[first_index + 9] == 1) {
+					if (read_buf[first_index + 27*35 + 9] == 39) {
+						if (read_buf[first_index + 27*35 + 22] == 69) {
+							if (read_buf[first_index + 27*45 + 91*13 + 9] == 122) {
+								if (read_buf[first_index + 27*45 + 91*14 + 9] == 123) {
+									if (read_buf[first_index + 27*45 + 91*14 + 86] == 69) {
+										break;
+									} else {
+										first_index++;
+									}
+								} else {
+									first_index++;
+								}
+							} else {
+								first_index++;
+							}
+						} else {
+							first_index++;
+						}
+					} else {
+						first_index++;
+					}
+				} else {
+					first_index++;
+				}
+			} else {
+				first_index++;
+			}
+		} else {
+			first_index++;
+		}
+	}
+
 	// Definition of buffer array and buffer size to return after reading.
-	*size = 2580;
-	*buffer = read_buf;
+	*size = num_bytes;
+	*buffer = &read_buf[first_index];
+
 
 	return RETURN_OK;
 
diff --git a/mission/DeviceHandler/ArduinoComIF.h b/mission/DeviceHandler/ArduinoComIF.h
index 913d2e9..3e32e2b 100644
--- a/mission/DeviceHandler/ArduinoComIF.h
+++ b/mission/DeviceHandler/ArduinoComIF.h
@@ -1,7 +1,7 @@
 /*
  * Title: ArduinoComIF.h
  *
- * Created on: 02/06/2021
+ * Last Modify: 20/09/2021
  * Author: Marco Modè
  *
  */
@@ -9,14 +9,11 @@
 #ifndef MISSION_DEVICEHANDLER_ARDUINOCOMIF_H_
 #define MISSION_DEVICEHANDLER_ARDUINOCOMIF_H_
 
-
 #include <fsfw/devicehandlers/DeviceCommunicationIF.h>
 #include <fsfw/objectmanager/SystemObject.h>
 #include <fsfw/ipc/MessageQueueIF.h>
 #include <fsfw/tmtcservices/AcceptsTelemetryIF.h>
-
 #include <vector>
-
 #include <iostream>
 #include <stdio.h>
 #include <string.h>
@@ -24,9 +21,13 @@
 #include <map>
 
 /**
- * @brief Used to simply return sent data from device handler
- * @details Assign this com IF in the factory when creating the device handler
- * @ingroup test
+ * @brief Used to initialize the communication Interface (USB connector, serial port) and
+ * 		  to perform the 4 periodical function described in the DeviceCommunicationIF.
+ * 		  In this case sendMessage, getSendSuccess and requestReceiveMessage are
+ * 		  just returning RETURN_OK.
+ * @details The ArduinoComIF is instantiated in the generic factory.
+ * @author: Marco Modè
+ * @ingroup mission/DeviceHandler
  */
 
 class ArduinoComIF: public DeviceCommunicationIF, public SystemObject {
@@ -34,16 +35,15 @@ public:
 	ArduinoComIF(object_id_t objectId);
 	virtual ~ArduinoComIF();
 
-	ReturnValue_t initializeInterface(CookieIF * cookie) override;
-	ReturnValue_t sendMessage(CookieIF *cookie, const uint8_t * sendData,
-	        size_t sendLen) override;
+	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 requestReceiveMessage(CookieIF *cookie, size_t requestLen)
+			override;
 	ReturnValue_t readReceivedMessage(CookieIF *cookie, uint8_t **buffer,
-	        size_t *size) override;
+			size_t *size) override;
 
 };
 
-
 #endif /* MISSION_DEVICEHANDLER_ARDUINOCOMIF_H_ */
diff --git a/mission/DeviceHandler/ArduinoCookie.cpp b/mission/DeviceHandler/ArduinoCookie.cpp
index 73ee303..bc5e98d 100644
--- a/mission/DeviceHandler/ArduinoCookie.cpp
+++ b/mission/DeviceHandler/ArduinoCookie.cpp
@@ -1,16 +1,18 @@
 /*
  *  ArduinoCookie.cpp
  *
- * Created on: 02/06/2021
+ * Last Modify: 20/09/2021
  * Author: Marco Modè
  *
  */
 
 #include <mission/DeviceHandler/ArduinoCookie.h>
 
-//The cookie tells to device handler which is the device to communicate with if more
-// devices are connected. In this case only one device, the Arduino motherboard, is
-// connected. Therefore the cookie adress and MaxLen are set to default.
+//The cookie tells to device handler which is the device to communicate with, if more
+// devices are connected. In this case only one device, the Arduino board, is
+// connected.
 
-ArduinoCookie::ArduinoCookie() {}
-ArduinoCookie::~ArduinoCookie() {}
+ArduinoCookie::ArduinoCookie() {
+}
+ArduinoCookie::~ArduinoCookie() {
+}
diff --git a/mission/DeviceHandler/ArduinoCookie.h b/mission/DeviceHandler/ArduinoCookie.h
index fdbc64e..fc3de3a 100644
--- a/mission/DeviceHandler/ArduinoCookie.h
+++ b/mission/DeviceHandler/ArduinoCookie.h
@@ -1,7 +1,7 @@
 /*
  * ArduinoCookie.h
  *
- * Created on: 02/06/2021
+ * Last Modify: 20/09/2021
  * Author: Marco Modè
  *
  */
@@ -9,21 +9,24 @@
 #ifndef MISSION_DEVICEHANDLER_ARDUINOCOOKIE_H_
 #define MISSION_DEVICEHANDLER_ARDUINOCOOKIE_H_
 
-
 #include <fsfw/devicehandlers/CookieIF.h>
 #include <cstddef>
 
 /**
  * @brief	Simple cookie which initialize the variables
  * 			for the Linux serial port.
+ * @details The ArduinoCookie is instantiated in the generic factory.
+ * @author: Marco Modè
+ * @ingroup mission/DeviceHandler
  */
 class ArduinoCookie: public CookieIF {
 public:
 	ArduinoCookie();
 	virtual ~ArduinoCookie();
 
+	// Serial port for the communication with Arduino board is here initialized.
+	// It will be exploited in the ComIF to manage the computer serial port.
 	int Serial_port_number;
 };
 
-
 #endif /* MISSION_DEVICEHANDLER_ARDUINOCOOKIE_H_ */
diff --git a/mission/DeviceHandler/ArduinoDeviceHandler.cpp b/mission/DeviceHandler/ArduinoDeviceHandler.cpp
index 2abaad8..8f626a9 100644
--- a/mission/DeviceHandler/ArduinoDeviceHandler.cpp
+++ b/mission/DeviceHandler/ArduinoDeviceHandler.cpp
@@ -1,7 +1,7 @@
 /*
  * DeviceHandler.cpp
  *
- * Created on: 02/06/2021
+ * Last Modify: 20/09/2021
  * Author: Marco Modè
  *
  */
@@ -12,6 +12,8 @@
 #include <fsfw/datapool/PoolVector.h>
 #include <bsp_linux/fsfwconfig/datapool/dataPoolInit.h>
 #include <cstdlib>
+#include <map>
+//#include <iostream>
 
 
 ArduinoDH::ArduinoDH(object_id_t objectId, object_id_t comIF, CookieIF *cookie) :
@@ -41,6 +43,7 @@ ReturnValue_t ArduinoDH::buildTransitionDeviceCommand(DeviceCommandId_t *id) {
 
 void ArduinoDH::doTransition(Mode_t modeFrom, Submode_t submodeFrom) {
 	if (mode == _MODE_TO_NORMAL) {
+		setMode(_MODE_TO_NORMAL);
 		std::cout<<"Arduino device is in Normal mode"<<std::endl;
 	} else {
 		setMode(_MODE_TO_NORMAL);
@@ -58,43 +61,51 @@ void ArduinoDH::fillCommandAndReplyMap() {}
 ReturnValue_t ArduinoDH::scanForReply(const uint8_t *start, size_t len,
 		DeviceCommandId_t *foundId, size_t *foundLen) {
 
-	//Unless a command was sent explicitely, we don't expect any replies and ignore
-	//the packet. On a real device, there might be replies which are sent without a previous
-	//command.
+	// In this case the data are sent from the Arduino board without any command.
+	// No replies of data received are required.
+	// This function checks the validity of the data packet received.
 
-	*foundLen = 2580;
+	//*foundLen = 2580;
+	//*foundId = 0xFFFFFF; // assigned here as test
+	*foundLen = len;
+	//DeviceCommandId_t *foundId = 0xFFFFFF;
+	std::map <DeviceCommandId_t, size_t> id {{*foundId, len}};
+	//*foundId = id;
 
 	// check validity
 	if (len == *foundLen){
 		// start character: '['
 		if (*start == 91 ){
-			return APERIODIC_REPLY;
-		} else{
+			// fourth character: 'S' (First variable is the char [ChStr])
+			if (*(start + 3) == 83) {
+				// first channel must be channel 1 (SPCChNumber: 1)
+				if (*(start + 9) == 1) {
+					return APERIODIC_REPLY;
+				} else {
+					return DeviceHandlerIF::LENGTH_MISSMATCH;
+				}
+			} else {
+				return DeviceHandlerIF::LENGTH_MISSMATCH;
+			}
+		} else {
 			return DeviceHandlerIF::LENGTH_MISSMATCH;
 		}
 	} else {
 		return IGNORE_REPLY_DATA;
 	}
-	/*if (len == *foundLen){
-		// start character: '['
-		if (*start == 91 ){
-			if (*start + 9 == 49) {
-				return APERIODIC_REPLY;
-			}
-		} else{
-			return DeviceHandlerIF::LENGTH_MISSMATCH;
-		}
-	} else {
-		return IGNORE_REPLY_DATA;
-	}*/
 }
 
 ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 		const uint8_t *packet) {
 	
+	DataSet ArduinoDataSet;
+
 	//sif::debug<<"DEBUG_DH: interprete for reply"<<std::endl;
 	// The data stored in the read buffer are here copied in the variables with the SPC format.
 	// After copying, the data of temperature, environment and accelerometer are stored in three separated vectors.
+	printf(
+			"\n***********************************************************************************************\n");
+	printf("TEMPERATURE parameters are: ");
 	for (int i = 0; i < 36; i++) {
 		memcpy(&Temp_ch.start_string, &packet[27 * i + 0], 8);
 		memcpy(&Temp_ch.Typ, &packet[27 * i + 8], 1);
@@ -103,9 +114,20 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 		memcpy(&Temp_ch.temperature, &packet[27 * i + 11], 4);
 		memcpy(&Temp_ch.Timestamp, &packet[27 * i + 15], 4);
 		memcpy(&Temp_ch.end_string, &packet[27 * i + 19], 8);
+		// The data are here printed (useful for tests).
+		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);
 	}
-	for (int j = 0; j < 9; j++) {
+	printf(
+			"\n\n***********************************************************************************************\n");
+	printf("ENVIRONMENTAL parameters are: ");
+		for (int j = 0; j < 9; j++) {
 		memcpy(&Env_ch.start_string, &packet[27 * (36 + j) + 0], 8);
 		memcpy(&Env_ch.Typ, &packet[27 * (36 + j) + 8], 1);
 		memcpy(&Env_ch.SPCChNumber, &packet[27 * (36 + j) + 9], 1);
@@ -113,136 +135,85 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 		memcpy(&Env_ch.Value, &packet[27 * (36 + j) + 11], 4);
 		memcpy(&Env_ch.Timestamp, &packet[27 * (36 + j) + 15], 4);
 		memcpy(&Env_ch.end_string, &packet[27 * (36 + j) + 19], 8);
+		// The data are here printed (useful for tests).
+		if (j == 0 || j == 3 || j == 6){
+			printf("\n\nHUMIDITY: ");
+		}
+		else if (j == 1 || j == 4 || j == 7) {
+			printf("\n\nPRESSURE: ");
+		}
+		else {
+			printf("\n\nTEMPERATURE: ");
+		}
+		printf("\n\nStart: %7s", Env_ch.start_string);
+		printf("\nTyp: %u", Env_ch.Typ);
+		printf("\nSPCChNumber: %u", Env_ch.SPCChNumber);
+		printf("\nValue_Cnt: %u", Env_ch.Value_Cnt);
+		printf("\nTemperature: %f", Env_ch.Value);
+		printf("\nTimestamp: %u", Env_ch.Timestamp);
+		printf("\nEnd: %7s", Env_ch.end_string);
 		vecEnv.emplace_back(Env_ch);
 	}
-	for (int k = 0; k < 15; k++) {
-		memcpy(&Acc_ch.start_string, &packet[27 * (36 + 9) + 91 * k + 0], 8);
-		memcpy(&Acc_ch.Typ, &packet[27 * (36 + 9) + 91 * k + 8], 1);
-		memcpy(&Acc_ch.SPCChNumber, &packet[27 * (36 + 9) + 91 * k + 9], 1);
-		memcpy(&Acc_ch.Value_Cnt, &packet[27 * (36 + 9) + 91 * k + 10], 1);
-		memcpy(&Acc_ch.Value, &packet[27 * (36 + 9) + 91 * k + 11], 36);
-		memcpy(&Acc_ch.Timestamp, &packet[27 * (36 + 9) + 91 * k + 47], 36);
-		memcpy(&Acc_ch.end_string, &packet[27 * (36 + 9) + 91 * k + 83], 8);
-		vecAcc.emplace_back(Acc_ch);
-	}
-	
-	// All data are here printed to monitor from the three vectors of data measurements.
-	/*printf(
-			"\n***********************************************************************************************\n");
-	printf("TEMPERATURE parameters are: ");
-
-	for (int i = 0; i < 36; i++) {
-		printf("\n\nStart: %7s", vecTemp[i].start_string);
-		printf("\nTyp: %u", vecTemp[i].Typ);
-		printf("\nSPCChNumber: %u", vecTemp[i].SPCChNumber);
-		printf("\nValue_Cnt: %u", vecTemp[i].Value_Cnt);
-		printf("\nTemperature: %f", vecTemp[i].temperature);
-		printf("\nTimestamp: %u", vecTemp[i].Timestamp);
-		printf("\nEnd: %7s", vecTemp[i].end_string);
-	}
-	printf(
-			"\n\n***********************************************************************************************\n");
-	printf("ENVIRONMENTAL parameters are: ");
-
-	for (int j = 0; j < 3; j++) {
-		printf("\n\nHUMIDITY: ");
-		printf("\nStart: %7s", vecEnv[3 * j].start_string);
-		printf("\nTyp: %u", vecEnv[3 * j].Typ);
-		printf("\nSPCChNumber: %u", vecEnv[3 * j].SPCChNumber);
-		printf("\nValue_Cnt: %u", vecEnv[3 * j].Value_Cnt);
-		printf("\nValue: %f", vecEnv[3 * j].Value);
-		printf("\nTimestamp: %u", vecEnv[3 * j].Timestamp);
-		printf("\nEnd: %7s", vecEnv[3 * j].end_string);
-
-		printf("\n\nPRESSURE: ");
-		printf("\nStart: %7s", vecEnv[3 * j + 1].start_string);
-		printf("\nTyp: %u", vecEnv[3 * j + 1].Typ);
-		printf("\nSPCChNumber: %u", vecEnv[3 * j + 1].SPCChNumber);
-		printf("\nValue_Cnt: %u", vecEnv[3 * j + 1].Value_Cnt);
-		printf("\nValue: %f", vecEnv[3 * j + 1].Value);
-		printf("\nTimestamp: %u", vecEnv[3 * j + 1].Timestamp);
-		printf("\nEnd: %7s", vecEnv[3 * j + 1].end_string);
-
-		printf("\n\nTEMPERATURE: ");
-		printf("\nStart: %7s", vecEnv[3 * j + 2].start_string);
-		printf("\nTyp: %u", vecEnv[3 * j + 2].Typ);
-		printf("\nSPCChNumber: %u", vecEnv[3 * j + 2].SPCChNumber);
-		printf("\nValue_Cnt: %u", vecEnv[3 * j + 2].Value_Cnt);
-		printf("\nValue: %f", vecEnv[3 * j + 2].Value);
-		printf("\nTimestamp: %u", vecEnv[3 * j + 2].Timestamp);
-		printf("\nEnd: %7s", vecEnv[3 * j + 2].end_string);
-
-	}
 	printf(
 			"\n\n***********************************************************************************************\n");
 	printf("ACCELEROMETER parameters are: ");
-
-	for (int k = 0; k < 5; k++) {
+	for (int k = 0; k < 15; k++) {
+		memcpy(&Ornt_ch.start_string, &packet[27 * (36 + 9) + 91 * k + 0], 8);
+		memcpy(&Ornt_ch.Typ, &packet[27 * (36 + 9) + 91 * k + 8], 1);
+		memcpy(&Ornt_ch.SPCChNumber, &packet[27 * (36 + 9) + 91 * k + 9], 1);
+		memcpy(&Ornt_ch.Value_Cnt, &packet[27 * (36 + 9) + 91 * k + 10], 1);
+		memcpy(&Ornt_ch.Value, &packet[27 * (36 + 9) + 91 * k + 11], 36);
+		memcpy(&Ornt_ch.Timestamp, &packet[27 * (36 + 9) + 91 * k + 47], 36);
+		memcpy(&Ornt_ch.end_string, &packet[27 * (36 + 9) + 91 * k + 83], 8);
+		// The data are here printed (useful for tests).
 		switch (k) {
 		case 0:
 			printf("\n\nACCELERATION: ");
 			break;
-		case 1:
+		case 3:
 			printf("\n\nGYROSCOPE: ");
 			break;
-		case 2:
+		case 6:
 			printf("\n\nMAGNETOMETER: ");
 			break;
-		case 3:
+		case 9:
 			printf("\n\nLINEAR ACCELERATION: ");
 			break;
-		case 4:
+		case 12:
 			printf("\n\nEULER ANGLES: ");
 			break;
 		}
-		printf("\n\nX ==> ");
-		printf("\nStart: %7s", vecAcc[3 * k].start_string);
-		printf("\nTyp: %u", vecAcc[3 * k].Typ);
-		printf("\nSPCChNumber: %u", vecAcc[3 * k].SPCChNumber);
-		printf("\nValue_Cnt: %u", vecAcc[3 * k].Value_Cnt);
+		if (k == 0 || k == 3 || k == 6 || k == 9 || k == 12){
+			printf("\n\nX ==> ");
+		}
+		else if (k == 1 || k == 4 || k == 7 || k == 10 || k == 13) {
+			printf("\n\nY ==> ");
+		}
+		else {
+			printf("\n\nZ ==> ");
+		}
+		printf("\nStart: %7s", Ornt_ch.start_string);
+		printf("\nTyp: %u", Ornt_ch.Typ);
+		printf("\nSPCChNumber: %u", Ornt_ch.SPCChNumber);
+		printf("\nValue_Cnt: %u", Ornt_ch.Value_Cnt);
 		for (int i = 0; i < 9; i++) {
 			printf("\nMeasurement number: %d", i);
-			printf("\nValue: %f", vecAcc[3 * k].Value[i]);
-			printf("\nTimestamp: %u", vecAcc[3 * k].Timestamp[i]);
+			printf("\nValue: %f", Ornt_ch.Value[i]);
+			printf("\nTimestamp: %u", Ornt_ch.Timestamp[i]);
 		}
-		printf("\nEnd: %7s", vecAcc[3 * k].end_string);
-
-		printf("\n\nY ==> ");
-		printf("\nStart: %7s", vecAcc[3 * k + 1].start_string);
-		printf("\nTyp: %u", vecAcc[3 * k + 1].Typ);
-		printf("\nSPCChNumber: %u", vecAcc[3 * k + 1].SPCChNumber);
-		printf("\nValue_Cnt: %u", vecAcc[3 * k + 1].Value_Cnt);
-		for (int i = 0; i < 9; i++) {
-			printf("\nMeasurement number: %d", i);
-			printf("\nValue: %f", vecAcc[3 * k + 1].Value[i]);
-			printf("\nTimestamp: %u", vecAcc[3 * k + 1].Timestamp[i]);
-		}
-		printf("\nEnd: %7s", vecAcc[3 * k + 1].end_string);
-
-		printf("\n\nZ ==> ");
-		printf("\nStart: %7s", vecAcc[3 * k + 2].start_string);
-		printf("\nTyp: %u", vecAcc[3 * k + 2].Typ);
-		printf("\nSPCChNumber: %u", vecAcc[3 * k + 2].SPCChNumber);
-		printf("\nValue_Cnt: %u", vecAcc[3 * k + 2].Value_Cnt);
-		for (int i = 0; i < 9; i++) {
-			printf("\nMeasurement number: %d", i);
-			printf("\nValue: %f", vecAcc[3 * k + 2].Value[i]);
-			printf("\nTimestamp: %u", vecAcc[3 * k + 2].Timestamp[i]);
-		}
-		printf("\nEnd: %7s", vecAcc[3 * k + 2].end_string);
-	}*/
+		printf("\nEnd: %7s", Ornt_ch.end_string);
+		vecOrnt.emplace_back(Ornt_ch);
+	}//*/
 
 	std::cout << "\n\nEnd reading data.\n" << std::endl;
 
 	// The data are here written to the data pool where they would be available to be used for other objects
 	
-	DataSet ArduinoDataSet;
-	
-	PoolVector <float, 36> TempValueVec(datapool::Temperature_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
+	/*PoolVector <float, 36> TempValueVec(datapool::Temperature_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
 	for (int i = 0; i < 36; i++) {
 		memcpy(&TempValueVec[i], &vecTemp[i].temperature, 4);
 	}
-	ArduinoDataSet.commit(PoolVariableIF::VALID);
+	ArduinoDataSet.commit(PoolVariableIF::VALID);*/
 	
 
 	/*PoolVector <unsigned int, 36> TempTimeVec(datapool::Temperature_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
@@ -269,14 +240,14 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 
 	PoolVector <float, 15> AccValueVec(datapool::Accelerometer_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
 	for (int k = 0; k < 15; k++) {
-		memcpy(&AccValueVec[k], &vecAcc[k].Value, 36);
+		memcpy(&AccValueVec[k], &vecOrnt[k].Value, 36);
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
 	sif::debug<<"\nDEBUG_DHk1: End of copy to datapool"<<std::endl;
 	
 	PoolVector <unsigned int, 15> AccTimeVec(datapool::Accelerometer_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
 	for (int k = 0; k < 15; k++) {
-		memcpy(&AccTimeVec[k], &vecAcc[k].Timestamp, 36);
+		memcpy(&AccTimeVec[k], &vecOrnt[k].Timestamp, 36);
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
 	sif::debug<<"\nDEBUG_DHk2: End of copy to datapool"<<std::endl;*/
diff --git a/mission/DeviceHandler/ArduinoDeviceHandler.h b/mission/DeviceHandler/ArduinoDeviceHandler.h
index 25b79df..5245e0b 100644
--- a/mission/DeviceHandler/ArduinoDeviceHandler.h
+++ b/mission/DeviceHandler/ArduinoDeviceHandler.h
@@ -1,7 +1,7 @@
 /*
  * DeviceHandler.h
  *
- * Created on: 02/06/2021
+ * Last Modify: 20/09/2021
  * Author: Marco Modè
  *
  */
@@ -15,19 +15,13 @@
 #include <fsfw/timemanager/Countdown.h>
 
 /**
- * @brief 	Basic device handler to test device commanding without a physical device.
- * @details
- * This test device handler provided a basic demo for the device handler object.
- * It can also be commanded with the following PUS services, using
- * the specified object ID of the test device handler.
- *
- * 	1. PUS Service 8 - Functional commanding
- * 	2. PUS Service 2 - Device access, raw commanding
- * 	3. PUS Service 20 - Parameter Management
- * 	4. PUS Service 3 - Housekeeping
-
- * @author	R. Mueller
- * @ingroup devices
+ * @brief 	Basic device handler to manage the communication with the Arduino sensor board.
+ * 	 		The data are sent to the serial port of the computer from the Arduino board.
+ * 	 		The device handler read and manages these data exploiting the ComIF and the
+ * 	 		DeviceHandlerBase functions.
+ * @details The ArduinoDH object is instantiated in the generic factory.
+ * @author	Marco Modè
+ * @ingroup mission/DeviceHandler
  */
 class ArduinoDH: public DeviceHandlerBase {
 public:
@@ -44,10 +38,11 @@ public:
 	ArduinoDH(object_id_t objectId, object_id_t comIF, CookieIF *cookie);
 	virtual ~ ArduinoDH();
 
-	// Definiton of data structure for SPC communication. Three different structures are defined for measurements of:
-	// - Temperature,
-	// - Environmental data,
-	// - Accelerometer data.
+	/* Definiton of data structure for SPC communication. Three different structures are defined for measurements of:
+	 * - Temperature data,
+	 * - Environmental data,
+	 * - Orientation data.
+	 */
 	struct Temperature {
 		char start_string[8];
 		uint8_t Typ;
@@ -84,7 +79,7 @@ public:
 			strncpy(end_string, _end_string, sizeof(end_string) - 1);
 		}
 	};
-	struct Accelerometer {
+	struct Orientation {
 		char start_string[8];
 		uint8_t Typ;
 		uint8_t SPCChNumber;
@@ -92,8 +87,8 @@ public:
 		float Value[9]; //max buffer
 		unsigned int Timestamp[9]; //max buffer
 		char end_string[8];
-		Accelerometer() = default;
-		Accelerometer(const char *_start_string, uint8_t _Typ,
+		Orientation() = default;
+		Orientation(const char *_start_string, uint8_t _Typ,
 				uint8_t _SPCChNumber, uint8_t _Value_Cnt, const float *_Value,
 				const unsigned int *_Timestamp, const char *_end_string) :
 				Typ(_Typ), SPCChNumber(_SPCChNumber), Value_Cnt(_Value_Cnt) {
@@ -108,7 +103,7 @@ public:
 	// during the phase of reading copying data from the buffers
 	std::vector<Temperature> vecTemp;
 	std::vector<Environmental> vecEnv;
-	std::vector<Accelerometer> vecAcc;
+	std::vector<Orientation> vecOrnt;
 
 	// Three dummy child structures are defined. They are used to store the three
 	// different types of data during the measurement loop and then the data are
@@ -118,11 +113,13 @@ public:
 	// using the three different structures.
 	Temperature Temp_ch;
 	Environmental Env_ch;
-	Accelerometer Acc_ch;
-
+	Orientation Ornt_ch;
 
 protected:
 
+	//#define BUFFER_ID(id, value, msg);
+
+	// DeviceHandlerBase inherited functions.
 	virtual void doStartUp() override;
 	virtual void doShutDown() override;
 	virtual ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id)