AP_Vehicle: let AP_Vehicle handle loop()

libraries/AP_Vehicle/AP_Vehicle.cpp

@@ -42,6 +42,11 @@ 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();
 
@@ -53,6 +58,12 @@ void AP_Vehicle::setup()
     AP::scheduler().init(tasks, task_count, log_bit);
 }
 
+void AP_Vehicle::loop()
+{
+    scheduler.loop();
+    G_Dt = scheduler.get_loop_period_s();
+}
+
 /*
   common scheduler table for fast CPUs - all common vehicle tasks
   should be listed here, along with how often they should be called (in hz)

libraries/AP_Vehicle/AP_Vehicle.h

@@ -65,6 +65,9 @@ public:
     // which is called from setup().
     void setup(void) override final;
 
+    // HAL::Callbacks implementation.
+    void loop() override final;
+
     bool virtual set_mode(const uint8_t new_mode, const ModeReason reason) = 0;
     uint8_t virtual get_mode() const = 0;
 
@@ -173,6 +176,14 @@ protected:
     AP_BoardConfig_CAN BoardConfig_CAN;
 #endif
 
+    // main loop scheduler
+    AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&AP_Vehicle::fast_loop, void)};
+    virtual void fast_loop() { }
+
+    // IMU variables
+    // Integration time; time last loop took to run
+    float G_Dt;
+
     // sensor drivers
     AP_GPS gps;
     AP_Baro barometer;