Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_HAL_AVR_SITL: remove begin/end atomic, adjust timer procs

This commit is contained in:
Pat Hickey 2013-01-03 17:32:52 -08:00
parent 688ec864dc
commit bc3b6fcb9f
3 changed files with 29 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_HAL_AVR_SITL/SITL_State.cpp
View File

@ -193,11 +193,11 @@ void SITL_State::_timer_handler(int signum)
static bool in_timer; static bool in_timer;
if (in_timer || ((SITLScheduler *)hal.scheduler)->interrupts_are_blocked()) { if (in_timer || _scheduler->interrupts_are_blocked()){
return; return;
} }
_scheduler->begin_atomic();
_scheduler->sitl_begin_atomic();
in_timer = true; in_timer = true;
#ifndef __CYGWIN__ #ifndef __CYGWIN__
@ -233,14 +233,14 @@ void SITL_State::_timer_handler(int signum)
if (_update_count == 0 && _sitl != NULL) { if (_update_count == 0 && _sitl != NULL) {
_update_gps(0, 0, 0, 0, 0, false); _update_gps(0, 0, 0, 0, 0, false);
_scheduler->timer_event(); _scheduler->timer_event();
_scheduler->end_atomic(); _scheduler->sitl_end_atomic();
in_timer = false; in_timer = false;
return; return;
} }
if (_update_count == last_update_count) { if (_update_count == last_update_count) {
_scheduler->timer_event(); _scheduler->timer_event();
_scheduler->end_atomic(); _scheduler->sitl_end_atomic();
in_timer = false; in_timer = false;
return; return;
} }
@ -262,7 +262,7 @@ void SITL_State::_timer_handler(int signum)
// interrupts, which can lead to recursion // interrupts, which can lead to recursion
_scheduler->timer_event(); _scheduler->timer_event();
_scheduler->end_atomic(); _scheduler->sitl_end_atomic();
in_timer = false; in_timer = false;
} }

26
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -15,6 +15,7 @@ extern const AP_HAL::HAL& hal;
AP_HAL::TimedProc SITLScheduler::_failsafe = NULL; AP_HAL::TimedProc SITLScheduler::_failsafe = NULL;
volatile bool SITLScheduler::_timer_suspended = false; 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}; AP_HAL::TimedProc SITLScheduler::_timer_proc[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;
@ -110,26 +111,25 @@ void SITLScheduler::suspend_timer_procs() {
void SITLScheduler::resume_timer_procs() { void SITLScheduler::resume_timer_procs() {
_timer_suspended = false; _timer_suspended = false;
} if (_timer_event_missed) {
_timer_event_missed = false;
void SITLScheduler::begin_atomic() { _run_timer_procs(false);
_nested_atomic_ctr++;
}
void SITLScheduler::end_atomic() {
if (_nested_atomic_ctr == 0) {
hal.uartA->println_P(PSTR("ATOMIC NESTING ERROR"));
return;
} }
_nested_atomic_ctr--;
} }
void SITLScheduler::sitl_end_atomic() {
if (_nested_atomic_ctr == 0)
hal.uartA->println_P(PSTR("NESTED ATOMIC ERROR"));
else
_nested_atomic_ctr--;
}
void SITLScheduler::reboot() void SITLScheduler::reboot()
{ {
hal.uartA->println_P(PSTR("REBOOT NOT IMPLEMENTED\r\n")); hal.uartA->println_P(PSTR("REBOOT NOT IMPLEMENTED\r\n"));
} }
void SITLScheduler::timer_event() void SITLScheduler::_run_timer_procs(bool called_from_isr)
{ {
uint32_t tnow = _micros(); uint32_t tnow = _micros();
if (_in_timer_proc) { if (_in_timer_proc) {
@ -157,6 +157,8 @@ void SITLScheduler::timer_event()
_timer_proc[i](tnow); _timer_proc[i](tnow);
} }
} }
} else if (called_from_isr) {
_timer_event_missed = true;
} }
// and the failsafe, if one is setup // and the failsafe, if one is setup

13
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -21,21 +21,23 @@ 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 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();
void begin_atomic();
void end_atomic(); void register_timer_failsafe(AP_HAL::TimedProc, uint32_t period_us);
void reboot(); void reboot();
void panic(const prog_char_t *errormsg); void panic(const prog_char_t *errormsg);
bool interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; } bool interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; }
static void timer_event(void); void sitl_begin_atomic() { _nested_atomic_ctr++; }
void sitl_end_atomic();
// callable from interrupt handler // callable from interrupt handler
static uint32_t _micros(); static uint32_t _micros();
static void timer_event() { _run_timer_procs(true); }
private: private:
uint8_t _nested_atomic_ctr; uint8_t _nested_atomic_ctr;
@ -44,7 +46,10 @@ private:
static struct timeval _sketch_start_time; static struct timeval _sketch_start_time;
static AP_HAL::TimedProc _failsafe; static AP_HAL::TimedProc _failsafe;
static void _run_timer_procs(bool called_from_isr);
static volatile bool _timer_suspended; static volatile bool _timer_suspended;
static volatile bool _timer_event_missed;
static AP_HAL::TimedProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS]; static AP_HAL::TimedProc _timer_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
static uint8_t _num_timer_procs; static uint8_t _num_timer_procs;
static bool _in_timer_proc; static bool _in_timer_proc;