From 89676be825906aeae6d525c1a0a36ab76f7940b5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Tue, 31 Dec 2013 22:24:55 +1100
Subject: [PATCH] HAL_PX4: fixed RCInput race condition

the number of channels could be zero with thread switch at the wrong
time
---
 libraries/AP_HAL_PX4/RCInput.cpp | 23 +++++++++++++++++------
 libraries/AP_HAL_PX4/RCInput.h   |  4 +++-
 2 files changed, 20 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_HAL_PX4/RCInput.cpp b/libraries/AP_HAL_PX4/RCInput.cpp
index 4e60c4c938..81895c851f 100644
--- a/libraries/AP_HAL_PX4/RCInput.cpp
+++ b/libraries/AP_HAL_PX4/RCInput.cpp
@@ -17,11 +17,15 @@ void PX4RCInput::init(void* unused)
 		hal.scheduler->panic("Unable to subscribe to input_rc");		
 	}
 	clear_overrides();
+        pthread_mutex_init(&rcin_mutex, NULL);
 }
 
 uint8_t PX4RCInput::valid_channels() 
 {
-	return _rcin.timestamp != _last_read || _override_valid;
+    pthread_mutex_lock(&rcin_mutex);
+    bool valid = _rcin.timestamp != _last_read || _override_valid;
+    pthread_mutex_unlock(&rcin_mutex);
+    return valid;
 }
 
 uint16_t PX4RCInput::read(uint8_t ch) 
@@ -29,15 +33,21 @@ uint16_t PX4RCInput::read(uint8_t ch)
 	if (ch >= RC_INPUT_MAX_CHANNELS) {
 		return 0;
 	}
+        pthread_mutex_lock(&rcin_mutex);
 	_last_read = _rcin.timestamp;
 	_override_valid = false;
 	if (_override[ch]) {
-		return _override[ch];
+            uint16_t v = _override[ch];
+            pthread_mutex_unlock(&rcin_mutex);
+            return v;
 	}
 	if (ch >= _rcin.channel_count) {
-		return 0;
+            pthread_mutex_unlock(&rcin_mutex);
+            return 0;
 	}
-	return _rcin.values[ch];
+	uint16_t v = _rcin.values[ch];
+        pthread_mutex_unlock(&rcin_mutex);
+        return v;
 }
 
 uint8_t PX4RCInput::read(uint16_t* periods, uint8_t len) 
@@ -87,8 +97,9 @@ void PX4RCInput::_timer_tick(void)
 	perf_begin(_perf_rcin);
 	bool rc_updated = false;
 	if (orb_check(_rc_sub, &rc_updated) == 0 && rc_updated) {
-		orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
-		_last_input = _rcin.timestamp;
+            pthread_mutex_lock(&rcin_mutex);
+            orb_copy(ORB_ID(input_rc), _rc_sub, &_rcin);
+            pthread_mutex_unlock(&rcin_mutex);
 	}
         // note, we rely on the vehicle code checking valid_channels() 
         // and a timeout for the last valid input to handle failsafe
diff --git a/libraries/AP_HAL_PX4/RCInput.h b/libraries/AP_HAL_PX4/RCInput.h
index c1384f216c..f29eec2f85 100644
--- a/libraries/AP_HAL_PX4/RCInput.h
+++ b/libraries/AP_HAL_PX4/RCInput.h
@@ -5,6 +5,7 @@
 #include <AP_HAL_PX4.h>
 #include <drivers/drv_rc_input.h>
 #include <systemlib/perf_counter.h>
+#include <pthread.h>
 
 class PX4::PX4RCInput : public AP_HAL::RCInput {
 public:
@@ -25,9 +26,10 @@ private:
     struct rc_input_values _rcin;
     int _rc_sub;
     uint64_t _last_read;
-    uint64_t _last_input;
     bool _override_valid;
     perf_counter_t _perf_rcin;
+
+    pthread_mutex_t rcin_mutex;
 };
 
 #endif // __AP_HAL_PX4_RCINPUT_H__