Browse Source

HAL_ChibiOS: implement in_expected_delay()

copter407
Andrew Tridgell 5 years ago
parent
commit
8c49690acf
  1. 22
      libraries/AP_HAL_ChibiOS/Scheduler.cpp
  2. 6
      libraries/AP_HAL_ChibiOS/Scheduler.h

22
libraries/AP_HAL_ChibiOS/Scheduler.cpp

@ -321,13 +321,25 @@ void Scheduler::_timer_thread(void *arg) @@ -321,13 +321,25 @@ void Scheduler::_timer_thread(void *arg)
// process any pending RC output requests
hal.rcout->timer_tick();
if (sched->expect_delay_start != 0) {
uint32_t now = AP_HAL::millis();
if (now - sched->expect_delay_start <= sched->expect_delay_length) {
sched->watchdog_pat();
}
if (sched->in_expected_delay()) {
sched->watchdog_pat();
}
}
}
/*
return true if we are in a period of expected delay. This can be
used to suppress error messages
*/
bool Scheduler::in_expected_delay(void) const
{
if (expect_delay_start != 0) {
uint32_t now = AP_HAL::millis();
if (now - expect_delay_start <= expect_delay_length) {
return true;
}
}
return false;
}
#ifndef HAL_NO_MONITOR_THREAD

6
libraries/AP_HAL_ChibiOS/Scheduler.h

@ -105,6 +105,12 @@ public: @@ -105,6 +105,12 @@ public:
*/
void expect_delay_ms(uint32_t ms) override;
/*
return true if we are in a period of expected delay. This can be
used to suppress error messages
*/
bool in_expected_delay(void) const override;
/*
disable interrupts and return a context that can be used to
restore the interrupt state. This can be used to protect

Loading…
Cancel
Save