Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 09:28:31 -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

23
libraries/AP_HAL_ChibiOS/Scheduler.cpp
View File

@ -49,10 +49,13 @@ THD_WORKING_AREA(_uavcan_thread_wa, 4096);
#endif
Scheduler::Scheduler()
{}
{
}
void Scheduler::init()
{
chBSemObjectInit(&_timer_semaphore, false);
chBSemObjectInit(&_io_semaphore, false);
// setup the timer thread - this will call tasks at 1kHz
_timer_thread_ctx = chThdCreateStatic(_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)
{
chBSemWait(&_timer_semaphore);
for (uint8_t i = 0; i < _num_timer_procs; i++) {
if (_timer_proc[i] == proc) {
chBSemSignal(&_timer_semaphore);
return;
}
}
@ -199,12 +204,15 @@ void Scheduler::register_timer_process(AP_HAL::MemberProc proc)
} else {
hal.console->printf("Out of timer processes\n");
}
chBSemSignal(&_timer_semaphore);
}
void Scheduler::register_io_process(AP_HAL::MemberProc proc)
{
chBSemWait(&_io_semaphore);
for (uint8_t i = 0; i < _num_io_procs; i++) {
if (_io_proc[i] == proc) {
chBSemSignal(&_io_semaphore);
return;
}
}
@ -215,6 +223,7 @@ void Scheduler::register_io_process(AP_HAL::MemberProc proc)
} else {
hal.console->printf("Out of IO processes\n");
}
chBSemSignal(&_io_semaphore);
}
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;
int num_procs = 0;
chBSemWait(&_timer_semaphore);
num_procs = _num_timer_procs;
chBSemSignal(&_timer_semaphore);
// 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]) {
_timer_proc[i]();
}
@ -339,8 +352,12 @@ void Scheduler::_run_io(void)
}
_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
for (int i = 0; i < _num_io_procs; i++) {
for (int i = 0; i < num_procs; i++) {
if (_io_proc[i]) {
_io_proc[i]();
}

2
libraries/AP_HAL_ChibiOS/Scheduler.h
View File

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