AP_PeriodicProcess: queue_process changes to suspend other processes before running queued process

Also restored suspend_timer to void return type

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) {

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;
 };
 

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

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;

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;
 };