From f2e77ab7731e79590c2b0434b1e7015bcfeb33b0 Mon Sep 17 00:00:00 2001
From: Siddharth Purohit <siddharthbharatpurohit@gmail.com>
Date: Fri, 2 Feb 2018 00:51:18 +0530
Subject: [PATCH] AP_HAL_ChibiOS: add uavcan thread

---
 libraries/AP_HAL_ChibiOS/Scheduler.cpp | 31 +++++++++++++++++++++++++-
 libraries/AP_HAL_ChibiOS/Scheduler.h   | 11 ++++++++-
 2 files changed, 40 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 85c6cf6762..e9a3cc3e91 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -25,6 +25,7 @@
 #include <AP_HAL_ChibiOS/Storage.h>
 #include <AP_HAL_ChibiOS/RCOutput.h>
 #include <AP_HAL_ChibiOS/RCInput.h>
+#include <AP_HAL_ChibiOS/CAN.h>
 
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
@@ -37,7 +38,9 @@ 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);
-
+#ifdef HAL_WITH_UAVCAN
+THD_WORKING_AREA(_uavcan_thread_wa, 2048);
+#endif
 #if HAL_WITH_IO_MCU
 extern ChibiOS::UARTDriver uart_io;
 #endif
@@ -54,6 +57,14 @@ void Scheduler::init()
                      _timer_thread,             /* Thread function.     */
                      this);                     /* Thread parameter.    */
 
+    // setup the timer thread - this will call tasks at 1kHz
+#ifdef HAL_WITH_UAVCAN
+    _uavcan_thread_ctx = chThdCreateStatic(_uavcan_thread_wa,
+                     sizeof(_uavcan_thread_wa),
+                     APM_UAVCAN_PRIORITY,        /* Initial priority.    */
+                     _uavcan_thread,            /* Thread function.     */
+                     this);                     /* Thread parameter.    */
+#endif
     // setup the RCIN thread - this will call tasks at 1kHz
     _rcin_thread_ctx = chThdCreateStatic(_rcin_thread_wa,
                      sizeof(_rcin_thread_wa),
@@ -264,6 +275,24 @@ void Scheduler::_timer_thread(void *arg)
         hal.rcout->timer_tick();
     }
 }
+#if HAL_WITH_UAVCAN
+void Scheduler::_uavcan_thread(void *arg)
+{
+    Scheduler *sched = (Scheduler *)arg;
+    sched->_rcin_thread_ctx->name = "apm_uavcan";
+    while (!sched->_hal_initialized) {
+        sched->delay_microseconds(20000);
+    }
+    while (true) {
+        sched->delay_microseconds(1000);
+        for (int i = 0; i < MAX_NUMBER_OF_CAN_INTERFACES; i++) {
+            if(hal.can_mgr[i] != nullptr) {
+                CANManager::from(hal.can_mgr[i])->_timer_tick();
+            }
+        }
+    }
+}
+#endif
 
 void Scheduler::_rcin_thread(void *arg)
 {
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.h b/libraries/AP_HAL_ChibiOS/Scheduler.h
index 426df0abf4..c2b4acb6e3 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.h
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.h
@@ -36,6 +36,10 @@
 #define APM_SPI_PRIORITY        179
 #endif
 
+#ifndef APM_UAVCAN_PRIORITY
+#define APM_UAVCAN_PRIORITY     178
+#endif
+
 #ifndef APM_CAN_PRIORITY
 #define APM_CAN_PRIORITY        177
 #endif
@@ -108,12 +112,17 @@ private:
     thread_t* _io_thread_ctx;
     thread_t* _storage_thread_ctx;
     thread_t* _uart_thread_ctx;
-
+#if HAL_WITH_UAVCAN
+    thread_t* _uavcan_thread_ctx;
+#endif
     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);
+#if HAL_WITH_UAVCAN
+    static void _uavcan_thread(void *arg);
+#endif
     void _run_timers(bool called_from_timer_thread);
     void _run_io(void);
 };