Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_SMACCM: Scheduler timer process implemented correctly. 

- Use "g_atomic" for suspend/resume timer procs.
This commit is contained in:
James Bielman 2013-01-03 15:52:07 -08:00 committed by Pat Hickey
parent d84ba8ef59
commit 8f4a2e4c0e
2 changed files with 14 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_HAL_SMACCM/Scheduler.cpp
View File

@ -64,17 +64,12 @@ static void scheduler_task(void *arg)
* time. */ * time. */
now = xTaskGetTickCount(); now = xTaskGetTickCount();
if (last_wake_time + SCHEDULER_TICKS <= now) { if (last_wake_time + SCHEDULER_TICKS <= now) {
hal.console->printf("now %lu last %lu\r\n", last_wake_time, now);
sched->run_failsafe_cb(); sched->run_failsafe_cb();
last_wake_time = now; last_wake_time = now;
} else { } else {
vTaskDelayUntil(&last_wake_time, SCHEDULER_TICKS); vTaskDelayUntil(&last_wake_time, SCHEDULER_TICKS);
if (!xSemaphoreTakeRecursive(g_atomic, 5)) { xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
asm volatile ("bkpt");
hal.scheduler->panic("g_atomic took too long");
}
sched->run_callbacks(); sched->run_callbacks();
xSemaphoreGiveRecursive(g_atomic); xSemaphoreGiveRecursive(g_atomic);
} }
@ -135,8 +130,7 @@ static void delay_cb_task(void *arg)
} }
SMACCMScheduler::SMACCMScheduler() SMACCMScheduler::SMACCMScheduler()
: m_delay_cb(NULL), m_suspended(false), : m_delay_cb(NULL), m_task(NULL), m_delay_cb_task(NULL),
m_task(NULL), m_delay_cb_task(NULL),
m_failsafe_cb(NULL), m_num_procs(0) 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() void SMACCMScheduler::suspend_timer_procs()
{ {
m_suspended = true; xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
} }
void SMACCMScheduler::resume_timer_procs() void SMACCMScheduler::resume_timer_procs()
{ {
m_suspended = false; xSemaphoreGiveRecursive(g_atomic);
} }
void SMACCMScheduler::begin_atomic() void SMACCMScheduler::begin_atomic()
{ {
xSemaphoreTakeRecursive(g_atomic, portMAX_DELAY);
} }
void SMACCMScheduler::end_atomic() void SMACCMScheduler::end_atomic()
{ {
xSemaphoreGiveRecursive(g_atomic);
} }
void SMACCMScheduler::panic(const prog_char_t *errormsg) void SMACCMScheduler::panic(const prog_char_t *errormsg)
{ {
hal.console->println_P(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(;;) for(;;)
; ;
@ -256,7 +251,6 @@ void SMACCMScheduler::run_callbacks()
uint32_t now = micros(); uint32_t now = micros();
// Run timer processes if not suspended. // Run timer processes if not suspended.
if (!m_suspended) {
portENTER_CRITICAL(); portENTER_CRITICAL();
uint8_t num_procs = m_num_procs; uint8_t num_procs = m_num_procs;
portEXIT_CRITICAL(); portEXIT_CRITICAL();
@ -265,7 +259,6 @@ void SMACCMScheduler::run_callbacks()
if (m_procs[i] != NULL) if (m_procs[i] != NULL)
m_procs[i](now); m_procs[i](now);
} }
}
} }
void SMACCMScheduler::run_failsafe_cb() void SMACCMScheduler::run_failsafe_cb()

1
libraries/AP_HAL_SMACCM/Scheduler.h
View File

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