From b1b3b806adb87901eb7621772a26521fd0ea16bc Mon Sep 17 00:00:00 2001
From: rmackay9 <rmackay9@yahoo.com>
Date: Thu, 6 Sep 2012 14:07:29 +0900
Subject: [PATCH] AP_PeriodicProcess: queue_process changes to suspend other
 processes before running queued process

Also restored suspend_timer to void return type
---
 .../AP_PeriodicProcessStub.cpp                |  2 ++
 .../AP_PeriodicProcessStub.h                  |  2 ++
 .../AP_PeriodicProcess/AP_TimerProcess.cpp    | 31 +++++++++++++++++++
 .../AP_PeriodicProcess/AP_TimerProcess.h      |  2 ++
 .../AP_PeriodicProcess/PeriodicProcess.h      |  1 +
 5 files changed, 38 insertions(+)

diff --git a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp
index f542c85a7a..d1537a91be 100644
--- a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp
+++ b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.cpp
@@ -8,6 +8,8 @@ void AP_PeriodicProcessStub::register_process(ap_procedure proc) {
 }
 void AP_PeriodicProcessStub::set_failsafe(ap_procedure proc) {
 }
+bool AP_PeriodicProcessStub::queue_process(ap_procedure proc) {
+}
 void AP_PeriodicProcessStub::suspend_timer(void) {
 }
 void AP_PeriodicProcessStub::resume_timer(void) {
diff --git a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h
index 7d3556c1ce..31e14a09c7 100644
--- a/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h
+++ b/libraries/AP_PeriodicProcess/AP_PeriodicProcessStub.h
@@ -13,6 +13,7 @@ public:
     void                    init( Arduino_Mega_ISR_Registry * isr_reg );
     void                    register_process(ap_procedure proc);
     void                    set_failsafe(ap_procedure proc);
+    bool                    queue_process(ap_procedure proc);   // queue process to run as soon as possible after any currently running ap_processes complete.  returns true if it ran immediately
     void                    suspend_timer(void);
     void                    resume_timer(void);
     static void             run(void);
@@ -20,6 +21,7 @@ protected:
     static uint8_t          _period;
     static void             (*_proc)(void);
     static void             (*_failsafe)(void);
+    static void             (*_queued_proc)(void);
     static bool             _suspended;
 };
 
diff --git a/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp b/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
index f60d815aac..a95690e9e3 100644
--- a/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
+++ b/libraries/AP_PeriodicProcess/AP_TimerProcess.cpp
@@ -16,6 +16,7 @@ extern "C" {
 uint8_t AP_TimerProcess::_period;
 ap_procedure AP_TimerProcess::_proc[AP_TIMERPROCESS_MAX_PROCS];
 ap_procedure AP_TimerProcess::_failsafe;
+ap_procedure AP_TimerProcess::_queued_proc = NULL;
 bool AP_TimerProcess::_in_timer_call;
 uint8_t AP_TimerProcess::_pidx = 0;
 bool AP_TimerProcess::_suspended;
@@ -66,6 +67,25 @@ void AP_TimerProcess::set_failsafe(ap_procedure proc)
     _failsafe = proc;
 }
 
+/*
+ *  queue a process to be run as soon as any currently running ap_processes complete
+ */
+bool AP_TimerProcess::queue_process(ap_procedure proc)
+{
+    // check if we are running any ap_processes
+    if( _in_timer_call || _suspended ) {
+         // queue the process to run after current processes finish
+        _queued_proc = proc;
+        return false;
+    }else{
+        // run process immediately
+        _suspended = true;
+        proc(micros());
+        _suspended = false;
+        return true;
+    }
+}
+
 void AP_TimerProcess::suspend_timer(void)
 {
     _suspended = true;
@@ -114,6 +134,17 @@ void AP_TimerProcess::run(void)
                 _proc[i](tnow);
             }
         }
+
+        // run any queued processes
+        cli();
+        ap_procedure qp = _queued_proc;
+        _queued_proc = NULL;
+        sei();
+        if( qp != NULL ) {
+            _suspended = true;
+            qp(tnow);
+            _suspended = false;
+        }
     }
 
     // and the failsafe, if one is setup
diff --git a/libraries/AP_PeriodicProcess/AP_TimerProcess.h b/libraries/AP_PeriodicProcess/AP_TimerProcess.h
index 1755e2abd5..3d30203d3d 100644
--- a/libraries/AP_PeriodicProcess/AP_TimerProcess.h
+++ b/libraries/AP_PeriodicProcess/AP_TimerProcess.h
@@ -16,6 +16,7 @@ public:
     AP_TimerProcess(uint8_t period = TIMERPROCESS_PER_DEFAULT);
     void                        init( Arduino_Mega_ISR_Registry * isr_reg );
     void                        register_process(ap_procedure proc);
+    bool                        queue_process(ap_procedure proc);   // queue process to run as soon as possible after any currently running ap_processes complete.  returns true if it ran immediately
     void                        set_failsafe(ap_procedure proc);
     void                        suspend_timer(void);
     void                        resume_timer(void);
@@ -24,6 +25,7 @@ protected:
     static uint8_t              _period;
     static ap_procedure         _proc[AP_TIMERPROCESS_MAX_PROCS];
     static ap_procedure         _failsafe;
+    static ap_procedure         _queued_proc;
     static uint8_t              _pidx;
     static bool                 _in_timer_call;
     static bool                 _suspended;
diff --git a/libraries/AP_PeriodicProcess/PeriodicProcess.h b/libraries/AP_PeriodicProcess/PeriodicProcess.h
index 4d08ab6845..2e2d7c95d3 100644
--- a/libraries/AP_PeriodicProcess/PeriodicProcess.h
+++ b/libraries/AP_PeriodicProcess/PeriodicProcess.h
@@ -14,6 +14,7 @@ class AP_PeriodicProcess
 public:
     virtual void        register_process(ap_procedure proc) = 0;
     virtual void        set_failsafe(ap_procedure proc) = 0;
+    virtual bool        queue_process(ap_procedure proc) = 0;   // queue process to run as soon as possible after any currently running ap_processes complete.  returns true if it ran immediately
     virtual void        suspend_timer(void) = 0;
     virtual void        resume_timer(void) = 0;
 };