Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

HAL_PX4: switched scheduler to use a pthread

Browse Source 
this allows the timer tasks to access file descriptors in the main APM
task, which makes writing PX4 device drivers much easier
This commit is contained in:
Andrew Tridgell 2013-01-21 13:54:09 +11:00
parent 4200593206
commit 39e28d48c2
2 changed files with 58 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

105
libraries/AP_HAL_PX4/Scheduler.cpp
View File

@ -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, &param);
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);
}

31
libraries/AP_HAL_PX4/Scheduler.h
View File

@ -7,9 +7,9 @@
#include "AP_HAL_PX4_Namespace.h"
#include <sys/time.h>
#include <signal.h>
#include <drivers/drv_hrt.h>
#include <pthread.h>
#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