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

HAL_AVR_SITL: updates for AP_HAL::MemberProc

Browse Source
This commit is contained in:
Andrew Tridgell 2013-09-30 20:35:42 +10:00
parent a2b879493d
commit 82fe40224b
2 changed files with 16 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -24,17 +24,15 @@ using namespace AVR_SITL;
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
AP_HAL::TimedProc SITLScheduler::_failsafe = NULL; AP_HAL::Proc SITLScheduler::_failsafe = NULL;
volatile bool SITLScheduler::_timer_suspended = false; volatile bool SITLScheduler::_timer_suspended = false;
volatile bool SITLScheduler::_timer_event_missed = false; volatile bool SITLScheduler::_timer_event_missed = false;
AP_HAL::TimedProc SITLScheduler::_timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; AP_HAL::MemberProc 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; uint8_t SITLScheduler::_num_timer_procs = 0;
bool SITLScheduler::_in_timer_proc = false; bool SITLScheduler::_in_timer_proc = false;
AP_HAL::TimedProc SITLScheduler::_io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS] = {NULL}; AP_HAL::MemberProc 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; uint8_t SITLScheduler::_num_io_procs = 0;
bool SITLScheduler::_in_io_proc = false; bool SITLScheduler::_in_io_proc = false;
@ -137,7 +135,7 @@ void SITLScheduler::register_delay_callback(AP_HAL::Proc proc,
_min_delay_cb_ms = min_time_ms; _min_delay_cb_ms = min_time_ms;
} }
void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg) void SITLScheduler::register_timer_process(AP_HAL::MemberProc proc)
{ {
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) {
@ -147,13 +145,12 @@ void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc, void *arg)
if (_num_timer_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) { if (_num_timer_procs < SITL_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++;
} }
} }
void SITLScheduler::register_io_process(AP_HAL::TimedProc proc, void *arg) void SITLScheduler::register_io_process(AP_HAL::MemberProc proc)
{ {
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) {
@ -163,13 +160,12 @@ void SITLScheduler::register_io_process(AP_HAL::TimedProc proc, void *arg)
if (_num_io_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) { if (_num_io_procs < SITL_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++;
} }
} }
void SITLScheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us) void SITLScheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
{ {
_failsafe = failsafe; _failsafe = failsafe;
} }
@ -228,7 +224,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
// block. If it does then we will recurse and die when // block. If it does then we will recurse and die when
// we run out of stack // we run out of stack
if (_failsafe != NULL) { if (_failsafe != NULL) {
_failsafe(NULL); _failsafe();
} }
return; return;
} }
@ -238,7 +234,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
// 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](_timer_arg[i]); _timer_proc[i]();
} }
} }
} else if (called_from_isr) { } else if (called_from_isr) {
@ -247,7 +243,7 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
// and the failsafe, if one is setup // and the failsafe, if one is setup
if (_failsafe != NULL) { if (_failsafe != NULL) {
_failsafe(NULL); //_failsafe(NULL);
} }
_in_timer_proc = false; _in_timer_proc = false;
@ -264,7 +260,7 @@ void SITLScheduler::_run_io_procs(bool called_from_isr)
// 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](_io_arg[i]); _io_proc[i]();
} }
} }
} else if (called_from_isr) { } else if (called_from_isr) {

14
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -22,14 +22,14 @@ public:
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 *); void register_timer_process(AP_HAL::MemberProc);
void register_io_process(AP_HAL::TimedProc, void *); void register_io_process(AP_HAL::MemberProc);
void suspend_timer_procs(); void suspend_timer_procs();
void resume_timer_procs(); void resume_timer_procs();
bool in_timerprocess(); bool in_timerprocess();
void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us); void register_timer_failsafe(AP_HAL::Proc, uint32_t period_us);
bool system_initializing(); bool system_initializing();
void system_initialized(); void system_initialized();
@ -51,17 +51,15 @@ private:
AP_HAL::Proc _delay_cb; AP_HAL::Proc _delay_cb;
uint16_t _min_delay_cb_ms; uint16_t _min_delay_cb_ms;
static struct timeval _sketch_start_time; static struct timeval _sketch_start_time;
static AP_HAL::TimedProc _failsafe; static AP_HAL::Proc _failsafe;
static void _run_timer_procs(bool called_from_isr); static void _run_timer_procs(bool called_from_isr);
static void _run_io_procs(bool called_from_isr); static void _run_io_procs(bool called_from_isr);
static volatile bool _timer_suspended; static volatile bool _timer_suspended;
static volatile bool _timer_event_missed; static volatile bool _timer_event_missed;
static AP_HAL::TimedProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS]; static AP_HAL::MemberProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
static void *_timer_arg[SITL_SCHEDULER_MAX_TIMER_PROCS]; static AP_HAL::MemberProc _io_proc[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_timer_procs;
static uint8_t _num_io_procs; static uint8_t _num_io_procs;
static bool _in_timer_proc; static bool _in_timer_proc;