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

HAL_AVR_SITL: updates for new scheduler API

Browse Source
This commit is contained in:
Andrew Tridgell 2013-09-28 16:24:23 +10:00
parent 0ade02a624
commit c08f29fca6
2 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -29,10 +29,12 @@ volatile bool SITLScheduler::_timer_suspended = false;
volatile bool SITLScheduler::_timer_event_missed = false;
AP_HAL::TimedProc SITLScheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
void *SITLScheduler::_timer_arg[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
uint8_t SITLScheduler::_num_timer_procs = 0;
bool SITLScheduler::_in_timer_proc = false;
AP_HAL::TimedProc SITLScheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
void *SITLScheduler::_io_arg[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL};
uint8_t SITLScheduler::_num_io_procs = 0;
bool SITLScheduler::_in_io_proc = false;
@ -135,7 +137,7 @@ void SITLScheduler::register_delay_callback(AP_HAL::Proc proc,
_min_delay_cb_ms = min_time_ms;
}
void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc)
void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg)
{
for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) {
@ -145,12 +147,13 @@ void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc)
if (_num_timer_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) {
_timer_proc[_num_timer_procs] = proc;
_timer_arg[_num_timer_procs] = arg;
_num_timer_procs++;
}
}
void SITLScheduler::register_io_process(AP_HAL::TimedProc proc)
void SITLScheduler::register_io_process(AP_HAL::TimedProc proc, void *arg)
{
for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
@ -160,6 +163,7 @@ void SITLScheduler::register_io_process(AP_HAL::TimedProc proc)
if (_num_io_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc;
_io_arg[_num_io_procs] = arg;
_num_io_procs++;
}
@ -212,7 +216,6 @@ void SITLScheduler::reboot(bool hold_in_bootloader)
void SITLScheduler::_run_timer_procs(bool called_from_isr)
{
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
@ -225,7 +228,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
// block. If it does then we will recurse and die when
// we run out of stack
if (_failsafe != NULL) {
_failsafe(tnow);
_failsafe(NULL);
}
return;
}
@ -235,7 +238,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
// 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_isr) {
@ -244,7 +247,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
// and the failsafe, if one is setup
if (_failsafe != NULL) {
_failsafe(tnow);
_failsafe(NULL);
}
_in_timer_proc = false;
@ -252,7 +255,6 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
void SITLScheduler::_run_io_procs(bool called_from_isr)
{
uint32_t tnow = _micros();
if (_in_io_proc) {
return;
}
@ -262,7 +264,7 @@ void SITLScheduler::_run_io_procs(bool called_from_isr)
// 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]);
}
}
} else if (called_from_isr) {

6
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -22,8 +22,8 @@ public:
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 suspend_timer_procs();
void resume_timer_procs();
@ -59,7 +59,9 @@ private:
static volatile bool _timer_suspended;
static volatile bool _timer_event_missed;
static AP_HAL::TimedProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
static void *_timer_arg[SITL_SCHEDULER_MAX_TIMER_PROCS];
static AP_HAL::TimedProc _io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
static void *_io_arg[SITL_SCHEDULER_MAX_TIMER_PROCS];
static uint8_t _num_timer_procs;
static uint8_t _num_io_procs;
static bool _in_timer_proc;