From d873ec4533d08e121065751556879e189e0d9730 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 7 Mar 2020 10:49:59 +0000 Subject: [PATCH] AP_Vehicle: separate gyrofft gyro sampling from fft initiation move FFT gyro sampling into fast loop for all vehicles rename FFT update method --- libraries/AP_Vehicle/AP_Vehicle.cpp | 12 +++++++++++- libraries/AP_Vehicle/AP_Vehicle.h | 2 +- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 4fb7d3f52b..3694bcbd3d 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -113,6 +113,16 @@ void AP_Vehicle::loop() G_Dt = scheduler.get_loop_period_s(); } +/* + fast loop callback for all vehicles. This will get called at the end of any vehicle-specific fast loop. + */ +void AP_Vehicle::fast_loop() +{ +#if HAL_GYROFFT_ENABLED + gyro_fft.sample_gyros(); +#endif +} + /* common scheduler table for fast CPUs - all common vehicle tasks should be listed here, along with how often they should be called (in hz) @@ -123,7 +133,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_RunCam, &vehicle.runcam, update, 50, 50), #endif #if HAL_GYROFFT_ENABLED - SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, sample_gyros, LOOP_RATE, 50), + SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update, 400, 50), SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50), #endif SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20), diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index a84c6865c8..ec56e3a9dd 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -190,7 +190,7 @@ protected: // main loop scheduler AP_Scheduler scheduler{FUNCTOR_BIND_MEMBER(&AP_Vehicle::fast_loop, void)}; - virtual void fast_loop() { } + virtual void fast_loop(); // IMU variables // Integration time; time last loop took to run