diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp index b2c7310dde..d535e21f57 100644 --- a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp +++ b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp @@ -42,28 +42,41 @@ const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; -// INS declaration -static AP_InertialSensor ins; +class SchedTest { +public: + void setup(); + void loop(); -// loop scheduler object -static AP_Scheduler scheduler; +private: -// counter for ins_update() -static uint32_t ins_counter; + AP_InertialSensor ins; + AP_Scheduler scheduler; + + uint32_t ins_counter; + static const AP_Scheduler::Task scheduler_tasks[]; + + void ins_update(void); + void one_hz_print(void); + void five_second_call(void); +}; + +static SchedTest schedtest; + +#define SCHED_TASK(func) FUNCTOR_BIND_VOID(&schedtest, &SchedTest::func, void) /* scheduler table - all regular tasks are listed here, along with how often they should be called (in 20ms units) and the maximum time they are expected to take (in microseconds) */ -static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = { - { ins_update, 1, 1000 }, - { one_hz_print, 50, 1000 }, - { five_second_call, 250, 1800 }, +const AP_Scheduler::Task SchedTest::scheduler_tasks[] PROGMEM = { + { SCHED_TASK(ins_update), 1, 1000 }, + { SCHED_TASK(one_hz_print), 50, 1000 }, + { SCHED_TASK(five_second_call), 250, 1800 }, }; -void setup(void) +void SchedTest::setup(void) { // we ins.init(AP_InertialSensor::COLD_START, @@ -73,7 +86,7 @@ void setup(void) scheduler.init(&scheduler_tasks[0], sizeof(scheduler_tasks)/sizeof(scheduler_tasks[0])); } -void loop(void) +void SchedTest::loop(void) { // wait for an INS sample ins.wait_for_sample(); @@ -88,7 +101,7 @@ void loop(void) /* update inertial sensor, reading data */ -static void ins_update(void) +void SchedTest::ins_update(void) { ins_counter++; ins.update(); @@ -97,7 +110,7 @@ static void ins_update(void) /* print something once a second */ -static void one_hz_print(void) +void SchedTest::one_hz_print(void) { hal.console->printf("one_hz: t=%lu\n", hal.scheduler->millis()); } @@ -105,9 +118,23 @@ static void one_hz_print(void) /* print something every 5 seconds */ -static void five_second_call(void) +void SchedTest::five_second_call(void) { hal.console->printf("five_seconds: t=%lu ins_counter=%u\n", hal.scheduler->millis(), ins_counter); } +/* + compatibility with old pde style build + */ +void setup(void); +void loop(void); + +void setup(void) +{ + schedtest.setup(); +} +void loop(void) +{ + schedtest.loop(); +} AP_HAL_MAIN(); diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/make.inc b/libraries/AP_Scheduler/examples/Scheduler_test/make.inc index c236a597a2..8ad3f0229e 100644 --- a/libraries/AP_Scheduler/examples/Scheduler_test/make.inc +++ b/libraries/AP_Scheduler/examples/Scheduler_test/make.inc @@ -31,3 +31,4 @@ LIBRARIES += Filter LIBRARIES += GCS_MAVLink LIBRARIES += SITL LIBRARIES += StorageManager +LIBRARIES += AP_OpticalFlow