diff --git a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h index 87a442ec3e..5c83bceaec 100644 --- a/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h +++ b/libraries/AP_HAL_PX4/AP_HAL_PX4_Namespace.h @@ -4,6 +4,7 @@ namespace PX4 { class PX4ConsoleDriver; + class PX4Scheduler; } #endif //__AP_HAL_PX4_NAMESPACE_H__ diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp index a424174320..aa8c466ff3 100644 --- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp +++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp @@ -8,6 +8,7 @@ #include "AP_HAL_PX4_Namespace.h" #include "HAL_PX4_Class.h" #include "Console.h" +#include "Scheduler.h" #include #include @@ -26,10 +27,10 @@ static Empty::EmptyStorage storageDriver; static Empty::EmptyGPIO gpioDriver; static Empty::EmptyRCInput rcinDriver; static Empty::EmptyRCOutput rcoutDriver; -static Empty::EmptyScheduler schedulerInstance; static Empty::EmptyUtil utilInstance; static PX4ConsoleDriver consoleDriver; +static PX4Scheduler schedulerInstance; HAL_PX4::HAL_PX4() : AP_HAL::HAL( diff --git a/libraries/AP_HAL_PX4/Scheduler.cpp b/libraries/AP_HAL_PX4/Scheduler.cpp new file mode 100644 index 0000000000..bfc90b5777 --- /dev/null +++ b/libraries/AP_HAL_PX4/Scheduler.cpp @@ -0,0 +1,225 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 + +#include "AP_HAL_PX4.h" +#include "Scheduler.h" +#include +#include +#include +#include +#include +#include + +#define MAIN_TIMER_SIGNAL 17 + +using namespace PX4; + +extern const AP_HAL::HAL& hal; + + +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; +struct timeval PX4Scheduler::_sketch_start_time; + +PX4Scheduler::PX4Scheduler() +{} + +void PX4Scheduler::init(void *unused) +{ + sigset_t sigset; + struct sigaction act; + struct sigaction oact; + struct sigevent notify; + struct itimerspec timer; + timer_t timerid; + int status; + + gettimeofday(&_sketch_start_time,NULL); + + /* setup a posix timer at 1kHz */ + (void)sigemptyset(&sigset); + (void)sigaddset(&sigset, MAIN_TIMER_SIGNAL); + status = sigprocmask(SIG_UNBLOCK, &sigset, NULL); + if (status != OK) goto failed; + + act.sa_sigaction = _timer_event; + act.sa_flags = SA_SIGINFO; + + (void)sigfillset(&act.sa_mask); + (void)sigdelset(&act.sa_mask, MAIN_TIMER_SIGNAL); + status = sigaction(MAIN_TIMER_SIGNAL, &act, &oact); + if (status != OK) goto failed; + + notify.sigev_notify = SIGEV_SIGNAL; + notify.sigev_signo = MAIN_TIMER_SIGNAL; + notify.sigev_value.sival_int = 0; + + status = timer_create(CLOCK_REALTIME, ¬ify, &timerid); + if (status != OK) goto failed; + + /* Start the POSIX timer */ + + timer.it_value.tv_sec = 0; + timer.it_value.tv_nsec = 1000000; + timer.it_interval.tv_sec = 0; + timer.it_interval.tv_nsec = 1000000; + + status = timer_settime(timerid, 0, &timer, NULL); + if (status != OK) goto failed; + + return; + +failed: + panic("Failed to setup PX4 1kHz timer"); +} + +uint32_t PX4Scheduler::_micros() +{ + struct timeval tp; + gettimeofday(&tp,NULL); + return 1.0e6*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - + (_sketch_start_time.tv_sec + + (_sketch_start_time.tv_usec*1.0e-6))); +} + +uint32_t PX4Scheduler::micros() +{ + return _micros(); +} + +uint32_t PX4Scheduler::millis() +{ + struct timeval tp; + gettimeofday(&tp,NULL); + return 1.0e3*((tp.tv_sec + (tp.tv_usec*1.0e-6)) - + (_sketch_start_time.tv_sec + + (_sketch_start_time.tv_usec*1.0e-6))); +} + +void PX4Scheduler::delay_microseconds(uint16_t usec) +{ + uint32_t start = micros(); + while (micros() - start < usec) { + usleep(usec - (micros() - start)); + } +} + +void PX4Scheduler::delay(uint16_t ms) +{ + uint32_t start = micros(); + + while (ms > 0) { + while ((micros() - start) >= 1000) { + ms--; + if (ms == 0) break; + start += 1000; + } + if (_min_delay_cb_ms <= ms) { + if (_delay_cb) { + _delay_cb(); + } + } + } +} + +void PX4Scheduler::register_delay_callback(AP_HAL::Proc proc, + uint16_t min_time_ms) +{ + _delay_cb = proc; + _min_delay_cb_ms = min_time_ms; +} + +void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc) +{ + for (uint8_t i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i] == proc) { + return; + } + } + + if (_num_timer_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) { + _timer_proc[_num_timer_procs] = proc; + _num_timer_procs++; + } + +} + +void PX4Scheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us) +{ + _failsafe = failsafe; +} + +void PX4Scheduler::suspend_timer_procs() { + _timer_suspended = true; +} + +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--; +} + +void PX4Scheduler::reboot() +{ + hal.uartA->println_P(PSTR("REBOOT NOT IMPLEMENTED\r\n")); +} + +void PX4Scheduler::_timer_event(int signo, siginfo_t *info, void *ucontext) +{ + 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; + + if (!_timer_suspended) { + // now call the timer based drivers + for (int i = 0; i < _num_timer_procs; i++) { + if (_timer_proc[i] != NULL) { + _timer_proc[i](tnow); + } + } + } + + // and the failsafe, if one is setup + if (_failsafe != NULL) { + _failsafe(tnow); + } + + _in_timer_proc = false; +} + +void PX4Scheduler::panic(const prog_char_t *errormsg) { + hal.console->println_P(errormsg); + for(;;); +} + +#endif diff --git a/libraries/AP_HAL_PX4/Scheduler.h b/libraries/AP_HAL_PX4/Scheduler.h new file mode 100644 index 0000000000..286532f24f --- /dev/null +++ b/libraries/AP_HAL_PX4/Scheduler.h @@ -0,0 +1,55 @@ + +#ifndef __AP_HAL_PX4_SCHEDULER_H__ +#define __AP_HAL_PX4_SCHEDULER_H__ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#include "AP_HAL_PX4_Namespace.h" +#include +#include + +#define PX4_SCHEDULER_MAX_TIMER_PROCS 4 + +/* Scheduler implementation: */ +class PX4::PX4Scheduler : public AP_HAL::Scheduler { +public: + PX4Scheduler(); + /* AP_HAL::Scheduler methods */ + + void init(void *unused); + void delay(uint16_t ms); + uint32_t millis(); + uint32_t micros(); + void delay_microseconds(uint16_t us); + void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); + void register_timer_process(AP_HAL::TimedProc); + 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; } + +private: + uint8_t _nested_atomic_ctr; + AP_HAL::Proc _delay_cb; + uint16_t _min_delay_cb_ms; + static struct timeval _sketch_start_time; + static AP_HAL::TimedProc _failsafe; + + 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(int signo, siginfo_t *info, void *ucontext); + +}; +#endif +#endif // __AP_HAL_PX4_SCHEDULER_H__ + +