mirror of https://github.com/ArduPilot/ardupilot
HAL_PX4: fixes for new Scheduler API
This commit is contained in:
parent
2fe4656a50
commit
7c7a215934
|
@ -82,6 +82,7 @@ static int main_loop(int argc, char **argv)
|
|||
hal.rcout->init(NULL);
|
||||
|
||||
setup();
|
||||
hal.scheduler->system_initialized();
|
||||
|
||||
while (true) {
|
||||
loop();
|
||||
|
|
|
@ -187,4 +187,20 @@ void PX4Scheduler::panic(const prog_char_t *errormsg) {
|
|||
exit(1);
|
||||
}
|
||||
|
||||
bool PX4Scheduler::in_timerprocess() {
|
||||
return _in_timer_proc;
|
||||
}
|
||||
|
||||
bool PX4Scheduler::system_initializing() {
|
||||
return !_initialized;
|
||||
}
|
||||
|
||||
void PX4Scheduler::system_initialized() {
|
||||
if (_initialized) {
|
||||
panic(PSTR("PANIC: scheduler::system_initialized called"
|
||||
"more than once"));
|
||||
}
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
@ -33,7 +33,12 @@ public:
|
|||
void panic(const prog_char_t *errormsg);
|
||||
bool interrupts_are_blocked(void) { return _nested_atomic_ctr != 0; }
|
||||
|
||||
bool in_timerprocess();
|
||||
bool system_initializing();
|
||||
void system_initialized();
|
||||
|
||||
private:
|
||||
bool _initialized;
|
||||
static uint8_t _nested_atomic_ctr;
|
||||
AP_HAL::Proc _delay_cb;
|
||||
uint16_t _min_delay_cb_ms;
|
||||
|
|
Loading…
Reference in New Issue