diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 829383dd31..b05851bdfe 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -367,9 +367,11 @@ void AP_Vehicle::setup() networking.init(); #endif +#if AP_SCHEDULER_ENABLED // Register scheduler_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay hal.scheduler->register_delay_callback(scheduler_delay_callback, 5); +#endif #if HAL_MSP_ENABLED // call MSP init before init_ardupilot to allow for MSP sensors @@ -431,7 +433,11 @@ void AP_Vehicle::setup() // gyro FFT needs to be initialized really late #if HAL_GYROFFT_ENABLED +#if AP_SCHEDULER_ENABLED gyro_fft.init(AP::scheduler().get_loop_rate_hz()); +#else + gyro_fft.init(1000); +#endif #endif #if HAL_RUNCAM_ENABLED runcam.init(); @@ -520,8 +526,13 @@ void AP_Vehicle::setup() void AP_Vehicle::loop() { +#if AP_SCHEDULER_ENABLED scheduler.loop(); G_Dt = scheduler.get_loop_period_s(); +#else + hal.scheduler->delay(1); + G_Dt = 0.001; +#endif if (!done_safety_init) { /* @@ -548,6 +559,7 @@ void AP_Vehicle::loop() } } +#if AP_SCHEDULER_ENABLED /* scheduler table - all regular tasks apart from the fast_loop() should be listed here. @@ -708,6 +720,7 @@ void AP_Vehicle::scheduler_delay_callback() logger.EnableWrites(true); #endif } +#endif // AP_SCHEDULER_ENABLED // if there's been a watchdog reset, notify the world via a statustext: void AP_Vehicle::send_watchdog_reset_statustext() diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index 0ea6a802ca..8b22c9395d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -127,10 +127,10 @@ public: // parameters for example. void notify_no_such_mode(uint8_t mode_number); +#if AP_SCHEDULER_ENABLED void get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks); // implementations *MUST* fill in all passed-in fields or we get // Valgrind errors -#if AP_SCHEDULER_ENABLED virtual void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) = 0; #endif @@ -310,8 +310,10 @@ protected: AP_CANManager can_mgr; #endif +#if AP_SCHEDULER_ENABLED // main loop scheduler AP_Scheduler scheduler; +#endif // IMU variables // Integration time; time last loop took to run @@ -461,7 +463,9 @@ protected: #endif static const struct AP_Param::GroupInfo var_info[]; +#if AP_SCHEDULER_ENABLED static const struct AP_Scheduler::Task scheduler_tasks[]; +#endif #if OSD_ENABLED void publish_osd_info(); @@ -495,8 +499,10 @@ protected: private: +#if AP_SCHEDULER_ENABLED // delay() callback that processing MAVLink packets static void scheduler_delay_callback(); +#endif // if there's been a watchdog reset, notify the world via a // statustext: