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

AP_HAL_ChibiOS: Wrap process registration with a semaphore

Browse Source
This commit is contained in:
Michael du Breuil 2018-05-21 16:08:35 -07:00 committed by Andrew Tridgell
parent a832bdfd04
commit 1123023a6f
2 changed files with 23 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
libraries/AP_HAL_ChibiOS/Scheduler.cpp
View File

@ -49,10 +49,13 @@ THD_WORKING_AREA(_uavcan_thread_wa, 4096);
#endif #endif
Scheduler::Scheduler() Scheduler::Scheduler()
{} {
}
void Scheduler::init() void Scheduler::init()
{ {
chBSemObjectInit(&_timer_semaphore, false);
chBSemObjectInit(&_io_semaphore, false);
// setup the timer thread - this will call tasks at 1kHz // setup the timer thread - this will call tasks at 1kHz
_timer_thread_ctx = chThdCreateStatic(_timer_thread_wa, _timer_thread_ctx = chThdCreateStatic(_timer_thread_wa,
sizeof(_timer_thread_wa), sizeof(_timer_thread_wa),
@ -187,8 +190,10 @@ void Scheduler::delay(uint16_t ms)
void Scheduler::register_timer_process(AP_HAL::MemberProc proc) void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
{ {
chBSemWait(&_timer_semaphore);
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) {
chBSemSignal(&_timer_semaphore);
return; return;
} }
} }
@ -199,22 +204,26 @@ void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
} else { } else {
hal.console->printf("Out of timer processes\n"); hal.console->printf("Out of timer processes\n");
} }
chBSemSignal(&_timer_semaphore);
} }
void Scheduler::register_io_process(AP_HAL::MemberProc proc) void Scheduler::register_io_process(AP_HAL::MemberProc proc)
{ {
chBSemWait(&_io_semaphore);
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) {
chBSemSignal(&_io_semaphore);
return; return;
} }
} }
if (_num_io_procs < CHIBIOS_SCHEDULER_MAX_TIMER_PROCS) { if (_num_io_procs < CHIBIOS_SCHEDULER_MAX_TIMER_PROCS) {
_io_proc[_num_io_procs] = proc; _io_proc[_num_io_procs] = proc;
_num_io_procs++; _num_io_procs++;
} else { } else {
hal.console->printf("Out of IO processes\n"); hal.console->printf("Out of IO processes\n");
} }
chBSemSignal(&_io_semaphore);
} }
void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us) void Scheduler::register_timer_failsafe(AP_HAL::Proc failsafe, uint32_t period_us)
@ -246,8 +255,12 @@ void Scheduler::_run_timers()
} }
_in_timer_proc = true; _in_timer_proc = true;
int num_procs = 0;
chBSemWait(&_timer_semaphore);
num_procs = _num_timer_procs;
chBSemSignal(&_timer_semaphore);
// 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_procs; i++) {
if (_timer_proc[i]) { if (_timer_proc[i]) {
_timer_proc[i](); _timer_proc[i]();
} }
@ -339,8 +352,12 @@ void Scheduler::_run_io(void)
} }
_in_io_proc = true; _in_io_proc = true;
int num_procs = 0;
chBSemWait(&_io_semaphore);
num_procs = _num_io_procs;
chBSemSignal(&_io_semaphore);
// 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_procs; i++) {
if (_io_proc[i]) { if (_io_proc[i]) {
_io_proc[i](); _io_proc[i]();
} }

2
libraries/AP_HAL_ChibiOS/Scheduler.h
View File

@ -117,6 +117,8 @@ private:
#if HAL_WITH_UAVCAN #if HAL_WITH_UAVCAN
thread_t* _uavcan_thread_ctx; thread_t* _uavcan_thread_ctx;
#endif #endif
binary_semaphore_t _timer_semaphore;
binary_semaphore_t _io_semaphore;
static void _timer_thread(void *arg); static void _timer_thread(void *arg);
static void _rcin_thread(void *arg); static void _rcin_thread(void *arg);
static void _io_thread(void *arg); static void _io_thread(void *arg);