diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 093a95f003..0819e23e0a 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -209,19 +209,17 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { #endif }; -constexpr int8_t Copter::_failsafe_priorities[7]; - -void Copter::setup() +void Copter::get_scheduler_tasks(const AP_Scheduler::Task *&tasks, + uint8_t &task_count, + uint32_t &log_bit) { - // Load the default values of variables listed in var_info[]s - AP_Param::setup_sketch_defaults(); - - init_ardupilot(); - - // initialise the main loop scheduler - scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), MASK_LOG_PM); + tasks = &scheduler_tasks[0]; + task_count = ARRAY_SIZE(scheduler_tasks); + log_bit = MASK_LOG_PM; } +constexpr int8_t Copter::_failsafe_priorities[7]; + void Copter::loop() { scheduler.loop(); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 81b29110cb..5c85a5bf74 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -236,7 +236,6 @@ public: Copter(void); // HAL::Callbacks implementation. - void setup() override; void loop() override; private: @@ -640,7 +639,10 @@ private: void set_failsafe_gcs(bool b); void update_using_interlock(); - // ArduCopter.cpp + // Copter.cpp + void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, + uint8_t &task_count, + uint32_t &log_bit) override; void fast_loop(); void rc_loop(); void throttle_loop(); @@ -859,7 +861,7 @@ private: void auto_trim(); // system.cpp - void init_ardupilot(); + void init_ardupilot() override; void startup_INS_ground(); void update_dynamic_notch(); bool position_ok() const;