AP_InertialSensor: user a timer to drive data collection on PX4

this reduces the chance of missing a sample if the main sketch is a
bit slow

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -14,6 +14,15 @@ const extern AP_HAL::HAL& hal;
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 
+Vector3f AP_InertialSensor_PX4::_accel_sum;
+uint32_t AP_InertialSensor_PX4::_accel_sum_count;
+Vector3f AP_InertialSensor_PX4::_gyro_sum;
+uint32_t AP_InertialSensor_PX4::_gyro_sum_count;
+volatile bool AP_InertialSensor_PX4::_in_accumulate;
+uint64_t AP_InertialSensor_PX4::_last_timestamp;
+int AP_InertialSensor_PX4::_accel_fd;
+int AP_InertialSensor_PX4::_gyro_fd;
+
 uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) 
 {
     switch (sample_rate) {
@@ -54,6 +63,9 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
     ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 10);
     ioctl(_gyro_fd,  SENSORIOCSQUEUEDEPTH, 10);
 
+    // register a 1kHz timer to read from PX4 sensor drivers
+    hal.scheduler->register_timer_process(_ins_timer);
+
     return AP_PRODUCT_ID_PX4;
 }
 
@@ -64,32 +76,34 @@ bool AP_InertialSensor_PX4::update(void)
     while (num_samples_available() == 0) {
         hal.scheduler->delay(1);
     }
-    uint32_t now = hal.scheduler->micros();
+    Vector3f accel_scale = _accel_scale.get();
 
-    // the current mpu6000 PX4 driver does not buffer samples, so
-    // using the sample count times 5ms would produce a bad delta time
-    // if we missed one. For now we need to use the clock to get the
-    // delta time
-    _delta_time = (now - _last_update_usec) * 1.0e-6f;
-    _last_update_usec = now;
+    hal.scheduler->suspend_timer_procs();
 
-    Vector3f accel_scale = _accel_scale.get();
+    // base the time on the number of gyro samples, as that is what is
+    // multiplied by time to integrate in DCM
+    _delta_time = _gyro_sum_count / 200.0;
+    _last_update_usec = (uint32_t)_last_timestamp;
+
+    _accel = _accel_sum / _accel_sum_count;
+    _accel_sum.zero();
+    _accel_sum_count = 0;
 
-    _accel   = _accel_sum / _accel_sum_count;
+    _gyro = _gyro_sum / _gyro_sum_count;
+    _gyro_sum.zero();
+    _gyro_sum_count = 0;
+
+    hal.scheduler->resume_timer_procs();
+
+    // add offsets and rotation
     _accel.rotate(_board_orientation);
     _accel.x *= accel_scale.x;
     _accel.y *= accel_scale.y;
     _accel.z *= accel_scale.z;
     _accel   -= _accel_offset;
 
-    _gyro    = _gyro_sum / _gyro_sum_count;
     _gyro.rotate(_board_orientation);
-    _gyro   -= _gyro_offset;
-
-    _accel_sum.zero();
-    _accel_sum_count = 0;
-    _gyro_sum.zero();
-    _gyro_sum_count = 0;
+    _gyro -= _gyro_offset;
 
     return true;
 }
@@ -121,11 +135,16 @@ float AP_InertialSensor_PX4::get_gyro_drift_rate(void)
     return ToRad(0.5/60);
 }
 
-uint16_t AP_InertialSensor_PX4::num_samples_available(void)
+void AP_InertialSensor_PX4::_accumulate(void)
 {
     struct accel_report	accel_report;
     struct gyro_report	gyro_report;
-    
+
+    if (_in_accumulate) {
+        return;
+    }
+    _in_accumulate = true;
+
     if (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report)) {
         _accel_sum += Vector3f(accel_report.x, accel_report.y, accel_report.z);
         _accel_sum_count++;
@@ -134,8 +153,19 @@ uint16_t AP_InertialSensor_PX4::num_samples_available(void)
     if (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report)) {
         _gyro_sum += Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
         _gyro_sum_count++;
+        _last_timestamp = gyro_report.timestamp;
 	}
+    _in_accumulate = false;
+}
+
+void AP_InertialSensor_PX4::_ins_timer(uint32_t now)
+{
+    _accumulate();
+}
 
+uint16_t AP_InertialSensor_PX4::num_samples_available(void)
+{
+    _accumulate();
     return min(_accel_sum_count, _gyro_sum_count) / _sample_divider;
 }
 

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -30,17 +30,21 @@ public:
 
 private:
     uint16_t        _init_sensor( Sample_rate sample_rate );
+    static		    void _ins_timer(uint32_t now);
+    static          void _accumulate(void);
     uint32_t        _last_update_usec;
     float           _delta_time;
-    Vector3f		_accel_sum;
-    uint32_t        _accel_sum_count;
-    Vector3f		_gyro_sum;
-    uint32_t        _gyro_sum_count;
-    uint8_t         _sample_divider;
+    static Vector3f	_accel_sum;
+    static uint32_t _accel_sum_count;
+    static Vector3f	_gyro_sum;
+    static uint32_t _gyro_sum_count;
+    static volatile bool _in_accumulate;
+    static uint64_t _last_timestamp;
+    uint8_t  _sample_divider;
 
     // accelerometer and gyro driver handles
-    int _accel_fd;
-    int _gyro_fd;
+    static int _accel_fd;
+    static int _gyro_fd;
 };
 #endif
 #endif // __AP_INERTIAL_SENSOR_PX4_H__