From 8c80e9f7c039ef5476b7865109d41c930d86afe2 Mon Sep 17 00:00:00 2001
From: mmode <marco.mode@mail.polimi.it>
Date: Tue, 14 Sep 2021 13:16:26 +0200
Subject: [PATCH] Errors fixed.

---
 bsp_linux/core/InitMission.cpp                |  4 +-
 fsfw/devicehandlers/DeviceHandlerBase.cpp     | 12 ++++-
 mission/DeviceHandler/ArduinoComIF.cpp        |  1 +
 .../DeviceHandler/ArduinoDeviceHandler.cpp    | 49 ++++++++++---------
 4 files changed, 39 insertions(+), 27 deletions(-)

diff --git a/bsp_linux/core/InitMission.cpp b/bsp_linux/core/InitMission.cpp
index 2656103..dfc1c17 100644
--- a/bsp_linux/core/InitMission.cpp
+++ b/bsp_linux/core/InitMission.cpp
@@ -120,7 +120,7 @@ void InitMission::createTasks(){
 	
 	PeriodicTaskIF* controllerTask = TaskFactory::instance()->
 			createPeriodicTask("CONTROLLER_TASK",40,
-					PeriodicTaskIF::MINIMUM_STACK_SIZE, 1, nullptr);
+					PeriodicTaskIF::MINIMUM_STACK_SIZE, 2, nullptr);
 	//result = pollingSequenceControllerFunction(controllerTask);
 	result = controllerTask->addComponent(objects::THERMAL_CONTROLLER);
 	if(result != HasReturnvaluesIF::RETURN_OK) {
@@ -156,7 +156,7 @@ void InitMission::createTasks(){
 	udpBridgeTask->startTask();
 	udpPollingTask->startTask();
 	//serializeTask->startTask();
-	//arduinoTask->startTask();
+	arduinoTask->startTask();
 	controllerTask->startTask();
 
 	//payloadTask->startTask();
diff --git a/fsfw/devicehandlers/DeviceHandlerBase.cpp b/fsfw/devicehandlers/DeviceHandlerBase.cpp
index 0199eb5..2bc62d2 100644
--- a/fsfw/devicehandlers/DeviceHandlerBase.cpp
+++ b/fsfw/devicehandlers/DeviceHandlerBase.cpp
@@ -67,6 +67,7 @@ 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();
@@ -82,17 +83,22 @@ 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;
@@ -663,10 +669,10 @@ void DeviceHandlerBase::parseReply(const uint8_t* receivedData,
 			handleReply(receivedData, foundId, foundLen);
 			break;
 		case APERIODIC_REPLY: {
-			sif::debug<<" DH: debug1"<<std::endl;
+			sif::debug<<" DH_base: debug1"<<std::endl;
 			result = interpretDeviceReply(foundId, receivedData);
 			if (result != RETURN_OK) {
-				sif::debug<<" DH: debug2"<<std::endl;
+				sif::debug<<" DH_base: debug2"<<std::endl;
 				replyRawReplyIfnotWiretapped(receivedData, foundLen);
 				triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result,
 						foundId);
@@ -686,7 +692,9 @@ 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/DeviceHandler/ArduinoComIF.cpp b/mission/DeviceHandler/ArduinoComIF.cpp
index 73ca7a4..dd64996 100644
--- a/mission/DeviceHandler/ArduinoComIF.cpp
+++ b/mission/DeviceHandler/ArduinoComIF.cpp
@@ -200,6 +200,7 @@ ReturnValue_t ArduinoComIF::readReceivedMessage(CookieIF *cookie,
 	buffer = &buf_ptr;*/
 	*buffer = read_buf;
 
+	close(Cookie->Serial_port_number);
 	return RETURN_OK;
 
 }
diff --git a/mission/DeviceHandler/ArduinoDeviceHandler.cpp b/mission/DeviceHandler/ArduinoDeviceHandler.cpp
index 9b623e4..45c9c63 100644
--- a/mission/DeviceHandler/ArduinoDeviceHandler.cpp
+++ b/mission/DeviceHandler/ArduinoDeviceHandler.cpp
@@ -52,7 +52,7 @@ void ArduinoDH::doTransition(Mode_t modeFrom, Submode_t submodeFrom) {
 	if (mode == _MODE_TO_NORMAL) {
 		std::cout<<"Arduino device -> Transition to Normal mode"<<std::endl;
 	} else {
-		DeviceHandlerBase::doTransition(modeFrom, submodeFrom);
+		setMode(_MODE_TO_NORMAL);
 	}
 }
 
@@ -126,11 +126,13 @@ 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);
-		vecTemp.emplace_back(Temp_ch.start_string, Temp_ch.Typ,
+		/*vecTemp.emplace_back(Temp_ch.start_string, Temp_ch.Typ,
 				Temp_ch.SPCChNumber, Temp_ch.Value_Cnt, Temp_ch.temperature,
-				Temp_ch.Timestamp, Temp_ch.end_string);
+				Temp_ch.Timestamp, Temp_ch.end_string);*/
+		vecTemp.emplace_back(Temp_ch);
 	}
-	for (int j = 0; j < 9; j++) {
+
+	/*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);
@@ -138,9 +140,10 @@ 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);
-		vecEnv.emplace_back(Env_ch.start_string, Env_ch.Typ, Env_ch.SPCChNumber,
-				Env_ch.Value_Cnt, Env_ch.Value, Env_ch.Timestamp,
-				Env_ch.end_string);
+		//vecEnv.emplace_back(Env_ch.start_string, Env_ch.Typ, Env_ch.SPCChNumber,
+				//Env_ch.Value_Cnt, Env_ch.Value, Env_ch.Timestamp,
+				//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);
@@ -150,10 +153,11 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 		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.start_string, Acc_ch.Typ, Acc_ch.SPCChNumber,
-				Acc_ch.Value_Cnt, Acc_ch.Value, Acc_ch.Timestamp,
-				Acc_ch.end_string);
-	}
+		//vecAcc.emplace_back(Acc_ch.start_string, Acc_ch.Typ, Acc_ch.SPCChNumber,
+				//Acc_ch.Value_Cnt, Acc_ch.Value, Acc_ch.Timestamp,
+				//Acc_ch.end_string);
+		vecAcc.emplace_back(Acc_ch);
+	}*/
 	
 	// All data are here printed to monitor from the three vectors of data measurements.
 	
@@ -270,14 +274,14 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 	
 	PoolVector <float, 36> TempValueVec(datapool::Temperature_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
 	for (int i = 0; i < 36; i++) {
-		memcpy(&TempValueVec[i], &vecTemp[27 * i + 11], 4);
+		memcpy(&TempValueVec[i], &vecTemp[i].temperature, 4);
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
 	
-	/*
-	PoolVector <unsigned int, 36> TempTimeVec(datapool::Temperature_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
+
+	/*PoolVector <unsigned int, 36> TempTimeVec(datapool::Temperature_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
 	for (int i = 0; i < 36; i++) {
-		memcpy(&TempTimeVec[i], &vecTemp[27 * i + 15], 4);
+		memcpy(&TempTimeVec[i], &vecTemp[i].Timestamp, 4);
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
 	
@@ -285,13 +289,13 @@ ReturnValue_t ArduinoDH::interpretDeviceReply(DeviceCommandId_t id,
 
 	PoolVector <float, 9> EnvValueVec(datapool::Environmental_value, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
 	for (int j = 0; j < 9; j++) {
-		memcpy(&EnvValueVec[j], &vecEnv[27 * (36 + j) + 11], 4);
+		memcpy(&EnvValueVec[j], &vecEnv[j].Value, 4);
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
 	
 	PoolVector <unsigned int, 9> EnvTimeVec(datapool::Environmental_Timestamp, &ArduinoDataSet, PoolVariableIF::VAR_WRITE);
 	for (int j = 0; j < 9; j++) {
-		memcpy(&EnvTimeVec[j], &vecEnv[27 * (36 + j) + 15], 4);
+		memcpy(&EnvTimeVec[j], &vecEnv[j].Timestamp, 4);
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
 	
@@ -299,19 +303,18 @@ 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[27 * (36 + 9) + 91 * k + 11], 36);
-		sif::debug<<"\nDEBUG_DHk1: End of copy to datapool"<<std::endl;
+		memcpy(&AccValueVec[k], &vecAcc[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[27 * (36 + 9) + 91 * k + 47], 36);
-		sif::debug<<"\nDEBUG_DHk2: End of copy to datapool"<<std::endl;
+		memcpy(&AccTimeVec[k], &vecAcc[k].Timestamp, 36);
 	}
 	ArduinoDataSet.commit(PoolVariableIF::VALID);
-	*/
-	
+	sif::debug<<"\nDEBUG_DHk2: End of copy to datapool"<<std::endl;*/
+
 	//sif::debug<<"DEBUG_DH: End of copy to datapool"<<std::endl;
 
 	return RETURN_OK;