aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

HAL_PX4: update for new scheduler API

Browse Source
This commit is contained in:
Andrew Tridgell 2013-09-28 16:29:04 +10:00
parent cca59ce3c9
commit 7ec242146d
2 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_HAL_PX4/Scheduler.cpp
View File

@ -128,7 +128,7 @@ void PX4Scheduler::register_delay_callback(AP_HAL::Proc proc,
_min_delay_cb_ms = min_time_ms;
}
void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc)
void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg)
{
for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) {
@ -138,13 +138,14 @@ void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc)
if (_num_timer_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
_timer_proc[_num_timer_procs] = proc;
_timer_arg[_num_timer_procs] = arg;
_num_timer_procs++;
} else {
hal.console->printf("Out of timer processes\n");
}
}
void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc)
void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc, void *arg)
{
for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
@ -154,6 +155,7 @@ void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc)
if (_num_io_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc;
_io_arg[_num_io_procs] = arg;
_num_io_procs++;
} else {
hal.console->printf("Out of IO processes\n");
@ -186,7 +188,6 @@ void PX4Scheduler::reboot(bool hold_in_bootloader)
void PX4Scheduler::_run_timers(bool called_from_timer_thread)
{
uint32_t tnow = micros();
if (_in_timer_proc) {
return;
}
@ -196,7 +197,7 @@ void PX4Scheduler::_run_timers(bool called_from_timer_thread)
// now call the timer based drivers
for (int i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] != NULL) {
_timer_proc[i](tnow);
_timer_proc[i](_timer_arg[i]);
}
}
} else if (called_from_timer_thread) {
@ -205,7 +206,7 @@ void PX4Scheduler::_run_timers(bool called_from_timer_thread)
// and the failsafe, if one is setup
if (_failsafe != NULL) {
_failsafe(tnow);
_failsafe(NULL);
}
// process analog input
@ -235,7 +236,6 @@ void *PX4Scheduler::_timer_thread(void)
void PX4Scheduler::_run_io(void)
{
uint32_t tnow = micros();
if (_in_io_proc) {
return;
}
@ -245,7 +245,7 @@ void PX4Scheduler::_run_io(void)
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] != NULL) {
_io_proc[i](tnow);
_io_proc[i](_io_arg[i]);
}
}
}

6
libraries/AP_HAL_PX4/Scheduler.h
View File

@ -31,8 +31,8 @@ public:
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_io_process(AP_HAL::TimedProc);
void register_timer_process(AP_HAL::TimedProc, void *);
void register_io_process(AP_HAL::TimedProc, void *);
void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
void suspend_timer_procs();
void resume_timer_procs();
@ -54,10 +54,12 @@ private:
volatile bool _timer_suspended;
AP_HAL::TimedProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
void * _timer_arg[PX4_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_timer_procs;
volatile bool _in_timer_proc;
AP_HAL::TimedProc _io_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
void * _io_arg[PX4_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_io_procs;
volatile bool _in_io_proc;