From aaef72955ec6d976d5afd86bce21ee773c24fc77 Mon Sep 17 00:00:00 2001
From: vassilis <vassilis>
Date: Sun, 25 Apr 2010 18:34:03 +0000
Subject: [PATCH] OP-4 Flight/Telemetry Make objectTransaction() fully
 reentrant

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@547 ebee16cc-31ac-478f-84a7-5cbb03baadba
---
 flight/OpenPilot/System/openpilot.c |  4 ++--
 flight/OpenPilot/UAVTalk/uavtalk.c  | 26 ++++++++++++++++++--------
 2 files changed, 20 insertions(+), 10 deletions(-)

diff --git a/flight/OpenPilot/System/openpilot.c b/flight/OpenPilot/System/openpilot.c
index b1ce57c22..87b266ff4 100644
--- a/flight/OpenPilot/System/openpilot.c
+++ b/flight/OpenPilot/System/openpilot.c
@@ -149,9 +149,9 @@ void OpenPilotInit()
 
 	/* Initialize modules */
 	TelemetryInitialize();
-	//ExampleModPeriodicInitialize();
+	ExampleModPeriodicInitialize();
 	//ExampleModThreadInitialize();
-	ExampleModEventInitialize();
+	//ExampleModEventInitialize();
 	GpsInitialize();
 }
 
diff --git a/flight/OpenPilot/UAVTalk/uavtalk.c b/flight/OpenPilot/UAVTalk/uavtalk.c
index 4e4ee5981..0ace34da1 100644
--- a/flight/OpenPilot/UAVTalk/uavtalk.c
+++ b/flight/OpenPilot/UAVTalk/uavtalk.c
@@ -46,6 +46,7 @@ typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxSta
 // Private variables
 static UAVTalkOutputStream outStream;
 static xSemaphoreHandle lock;
+static xSemaphoreHandle transLock;
 static xSemaphoreHandle respSema;
 static UAVObjHandle respObj;
 static uint16_t respInstId;
@@ -71,6 +72,7 @@ int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream)
 {
     outStream = outputStream;
     lock = xSemaphoreCreateRecursiveMutex();
+    transLock = xSemaphoreCreateRecursiveMutex();
     vSemaphoreCreateBinary(respSema);
     xSemaphoreTake(respSema, 0); // reset to zero
     UAVTalkResetStats();
@@ -159,37 +161,45 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type
 {
 	int32_t respReceived;
 
-	// Lock
-    xSemaphoreTakeRecursive(lock, portMAX_DELAY);
-
     // Send object depending on if a response is needed
     if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
     {
-    	xSemaphoreTake(respSema, 0); // non blocking call to make sure the value is zero (binary sema)
-        sendObject(obj, instId, type);
+    	// Get transaction lock (will block if a transaction is pending)
+    	xSemaphoreTakeRecursive(transLock, portMAX_DELAY);
+    	// Send object
+    	xSemaphoreTakeRecursive(lock, portMAX_DELAY);
         respObj = obj;
         respInstId = instId;
-        xSemaphoreGiveRecursive(lock); // need to release lock since the next call will block until a response is received
-    	respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout)
+        sendObject(obj, instId, type);
+        xSemaphoreGiveRecursive(lock);
+        // Wait for response (or timeout)
+    	respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS);
     	// Check if a response was received
     	if (respReceived == pdFALSE)
     	{
+    		// Cancel transaction
+    		xSemaphoreTakeRecursive(lock, portMAX_DELAY);
+    		xSemaphoreTake(respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)
+    		respObj = 0;
+    		xSemaphoreGiveRecursive(lock);
+    		xSemaphoreGiveRecursive(transLock);
     		return -1;
     	}
     	else
     	{
+    		xSemaphoreGiveRecursive(transLock);
     		return 0;
     	}
     }
     else if (type == TYPE_OBJ)
     {
+    	xSemaphoreTakeRecursive(lock, portMAX_DELAY);
         sendObject(obj, instId, TYPE_OBJ);
         xSemaphoreGiveRecursive(lock);
         return 0;
     }
     else
     {
-    	xSemaphoreGiveRecursive(lock);
         return -1;
     }
 }