AP_HAL_SMACCM: Scheduler timer process implemented correctly.

- Use "g_atomic" for suspend/resume timer procs.

libraries/AP_HAL_SMACCM/Scheduler.cpp

@@ -64,17 +64,12 @@ static void scheduler_task(void *arg)
      * time. */
     now = xTaskGetTickCount();
     if (last_wake_time + SCHEDULER_TICKS <= now) {
-      hal.console->printf("now %lu  last %lu\r\n", last_wake_time, now);
       sched->run_failsafe_cb();
       last_wake_time = now;
     } else {
       vTaskDelayUntil(&last_wake_time, SCHEDULER_TICKS);
 
-      if (!xSemaphoreTakeRecursive(g_atomic, 5)) {
-        asm volatile ("bkpt");
-        hal.scheduler->panic("g_atomic took too long");
-      }
-
+      xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
       sched->run_callbacks();
       xSemaphoreGiveRecursive(g_atomic);
     }
@@ -135,8 +130,7 @@ static void delay_cb_task(void *arg)
 }
 
 SMACCMScheduler::SMACCMScheduler()
-  : m_delay_cb(NULL), m_suspended(false),
-    m_task(NULL), m_delay_cb_task(NULL),
+  : m_delay_cb(NULL), m_task(NULL), m_delay_cb_task(NULL),
     m_failsafe_cb(NULL), m_num_procs(0)
 {
 }
@@ -218,28 +212,29 @@ void SMACCMScheduler::register_timer_failsafe(AP_HAL::TimedProc k, uint32_t)
 
 void SMACCMScheduler::suspend_timer_procs()
 {
-  m_suspended = true;
+  xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
 }
 
 void SMACCMScheduler::resume_timer_procs()
 {
-  m_suspended = false;
+  xSemaphoreGiveRecursive(g_atomic);
 }
 
 void SMACCMScheduler::begin_atomic()
 {
-  xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
 }
 
 void SMACCMScheduler::end_atomic()
 {
-  xSemaphoreGiveRecursive(g_atomic);
 }
 
 void SMACCMScheduler::panic(const prog_char_t *errormsg)
 {
   hal.console->println_P(errormsg);
-  m_suspended = true;
+
+  // Try to grab "g_atomic" to suspend timer processes, but with a
+  // timeout in case a timer proc is locked up.
+  xSemaphoreTakeRecursive(g_atomic, 10);
 
   for(;;)
     ;
@@ -256,7 +251,6 @@ void SMACCMScheduler::run_callbacks()
   uint32_t now = micros();
 
   // Run timer processes if not suspended.
-  if (!m_suspended) {
   portENTER_CRITICAL();
   uint8_t num_procs = m_num_procs;
   portEXIT_CRITICAL();
@@ -265,7 +259,6 @@ void SMACCMScheduler::run_callbacks()
     if (m_procs[i] != NULL)
       m_procs[i](now);
   }
-  }
 }
 
 void SMACCMScheduler::run_failsafe_cb()

libraries/AP_HAL_SMACCM/Scheduler.h

@@ -110,7 +110,6 @@ public:
 
 private:
   AP_HAL::Proc m_delay_cb;      /* delay callback */
-  bool m_suspended;             /* true if timers suspended */
   void *m_task;                 /* opaque scheduler task handle */
   void *m_delay_cb_task;        /* opaque delay cb task handle */
   AP_HAL::TimedProc m_procs[SMACCM_SCHEDULER_MAX_TIMER_PROCS];