From f7ac5aa079287cda62e1eeb985c3227b7d79e65f Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Fri, 19 Jan 2018 00:42:38 +0530
Subject: [PATCH] HAL_ChibiOS: put rcin on a separate thread

---
 libraries/AP_HAL_ChibiOS/Scheduler.cpp | 20 +++++++++++++++++++-
 libraries/AP_HAL_ChibiOS/Scheduler.h   |  3 +++
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 7c7f8e4a2d2..f40e4548cdb 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -33,6 +33,7 @@ using namespace ChibiOS;
 
 extern const AP_HAL::HAL& hal;
 THD_WORKING_AREA(_timer_thread_wa, 2048);
+THD_WORKING_AREA(_rcin_thread_wa, 512);
 THD_WORKING_AREA(_io_thread_wa, 2048);
 THD_WORKING_AREA(_storage_thread_wa, 2048);
 THD_WORKING_AREA(_uart_thread_wa, 2048);
@@ -53,6 +54,13 @@ void Scheduler::init()
                      _timer_thread,             /* Thread function.     */
                      this);                     /* Thread parameter.    */
 
+    // setup the RCIN thread - this will call tasks at 1kHz
+    _rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
+                     sizeof(_rcin_thread_wa),
+                     APM_RCIN_PRIORITY,        /* Initial priority.    */
+                     _rcin_thread,             /* Thread function.     */
+                     this);                     /* Thread parameter.    */
+
     // the UART thread runs at a medium priority
     _uart_thread_ctx = chThdCreateStatic(_uart_thread_wa,
                      sizeof(_uart_thread_wa),
@@ -254,8 +262,18 @@ void Scheduler::_timer_thread(void *arg)
 
         // process any pending RC output requests
         hal.rcout->timer_tick();
+    }
+}
 
-        // process any pending RC input requests
+void Scheduler::_rcin_thread(void *arg)
+{
+    Scheduler *sched = (Scheduler *)arg;
+    sched->_rcin_thread_ctx->name = "apm_rcin";
+    while (!sched->_hal_initialized) {
+        sched->delay_microseconds(20000);
+    }
+    while (true) {
+        sched->delay_microseconds(2500);
         ((RCInput *)hal.rcin)->_timer_tick();
     }
 }
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h
index d1168a281fa..c3c3f528768 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.h
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.h
@@ -25,6 +25,7 @@
 #define APM_MAIN_PRIORITY_BOOST 180 // same as normal for now
 #define APM_MAIN_PRIORITY       180
 #define APM_TIMER_PRIORITY      178
+#define APM_RCIN_PRIORITY       177
 #define APM_UART_PRIORITY        60
 #define APM_STORAGE_PRIORITY     59
 #define APM_IO_PRIORITY          58
@@ -103,11 +104,13 @@ private:
     volatile bool _timer_event_missed;
 
     thread_t* _timer_thread_ctx;
+    thread_t* _rcin_thread_ctx;
     thread_t* _io_thread_ctx;
     thread_t* _storage_thread_ctx;
     thread_t* _uart_thread_ctx;
 
     static void _timer_thread(void *arg);
+    static void _rcin_thread(void *arg);
     static void _io_thread(void *arg);
     static void _storage_thread(void *arg);
     static void _uart_thread(void *arg);