/
Scheduler.cpp
68 lines (57 loc) · 1.43 KB
/
Scheduler.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#include "Scheduler.h"
#include "AP_HAL.h"
#include <stdio.h>
using namespace AP_HAL;
extern const AP_HAL::HAL& hal;
void Scheduler::late_reboot() {
hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&Scheduler::do_late_reboot, void) , "scheduler", 2048, AP_HAL::Scheduler::PRIORITY_MAIN, 0);
}
void Scheduler::do_late_reboot() {
hal.scheduler->delay(1000);
hal.scheduler->reboot();
}
void Scheduler::register_delay_callback(AP_HAL::Proc proc,
uint16_t min_time_ms)
{
_delay_cb = proc;
_min_delay_cb_ms = min_time_ms;
}
void Scheduler::call_delay_cb()
{
if (_delay_cb == nullptr) {
return;
}
if (_in_delay_callback) {
// don't recurse!
return;
}
_in_delay_callback = true;
_delay_cb();
_in_delay_callback = false;
}
ExpectDelay::ExpectDelay(uint32_t ms)
{
hal.scheduler->expect_delay_ms(ms);
}
ExpectDelay::~ExpectDelay()
{
hal.scheduler->expect_delay_ms(0);
}
/*
implement TimeCheck class for TIME_CHECK() support
*/
TimeCheck::TimeCheck(uint32_t _limit_ms, const char *_file, uint32_t _line) :
limit_ms(_limit_ms),
file(_file),
line(_line)
{
start_ms = AP_HAL::millis();
}
TimeCheck::~TimeCheck()
{
const uint32_t end_ms = AP_HAL::millis();
const uint32_t delta_ms = end_ms - start_ms;
if (delta_ms > limit_ms) {
::printf("Delta %u at %s:%u\n", unsigned(delta_ms), file, unsigned(line));
}
}