From de2cf89b60cb4c87190ab709f992f2dd411efe82 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 29 Jan 2020 10:06:37 +1100 Subject: [PATCH] AP_Vehicle: initialise scheduler early So the loop rate gets clamped before we memoise it and the loop period in AP_Scheduler --- libraries/AP_Vehicle/AP_Vehicle.cpp | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 30a52adfa8..8bbbd33b82 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -42,20 +42,19 @@ void AP_Vehicle::setup() load_parameters(); - // time per loop - this gets updated in the main loop() based on - // actual loop rate. Note carefully that scheduler().init() has - // NOT been called yet! - G_Dt = scheduler.get_loop_period_s(); - - // init_ardupilot is where the vehicle does most of its initialisation. - init_ardupilot(); - // initialise the main loop scheduler const AP_Scheduler::Task *tasks; uint8_t task_count; uint32_t log_bit; get_scheduler_tasks(tasks, task_count, log_bit); AP::scheduler().init(tasks, task_count, log_bit); + + // time per loop - this gets updated in the main loop() based on + // actual loop rate + G_Dt = scheduler.get_loop_period_s(); + + // init_ardupilot is where the vehicle does most of its initialisation. + init_ardupilot(); } void AP_Vehicle::loop()