HAL_ChibiOS: cope with calling delay_microseconds_boost() multiple times

needed for updated IMU wait code
This commit is contained in:
Andrew Tridgell 2019-07-04 20:19:03 +10:00
parent 053f0cb689
commit 898150e460

View File

@ -163,12 +163,12 @@ void Scheduler::boost_end(void)
*/ */
void Scheduler::delay_microseconds_boost(uint16_t usec) void Scheduler::delay_microseconds_boost(uint16_t usec)
{ {
if (in_main_thread()) { if (!_priority_boosted && in_main_thread()) {
set_high_priority(); set_high_priority();
_priority_boosted = true; _priority_boosted = true;
_called_boost = true;
} }
delay_microseconds(usec); //Suspends Thread for desired microseconds delay_microseconds(usec); //Suspends Thread for desired microseconds
_called_boost = true;
} }
/* /*