From 39e28d48c2db76b8eb04e1f80d111bded6fabc1b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 21 Jan 2013 13:54:09 +1100 Subject: [PATCH] HAL_PX4: switched scheduler to use a pthread this allows the timer tasks to access file descriptors in the main APM task, which makes writing PX4 device drivers much easier --- libraries/AP_HAL_PX4/Scheduler.cpp | 105 +++++++++++++---------------- libraries/AP_HAL_PX4/Scheduler.h | 31 ++++----- 2 files changed, 58 insertions(+), 78 deletions(-) diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp index 6fa58e6313..4d78d3e535 100644 --- a/libraries/AP_HAL_PX4/Scheduler.cpp +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -20,15 +20,7 @@ using namespace PX4; extern const AP_HAL::HAL& hal; -uint64_t PX4Scheduler::_sketch_start_time; - -AP_HAL::TimedProc PX4Scheduler::_failsafe = NULL; -volatile bool PX4Scheduler::_timer_suspended = false; -AP_HAL::TimedProc PX4Scheduler::_timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; -uint8_t PX4Scheduler::_num_timer_procs = 0; -bool PX4Scheduler::_in_timer_proc = false; -uint8_t PX4Scheduler::_nested_atomic_ctr; -bool PX4Scheduler::_timer_pending; +extern bool _px4_thread_should_exit; PX4Scheduler::PX4Scheduler() {} @@ -37,19 +29,23 @@ void PX4Scheduler::init(void *unused) { _sketch_start_time = hrt_absolute_time(); - // setup a 1kHz timer - memset(&_call, 0, sizeof(_call)); - hrt_call_every(&_call, 1000, 1000, _timer_event, NULL); -} + // setup the timer thread - this will call tasks at 1kHz + pthread_attr_t thread_attr; + pthread_attr_init(&thread_attr); + pthread_attr_setstacksize(&thread_attr, 2048); -uint32_t PX4Scheduler::_micros() -{ - return (uint32_t)(hrt_absolute_time() - _sketch_start_time); + // the timer thread needs a higher priority than the main code, so + // it runs as soon as its poll() delay returns + struct sched_param param; + param.sched_priority = SCHED_PRIORITY_DEFAULT + 1; + (void)pthread_attr_setschedparam(&thread_attr, ¶m); + + pthread_create(&_thread, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_timer_thread, this); } uint32_t PX4Scheduler::micros() { - return _micros(); + return (uint32_t)(hrt_absolute_time() - _sketch_start_time); } uint32_t PX4Scheduler::millis() @@ -69,7 +65,8 @@ void PX4Scheduler::delay(uint16_t ms) { uint64_t start = hrt_absolute_time(); - while ((hrt_absolute_time() - start)/1000 < ms) { + while ((hrt_absolute_time() - start)/1000 < ms && + !_px4_thread_should_exit) { // this yields the CPU to other apps poll(NULL, 0, 1); if (_min_delay_cb_ms <= ms) { @@ -78,6 +75,9 @@ void PX4Scheduler::delay(uint16_t ms) } } } + if (_px4_thread_should_exit) { + exit(1); + } } void PX4Scheduler::register_delay_callback(AP_HAL::Proc proc, @@ -98,38 +98,27 @@ void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc) if (_num_timer_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) { _timer_proc[_num_timer_procs] = proc; _num_timer_procs++; + } else { + hal.console->printf("Out of timer processes\n"); } - } void PX4Scheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us) { - hal.console->printf("Not registering failsafe handler\n"); -// _failsafe = failsafe; + _failsafe = failsafe; } -void PX4Scheduler::suspend_timer_procs() { +void PX4Scheduler::suspend_timer_procs() +{ _timer_suspended = true; } -void PX4Scheduler::resume_timer_procs() { +void PX4Scheduler::resume_timer_procs() +{ _timer_suspended = false; -} - -void PX4Scheduler::begin_atomic() { - _nested_atomic_ctr++; -} - -void PX4Scheduler::end_atomic() { - if (_nested_atomic_ctr == 0) { - hal.uartA->println_P(PSTR("ATOMIC NESTING ERROR")); - return; - } - _nested_atomic_ctr--; - if (_nested_atomic_ctr == 0 && _timer_pending) { - // a timer went off during an atomic operation - run it now - _timer_pending = false; - _timer_event(NULL); + if (_timer_event_missed == true) { + _run_timers(false); + _timer_event_missed = false; } } @@ -138,28 +127,10 @@ void PX4Scheduler::reboot() up_systemreset(); } -void PX4Scheduler::_timer_event(void *arg) +void PX4Scheduler::_run_timers(bool called_from_timer_thread) { - if (_nested_atomic_ctr != 0) { - _timer_pending = true; - return; - } - - uint32_t tnow = _micros(); + uint32_t tnow = micros(); if (_in_timer_proc) { - // the timer calls took longer than the period of the - // timer. This is bad, and may indicate a serious - // driver failure. We can't just call the drivers - // again, as we could run out of stack. So we only - // call the _failsafe call. It's job is to detect if - // the drivers or the main loop are indeed dead and to - // activate whatever failsafe it thinks may help if - // need be. We assume the failsafe code can't - // block. If it does then we will recurse and die when - // we run out of stack - if (_failsafe != NULL) { - _failsafe(tnow); - } return; } _in_timer_proc = true; @@ -171,6 +142,8 @@ void PX4Scheduler::_timer_event(void *arg) _timer_proc[i](tnow); } } + } else if (called_from_timer_thread) { + _timer_event_missed = true; } // and the failsafe, if one is setup @@ -181,9 +154,21 @@ void PX4Scheduler::_timer_event(void *arg) _in_timer_proc = false; } -void PX4Scheduler::panic(const prog_char_t *errormsg) { +void *PX4Scheduler::_timer_thread(void) +{ + while (!_px4_thread_should_exit) { + // run timers at 1kHz + poll(NULL, 0, 1); + _run_timers(true); + } + return NULL; +} + +void PX4Scheduler::panic(const prog_char_t *errormsg) +{ write(1, errormsg, strlen(errormsg)); hal.scheduler->delay_microseconds(10000); + _px4_thread_should_exit = true; exit(1); } diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h index 797ae6cc93..4b427af534 100644 --- a/libraries/AP_HAL_PX4/Scheduler.h +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -7,9 +7,9 @@ #include "AP_HAL_PX4_Namespace.h" #include #include -#include +#include -#define PX4_SCHEDULER_MAX_TIMER_PROCS 4 +#define PX4_SCHEDULER_MAX_TIMER_PROCS 8 /* Scheduler implementation: */ class PX4::PX4Scheduler : public AP_HAL::Scheduler { @@ -27,11 +27,8 @@ public: void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us); void suspend_timer_procs(); void resume_timer_procs(); - void begin_atomic(); - void end_atomic(); void reboot(); void panic(const prog_char_t *errormsg); - bool interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; } bool in_timerprocess(); bool system_initializing(); @@ -39,22 +36,20 @@ public: private: bool _initialized; - static uint8_t _nested_atomic_ctr; AP_HAL::Proc _delay_cb; uint16_t _min_delay_cb_ms; - static AP_HAL::TimedProc _failsafe; - struct hrt_call _call; - static bool _timer_pending; - static uint64_t _sketch_start_time; + AP_HAL::TimedProc _failsafe; + pthread_t _thread; + volatile bool _timer_pending; + uint64_t _sketch_start_time; - static volatile bool _timer_suspended; - static AP_HAL::TimedProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS]; - static uint8_t _num_timer_procs; - static bool _in_timer_proc; - - // callable from interrupt handler - static uint32_t _micros(); - static void _timer_event(void *arg); + volatile bool _timer_suspended; + AP_HAL::TimedProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS]; + uint8_t _num_timer_procs; + volatile bool _in_timer_proc; + volatile bool _timer_event_missed; + void *_timer_thread(void); + void _run_timers(bool called_from_timer_thread); }; #endif