From 1d3a49e46642a435c95832497dadef212d39c530 Mon Sep 17 00:00:00 2001
From: Matthias Badaire <mbadaire@gmail.com>
Date: Sat, 6 Sep 2014 18:24:41 +0200
Subject: [PATCH] AP_HAL_PX4 : make UARTDriver capable to be called from
 different threads.

The modification allows the read and write functions to be called by any thread but the calling thread must be the last one that called the begin() function.
---
 libraries/AP_HAL_PX4/UARTDriver.cpp | 19 +++++++++++--------
 libraries/AP_HAL_PX4/UARTDriver.h   |  3 +++
 2 files changed, 14 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp
index 769da4ee8..835de4d2f 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.cpp
+++ b/libraries/AP_HAL_PX4/UARTDriver.cpp
@@ -150,6 +150,8 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
         }
         _initialised = true;
     }
+    _uart_owner_pid = getpid();
+
 }
 
 void PX4UARTDriver::set_flow_control(enum flow_control flow_control)
@@ -275,6 +277,9 @@ int16_t PX4UARTDriver::txspace()
 int16_t PX4UARTDriver::read() 
 { 
 	uint8_t c;
+    if (_uart_owner_pid != getpid()){
+        return -1;
+    }
     if (!_initialised) {
         try_initialise();
         return -1;
@@ -295,12 +300,11 @@ int16_t PX4UARTDriver::read()
  */
 size_t PX4UARTDriver::write(uint8_t c) 
 { 
-    if (!_initialised) {
-        try_initialise();
+    if (_uart_owner_pid != getpid()){
         return 0;
     }
-    if (hal.scheduler->in_timerprocess()) {
-        // not allowed from timers
+    if (!_initialised) {
+        try_initialise();
         return 0;
     }
     uint16_t _head;
@@ -321,14 +325,13 @@ size_t PX4UARTDriver::write(uint8_t c)
  */
 size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size)
 {
+    if (_uart_owner_pid != getpid()){
+        return 0;
+    }
 	if (!_initialised) {
         try_initialise();
 		return 0;
 	}
-    if (hal.scheduler->in_timerprocess()) {
-        // not allowed from timers
-        return 0;
-    }
 
     if (!_nonblocking_writes) {
         /*
diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h
index ff35e1c6c..23d25b543 100644
--- a/libraries/AP_HAL_PX4/UARTDriver.h
+++ b/libraries/AP_HAL_PX4/UARTDriver.h
@@ -75,6 +75,9 @@ private:
     uint32_t _total_read;
     uint32_t _total_written;
     enum flow_control _flow_control;
+
+    pid_t _uart_owner_pid;
+
 };
 
 #endif // __AP_HAL_PX4_UARTDRIVER_H__
-- 
GitLab