mirror of https://github.com/ArduPilot/ardupilot
HAL_PX4: run timer thread as SCHED_FIFO
this ensures it never yields to the main thread, which would break our locking assumptions
This commit is contained in:
parent
930e79a9e1
commit
2a10727902
|
@ -90,6 +90,12 @@ static int main_loop(int argc, char **argv)
|
||||||
thread_running = true;
|
thread_running = true;
|
||||||
while (!_px4_thread_should_exit) {
|
while (!_px4_thread_should_exit) {
|
||||||
loop();
|
loop();
|
||||||
|
if (hal.scheduler->in_timerprocess()) {
|
||||||
|
// we are running when a timer process is running! This is
|
||||||
|
// a scheduling error, and breaks the assumptions made in
|
||||||
|
// our locking system
|
||||||
|
::printf("ERROR: timer processing running in loop()\n");
|
||||||
|
}
|
||||||
// yield the CPU between loops to let other apps
|
// yield the CPU between loops to let other apps
|
||||||
// get some CPU time
|
// get some CPU time
|
||||||
pthread_yield();
|
pthread_yield();
|
||||||
|
|
|
@ -41,6 +41,10 @@ void PX4Scheduler::init(void *unused)
|
||||||
param.sched_priority = SCHED_PRIORITY_DEFAULT + 1;
|
param.sched_priority = SCHED_PRIORITY_DEFAULT + 1;
|
||||||
(void)pthread_attr_setschedparam(&thread_attr, ¶m);
|
(void)pthread_attr_setschedparam(&thread_attr, ¶m);
|
||||||
|
|
||||||
|
// we run as a FIFO task as we don't want the main task to preempt
|
||||||
|
// us. This keeps the locking much simpler between the two threads
|
||||||
|
pthread_attr_setschedpolicy(&thread_attr, SCHED_FIFO);
|
||||||
|
|
||||||
pthread_create(&_thread, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_timer_thread, this);
|
pthread_create(&_thread, &thread_attr, (pthread_startroutine_t)&PX4::PX4Scheduler::_timer_thread, this);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -56,6 +60,10 @@ uint32_t PX4Scheduler::millis()
|
||||||
|
|
||||||
void PX4Scheduler::delay_microseconds(uint16_t usec)
|
void PX4Scheduler::delay_microseconds(uint16_t usec)
|
||||||
{
|
{
|
||||||
|
if (_in_timer_proc) {
|
||||||
|
::printf("ERROR: delay_microseconds() from timer process\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
uint32_t start = micros();
|
uint32_t start = micros();
|
||||||
while (micros() - start < usec) {
|
while (micros() - start < usec) {
|
||||||
up_udelay(usec - (micros() - start));
|
up_udelay(usec - (micros() - start));
|
||||||
|
@ -64,6 +72,10 @@ void PX4Scheduler::delay_microseconds(uint16_t usec)
|
||||||
|
|
||||||
void PX4Scheduler::delay(uint16_t ms)
|
void PX4Scheduler::delay(uint16_t ms)
|
||||||
{
|
{
|
||||||
|
if (_in_timer_proc) {
|
||||||
|
::printf("ERROR: delay() from timer process\n");
|
||||||
|
return;
|
||||||
|
}
|
||||||
uint64_t start = hrt_absolute_time();
|
uint64_t start = hrt_absolute_time();
|
||||||
|
|
||||||
while ((hrt_absolute_time() - start)/1000 < ms &&
|
while ((hrt_absolute_time() - start)/1000 < ms &&
|
||||||
|
|
Loading…
Reference in New Issue