From 7ec242146d21e04cd97296a432569a32e01225f0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 28 Sep 2013 16:29:04 +1000
Subject: [PATCH] HAL_PX4: update for new scheduler API

---
 libraries/AP_HAL_PX4/Scheduler.cpp | 14 +++++++-------
 libraries/AP_HAL_PX4/Scheduler.h   |  6 ++++--
 2 files changed, 11 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp
index 7a96a084bf..ed1ecd8667 100644
--- a/libraries/AP_HAL_PX4/Scheduler.cpp
+++ b/libraries/AP_HAL_PX4/Scheduler.cpp
@@ -128,7 +128,7 @@ void PX4Scheduler::register_delay_callback(AP_HAL::Proc proc,
     _min_delay_cb_ms = min_time_ms;
 }
 
-void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc) 
+void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg) 
 {
     for (uint8_t i = 0; i < _num_timer_procs; i++) {
         if (_timer_proc[i] == proc) {
@@ -138,13 +138,14 @@ void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc)
 
     if (_num_timer_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
         _timer_proc[_num_timer_procs] = proc;
+        _timer_arg[_num_timer_procs] = arg;
         _num_timer_procs++;
     } else {
         hal.console->printf("Out of timer processes\n");
     }
 }
 
-void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc) 
+void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc, void *arg) 
 {
     for (uint8_t i = 0; i < _num_io_procs; i++) {
         if (_io_proc[i] == proc) {
@@ -154,6 +155,7 @@ void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc)
 
     if (_num_io_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
         _io_proc[_num_io_procs] = proc;
+        _io_arg[_num_io_procs] = arg;
         _num_io_procs++;
     } else {
         hal.console->printf("Out of IO processes\n");
@@ -186,7 +188,6 @@ void PX4Scheduler::reboot(bool hold_in_bootloader)
 
 void PX4Scheduler::_run_timers(bool called_from_timer_thread)
 {
-    uint32_t tnow = micros();
     if (_in_timer_proc) {
         return;
     }
@@ -196,7 +197,7 @@ void PX4Scheduler::_run_timers(bool called_from_timer_thread)
         // now call the timer based drivers
         for (int i = 0; i < _num_timer_procs; i++) {
             if (_timer_proc[i] != NULL) {
-                _timer_proc[i](tnow);
+                _timer_proc[i](_timer_arg[i]);
             }
         }
     } else if (called_from_timer_thread) {
@@ -205,7 +206,7 @@ void PX4Scheduler::_run_timers(bool called_from_timer_thread)
 
     // and the failsafe, if one is setup
     if (_failsafe != NULL) {
-        _failsafe(tnow);
+        _failsafe(NULL);
     }
 
     // process analog input
@@ -235,7 +236,6 @@ void *PX4Scheduler::_timer_thread(void)
 
 void PX4Scheduler::_run_io(void)
 {
-    uint32_t tnow = micros();
     if (_in_io_proc) {
         return;
     }
@@ -245,7 +245,7 @@ void PX4Scheduler::_run_io(void)
         // now call the IO based drivers
         for (int i = 0; i < _num_io_procs; i++) {
             if (_io_proc[i] != NULL) {
-                _io_proc[i](tnow);
+                _io_proc[i](_io_arg[i]);
             }
         }
     }
diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h
index fd544ebc6f..ccdf793f13 100644
--- a/libraries/AP_HAL_PX4/Scheduler.h
+++ b/libraries/AP_HAL_PX4/Scheduler.h
@@ -31,8 +31,8 @@ public:
     uint32_t micros();
     void     delay_microseconds(uint16_t us);
     void     register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
-    void     register_timer_process(AP_HAL::TimedProc);
-    void     register_io_process(AP_HAL::TimedProc);
+    void     register_timer_process(AP_HAL::TimedProc, void *);
+    void     register_io_process(AP_HAL::TimedProc, void *);
     void     register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
     void     suspend_timer_procs();
     void     resume_timer_procs();
@@ -54,10 +54,12 @@ private:
     volatile bool _timer_suspended;
 
     AP_HAL::TimedProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
+    void * _timer_arg[PX4_SCHEDULER_MAX_TIMER_PROCS];
     uint8_t _num_timer_procs;
     volatile bool _in_timer_proc;
 
     AP_HAL::TimedProc _io_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
+    void * _io_arg[PX4_SCHEDULER_MAX_TIMER_PROCS];
     uint8_t _num_io_procs;
     volatile bool _in_io_proc;