Skip to content

Commit

Permalink
AP_Vehicle: initialise scheduler early
Browse files Browse the repository at this point in the history
So the loop rate gets clamped before we memoise it and the loop period
in AP_Scheduler
  • Loading branch information
peterbarker committed Feb 6, 2020
1 parent 5580196 commit de2cf89
Showing 1 changed file with 7 additions and 8 deletions.
15 changes: 7 additions & 8 deletions libraries/AP_Vehicle/AP_Vehicle.cpp
Expand Up @@ -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()
Expand Down

0 comments on commit de2cf89

Please sign in to comment.