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

AP_HAL: added register_io_process() 

this is used to register a low priority IO task. Used for file IO in
DataFlash_File.cpp
This commit is contained in:
Andrew Tridgell 2013-04-17 21:33:50 +10:00
parent 5df4796b9a
commit af7381e9e0
7 changed files with 113 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL/Scheduler.h
View File

@ -18,7 +18,13 @@ public:
virtual void register_delay_callback(AP_HAL::Proc,
uint16_t min_time_ms) = 0;
// register a high priority timer task
virtual void register_timer_process(AP_HAL::TimedProc) = 0;
// register a low priority IO task
virtual void register_io_process(AP_HAL::TimedProc) = 0;
// suspend and resume both timer and IO processes
virtual void suspend_timer_procs() = 0;
virtual void resume_timer_procs() = 0;

5
libraries/AP_HAL_AVR/Scheduler.cpp
View File

@ -108,6 +108,11 @@ void AVRScheduler::register_timer_process(AP_HAL::TimedProc proc) {
}
void AVRScheduler::register_io_process(AP_HAL::TimedProc proc)
{
// IO processes not supported on AVR
}
void AVRScheduler::register_timer_failsafe(
AP_HAL::TimedProc failsafe, uint32_t period_us) {
/* XXX Assert period_us == 1000 */

1
libraries/AP_HAL_AVR/Scheduler.h
View File

@ -32,6 +32,7 @@ public:
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 suspend_timer_procs();
void resume_timer_procs();

45
libraries/AP_HAL_AVR_SITL/Scheduler.cpp
View File

@ -16,9 +16,15 @@ extern const AP_HAL::HAL& hal;
AP_HAL::TimedProc SITLScheduler::_failsafe = NULL;
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};
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};
uint8_t SITLScheduler::_num_io_procs = 0;
bool SITLScheduler::_in_io_proc = false;
struct timeval SITLScheduler::_sketch_start_time;
SITLScheduler::SITLScheduler()
@ -100,6 +106,21 @@ void SITLScheduler::register_timer_process(AP_HAL::TimedProc proc)
}
void SITLScheduler::register_io_process(AP_HAL::TimedProc proc)
{
for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
return;
}
}
if (_num_io_procs < SITL_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc;
_num_io_procs++;
}
}
void SITLScheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us)
{
_failsafe = failsafe;
@ -118,7 +139,7 @@ void SITLScheduler::resume_timer_procs() {
}
bool SITLScheduler::in_timerprocess() {
return _in_timer_proc;
return _in_timer_proc || _in_io_proc;
}
bool SITLScheduler::system_initializing() {
@ -185,6 +206,28 @@ void SITLScheduler::_run_timer_procs(bool called_from_isr)
_in_timer_proc = false;
}
void SITLScheduler::_run_io_procs(bool called_from_isr)
{
uint32_t tnow = _micros();
if (_in_io_proc) {
return;
}
_in_io_proc = true;
if (!_timer_suspended) {
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] != NULL) {
_io_proc[i](tnow);
}
}
} else if (called_from_isr) {
_timer_event_missed = true;
}
_in_io_proc = false;
}
void SITLScheduler::panic(const prog_char_t *errormsg) {
hal.console->println_P(errormsg);
for(;;);

7
libraries/AP_HAL_AVR_SITL/Scheduler.h
View File

@ -23,6 +23,7 @@ public:
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 suspend_timer_procs();
void resume_timer_procs();
@ -43,7 +44,7 @@ public:
// callable from interrupt handler
static uint32_t _micros();
static void timer_event() { _run_timer_procs(true); }
static void timer_event() { _run_timer_procs(true); _run_io_procs(true); }
private:
uint8_t _nested_atomic_ctr;
@ -53,12 +54,16 @@ private:
static AP_HAL::TimedProc _failsafe;
static void _run_timer_procs(bool called_from_isr);
static void _run_io_procs(bool called_from_isr);
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 _io_proc[SITL_SCHEDULER_MAX_TIMER_PROCS];
static uint8_t _num_timer_procs;
static uint8_t _num_io_procs;
static bool _in_timer_proc;
static bool _in_io_proc;
bool _initialized;

42
libraries/AP_HAL_PX4/Scheduler.cpp
View File

@ -29,6 +29,7 @@ extern bool _px4_thread_should_exit;
PX4Scheduler::PX4Scheduler() :
_perf_timers(perf_alloc(PC_ELAPSED, "APM_timers")),
_perf_io_timers(perf_alloc(PC_ELAPSED, "APM_IO_timers")),
_perf_delay(perf_alloc(PC_ELAPSED, "APM_delay"))
{}
@ -132,6 +133,22 @@ void PX4Scheduler::register_timer_process(AP_HAL::TimedProc proc)
}
}
void PX4Scheduler::register_io_process(AP_HAL::TimedProc proc)
{
for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
return;
}
}
if (_num_io_procs < PX4_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc;
_num_io_procs++;
} else {
hal.console->printf("Out of IO processes\n");
}
}
void PX4Scheduler::register_timer_failsafe(AP_HAL::TimedProc failsafe, uint32_t period_us)
{
_failsafe = failsafe;
@ -202,6 +219,26 @@ void *PX4Scheduler::_timer_thread(void)
return NULL;
}
void PX4Scheduler::_run_io(void)
{
uint32_t tnow = micros();
if (_in_io_proc) {
return;
}
_in_io_proc = true;
if (!_timer_suspended) {
// now call the IO based drivers
for (int i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] != NULL) {
_io_proc[i](tnow);
}
}
}
_in_io_proc = false;
}
void *PX4Scheduler::_io_thread(void)
{
while (!_px4_thread_should_exit) {
@ -214,6 +251,11 @@ void *PX4Scheduler::_io_thread(void)
// process any pending storage writes
((PX4Storage *)hal.storage)->_timer_tick();
// run registered IO processes
perf_begin(_perf_io_timers);
_run_io();
perf_end(_perf_io_timers);
}
return NULL;
}

9
libraries/AP_HAL_PX4/Scheduler.h
View File

@ -31,6 +31,7 @@ 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_failsafe(AP_HAL::TimedProc, uint32_t period_us);
void suspend_timer_procs();
void resume_timer_procs();
@ -50,9 +51,15 @@ private:
uint64_t _sketch_start_time;
volatile bool _timer_suspended;
AP_HAL::TimedProc _timer_proc[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];
uint8_t _num_io_procs;
volatile bool _in_io_proc;
volatile bool _timer_event_missed;
pthread_t _timer_thread_ctx;
@ -62,8 +69,10 @@ private:
void *_io_thread(void);
void _run_timers(bool called_from_timer_thread);
void _run_io(void);
perf_counter_t _perf_timers;
perf_counter_t _perf_io_timers;
perf_counter_t _perf_delay;
};
#endif