From 02b4ecc273ac62ab3c1109a7666a502d849de02b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 27 Jan 2013 12:51:34 +1100
Subject: [PATCH] HAL_PX4: get RC input from PX4IO board

this allows us to support DSM and SBUS receivers
---
 libraries/AP_HAL_PX4/RCInput.cpp   | 41 ++++++++++++++++++++----------
 libraries/AP_HAL_PX4/RCInput.h     | 11 +++++---
 libraries/AP_HAL_PX4/Scheduler.cpp |  4 +++
 3 files changed, 40 insertions(+), 16 deletions(-)

diff --git a/libraries/AP_HAL_PX4/RCInput.cpp b/libraries/AP_HAL_PX4/RCInput.cpp
index b6728d5b33..4e1f631b35 100644
--- a/libraries/AP_HAL_PX4/RCInput.cpp
+++ b/libraries/AP_HAL_PX4/RCInput.cpp
@@ -3,42 +3,47 @@
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
 #include "RCInput.h"
 #include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
 
 using namespace PX4;
 
 extern const AP_HAL::HAL& hal;
 
-extern uint16_t ppm_buffer[];
-extern unsigned ppm_decoded_channels;
-extern uint64_t ppm_last_valid_decode;
-
 void PX4RCInput::init(void* unused)
 {
+	_perf_rcin = perf_alloc(PC_ELAPSED, "APM_rcin");
+	_rc_sub = orb_subscribe(ORB_ID(input_rc));
+	if (_rc_sub == -1) {
+		hal.scheduler->panic("Unable to subscribe to input_rc");		
+	}
 	clear_overrides();
 }
 
 uint8_t PX4RCInput::valid() 
 {
-	return ppm_last_valid_decode != _last_read || _override_valid;
+	return _rcin.timestamp != _last_read || _override_valid;
 }
 
 uint16_t PX4RCInput::read(uint8_t ch) 
 {
-	_last_read = ppm_last_valid_decode;
-	_override_valid = false;
-	if (ch >= PX4_NUM_RCINPUT_CHANNELS) {
+	if (ch >= RC_INPUT_MAX_CHANNELS) {
 		return 0;
 	}
+	_last_read = _rcin.timestamp;
+	_override_valid = false;
 	if (_override[ch]) {
 		return _override[ch];
 	}
-	return ppm_buffer[ch];
+	if (ch >= _rcin.channel_count) {
+		return 0;
+	}
+	return _rcin.values[ch];
 }
 
 uint8_t PX4RCInput::read(uint16_t* periods, uint8_t len) 
 {
-	if (len > PX4_NUM_RCINPUT_CHANNELS) {
-		len = PX4_NUM_RCINPUT_CHANNELS;
+	if (len > RC_INPUT_MAX_CHANNELS) {
+		len = RC_INPUT_MAX_CHANNELS;
 	}
 	for (uint8_t i = 0; i < len; i++){
 		periods[i] = read(i);
@@ -59,7 +64,7 @@ bool PX4RCInput::set_override(uint8_t channel, int16_t override) {
 	if (override < 0) {
 		return false; /* -1: no change. */
 	}
-	if (channel >= PX4_NUM_RCINPUT_CHANNELS) {
+	if (channel >= RC_INPUT_MAX_CHANNELS) {
 		return false;
 	}
 	_override[channel] = override;
@@ -72,9 +77,19 @@ bool PX4RCInput::set_override(uint8_t channel, int16_t override) {
 
 void PX4RCInput::clear_overrides()
 {
-	for (uint8_t i = 0; i < PX4_NUM_RCINPUT_CHANNELS; i++) {
+	for (uint8_t i = 0; i < RC_INPUT_MAX_CHANNELS; i++) {
 		set_override(i, 0);
 	}
 }
 
+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);
+	}
+	perf_end(_perf_rcin);
+}
+
 #endif
diff --git a/libraries/AP_HAL_PX4/RCInput.h b/libraries/AP_HAL_PX4/RCInput.h
index 028d1e8c2d..3bf4718501 100644
--- a/libraries/AP_HAL_PX4/RCInput.h
+++ b/libraries/AP_HAL_PX4/RCInput.h
@@ -3,8 +3,8 @@
 #define __AP_HAL_PX4_RCINPUT_H__
 
 #include <AP_HAL_PX4.h>
-
-#define PX4_NUM_RCINPUT_CHANNELS 8
+#include <drivers/drv_rc_input.h>
+#include <systemlib/perf_counter.h>
 
 class PX4::PX4RCInput : public AP_HAL::RCInput {
 public:
@@ -17,11 +17,16 @@ public:
     bool set_override(uint8_t channel, int16_t override);
     void clear_overrides();
 
+    void _timer_tick(void);
+
 private:
     /* override state */
-    uint16_t _override[PX4_NUM_RCINPUT_CHANNELS];
+    uint16_t _override[RC_INPUT_MAX_CHANNELS];
+    struct rc_input_values _rcin;
+    int _rc_sub;
     uint64_t _last_read;
     bool _override_valid;
+    perf_counter_t _perf_rcin;
 };
 
 #endif // __AP_HAL_PX4_RCINPUT_H__
diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index 1e2ac97989..eaca06ea66 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -19,6 +19,7 @@
 #include "UARTDriver.h"
 #include "Storage.h"
 #include "RCOutput.h"
+#include "RCInput.h"
 
 using namespace PX4;
 
@@ -194,6 +195,9 @@ void *PX4Scheduler::_timer_thread(void)
 
         // process any pending RC output requests
         ((PX4RCOutput *)hal.rcout)->_timer_tick();
+
+        // process any pending RC input requests
+        ((PX4RCInput *)hal.rcin)->_timer_tick();
     }
     return NULL;
 }