Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -04:00
Code Issues 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; _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++) { for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) { 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) { if (_num_timer_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
_timer_proc[_num_timer_procs] = proc; _timer_proc[_num_timer_procs] = proc;
_timer_arg[_num_timer_procs] = arg;
_num_timer_procs++; _num_timer_procs++;
} else { } else {
hal.console->printf("Out of timer processes\n"); 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++) { for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) { 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) { if (_num_io_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc; _io_proc[_num_io_procs] = proc;
_io_arg[_num_io_procs] = arg;
_num_io_procs++; _num_io_procs++;
} else { } else {
hal.console->printf("Out of IO processes\n"); 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) void PX4Scheduler::_run_timers(bool called_from_timer_thread)
{ {
uint32_t tnow = micros();
if (_in_timer_proc) { if (_in_timer_proc) {
return; return;
} }
@ -196,7 +197,7 @@ void PX4Scheduler::_run_timers(bool called_from_timer_thread)
// now call the timer based drivers // now call the timer based drivers
for (int i = 0; i < _num_timer_procs; i++) { for (int i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] != NULL) { if (_timer_proc[i] != NULL) {
_timer_proc[i](tnow); _timer_proc[i](_timer_arg[i]);
} }
} }
} else if (called_from_timer_thread) { } 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 // and the failsafe, if one is setup
if (_failsafe != NULL) { if (_failsafe != NULL) {
_failsafe(tnow); _failsafe(NULL);
} }
// process analog input // process analog input
@ -235,7 +236,6 @@ void *PX4Scheduler::_timer_thread(void)
void PX4Scheduler::_run_io(void) void PX4Scheduler::_run_io(void)
{ {
uint32_t tnow = micros();
if (_in_io_proc) { if (_in_io_proc) {
return; return;
} }
@ -245,7 +245,7 @@ void PX4Scheduler::_run_io(void)
// now call the IO based drivers // now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) { for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] != NULL) { 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(); uint32_t micros();
void delay_microseconds(uint16_t us); void delay_microseconds(uint16_t us);
void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms); void register_delay_callback(AP_HAL::Proc, uint16_t min_time_ms);
void register_timer_process(AP_HAL::TimedProc); void register_timer_process(AP_HAL::TimedProc, void *);
void register_io_process(AP_HAL::TimedProc); void register_io_process(AP_HAL::TimedProc, void *);
void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us); void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
void suspend_timer_procs(); void suspend_timer_procs();
void resume_timer_procs(); void resume_timer_procs();
@ -54,10 +54,12 @@ private:
volatile bool _timer_suspended; volatile bool _timer_suspended;
AP_HAL::TimedProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS]; AP_HAL::TimedProc _timer_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
void * _timer_arg[PX4_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_timer_procs; uint8_t _num_timer_procs;
volatile bool _in_timer_proc; volatile bool _in_timer_proc;
AP_HAL::TimedProc _io_proc[PX4_SCHEDULER_MAX_TIMER_PROCS]; AP_HAL::TimedProc _io_proc[PX4_SCHEDULER_MAX_TIMER_PROCS];
void * _io_arg[PX4_SCHEDULER_MAX_TIMER_PROCS];
uint8_t _num_io_procs; uint8_t _num_io_procs;
volatile bool _in_io_proc; volatile bool _in_io_proc;