From a84af856896980e90ecee08ca541bc005c97cf1a Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 8 Jan 2019 07:15:02 +1100
Subject: [PATCH] AP_InertialSensor: support fast sampling in SITL

this allows testing of fast sample logs for FFT
---
 .../AP_InertialSensor_SITL.cpp                 | 18 +++++++++++++++---
 1 file changed, 15 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
index dc19ef42dd..f187723d71 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
@@ -42,6 +42,12 @@ bool AP_InertialSensor_SITL::init_sensor(void)
                                               AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 1, DEVTYPE_SITL));
         accel_instance[i] = _imu.register_accel(accel_sample_hz[i],
                                               AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SITL, i, 2, DEVTYPE_SITL));
+        if (enable_fast_sampling(accel_instance[i])) {
+            _set_accel_raw_sample_rate(accel_instance[i], accel_sample_hz[i]*4);
+        }
+        if (enable_fast_sampling(gyro_instance[i])) {
+            _set_gyro_raw_sample_rate(gyro_instance[i], gyro_sample_hz[i]*8);
+        }
     }
 
     hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_InertialSensor_SITL::timer_update, void));
@@ -109,8 +115,11 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
     Vector3f accel = Vector3f(xAccel, yAccel, zAccel);
 
     _rotate_and_correct_accel(accel_instance[instance], accel);
-    
-    _notify_new_accel_raw_sample(accel_instance[instance], accel, AP_HAL::micros64());
+
+    uint8_t nsamples = enable_fast_sampling(accel_instance[instance])?4:1;
+    for (uint8_t i=0; i<nsamples; i++) {
+        _notify_new_accel_raw_sample(accel_instance[instance], accel);
+    }
 }
 
 /*
@@ -152,7 +161,10 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
 
     _rotate_and_correct_gyro(gyro_instance[instance], gyro);
     
-    _notify_new_gyro_raw_sample(gyro_instance[instance], gyro, AP_HAL::micros64());
+    uint8_t nsamples = enable_fast_sampling(gyro_instance[instance])?8:1;
+    for (uint8_t i=0; i<nsamples; i++) {
+        _notify_new_gyro_raw_sample(gyro_instance[instance], gyro);
+    }
 }
 
 void AP_InertialSensor_SITL::timer_update(void)