AP_InertialSensor: switch PX4 driver to fixed time per sample model

this makes the driver much simpler, and does away with the need for an
accumulate function

libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp

@@ -13,37 +13,28 @@ const extern AP_HAL::HAL& hal;
 
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
-
+#include <drivers/drv_hrt.h>
-Vector3f AP_InertialSensor_PX4::_accel_in;
-uint32_t AP_InertialSensor_PX4::_accel_count;
-Vector3f AP_InertialSensor_PX4::_gyro_in;
-uint32_t AP_InertialSensor_PX4::_gyro_count;
-volatile bool AP_InertialSensor_PX4::_in_accumulate;
-uint64_t AP_InertialSensor_PX4::_last_accel_timestamp;
-uint64_t AP_InertialSensor_PX4::_last_gyro_timestamp;
-int AP_InertialSensor_PX4::_accel_fd;
-int AP_InertialSensor_PX4::_gyro_fd;
 
 uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate ) 
 {
-    uint32_t msec_per_sample;
-
     switch (sample_rate) {
     case RATE_50HZ:
-        msec_per_sample = 20;
         _default_filter_hz = 15;
+        _sample_time_usec = 20000;
         break;
     case RATE_100HZ:
-        msec_per_sample = 10;
         _default_filter_hz = 30;
+        _sample_time_usec = 10000;
         break;
     case RATE_200HZ:
     default:
-        msec_per_sample = 5;
         _default_filter_hz = 30;
+        _sample_time_usec = 5000;
         break;
     }
 
+    _delta_time = _sample_time_usec * 1.0e-6f;
+
 	// init accelerometers
 	_accel_fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 	if (_accel_fd < 0) {
@@ -69,15 +60,10 @@ uint16_t AP_InertialSensor_PX4::_init_sensor( Sample_rate sample_rate )
     ioctl(_gyro_fd,  GYROIOCSSAMPLERATE,  driver_rate);
     ioctl(_gyro_fd,  SENSORIOCSPOLLRATE,  driver_rate);
 
-    // ask for a 20 sample buffer
+    // ask for a 1 sample buffer. We're really only interested in the
-    ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 20);
+    // latest sample as the driver is doing the filtering for us
-    ioctl(_gyro_fd,  SENSORIOCSQUEUEDEPTH, 20);
+    ioctl(_accel_fd, SENSORIOCSQUEUEDEPTH, 1);
-
+    ioctl(_gyro_fd,  SENSORIOCSQUEUEDEPTH, 1);
-    // setup sample divider
-    _sample_divider = (driver_rate * msec_per_sample) / 1000;
-
-    // register a 1kHz timer to read from PX4 sensor drivers
-    hal.scheduler->register_timer_process(_ins_timer);
 
     _set_filter_frequency(_mpu6000_filter);
 
@@ -100,25 +86,13 @@ void AP_InertialSensor_PX4::_set_filter_frequency(uint8_t filter_hz)
 
 bool AP_InertialSensor_PX4::update(void) 
 {
-    while (num_samples_available() == 0) {
-        hal.scheduler->delay(1);
-    }
     Vector3f accel_scale = _accel_scale.get();
 
-    hal.scheduler->suspend_timer_procs();
+    // get the latest sample from the sensor drivers
-
+    _get_sample();
-    // base the time on the gyro timestamp, as that is what is
-    // multiplied by time to integrate in DCM
-    _delta_time = (_last_gyro_timestamp - _last_update_usec) * 1.0e-6f;
-    _last_update_usec = _last_gyro_timestamp;
 
     _accel = _accel_in;
-    _accel_count = 0;
+    _gyro  = _gyro_in;
-
-    _gyro = _gyro_in;
-    _gyro_count = 0;
-
-    hal.scheduler->resume_timer_procs();
 
     // add offsets and rotation
     _accel.rotate(_board_orientation);
@@ -135,61 +109,48 @@ bool AP_InertialSensor_PX4::update(void)
         _last_filter_hz = _mpu6000_filter;
     }
 
+    _num_samples_available = 0;
+
     return true;
 }
 
-float AP_InertialSensor_PX4::get_delta_time(void) 
+float AP_InertialSensor_PX4::get_delta_time(void)
 {
     return _delta_time;
 }
 
-uint32_t AP_InertialSensor_PX4::get_last_sample_time_micros(void) 
-{
-    return _last_update_usec;
-}
-
 float AP_InertialSensor_PX4::get_gyro_drift_rate(void) 
 {
     // 0.5 degrees/second/minute
     return ToRad(0.5/60);
 }
 
-void AP_InertialSensor_PX4::_accumulate(void)
+void AP_InertialSensor_PX4::_get_sample(void)
 {
     struct accel_report	accel_report;
     struct gyro_report	gyro_report;
 
-    if (_in_accumulate) {
-        return;
-    }
-    _in_accumulate = true;
-
     while (::read(_accel_fd, &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
         accel_report.timestamp != _last_accel_timestamp) {        
         _accel_in = Vector3f(accel_report.x, accel_report.y, accel_report.z);
-        _accel_count++;
         _last_accel_timestamp = accel_report.timestamp;
 	}
 
     while (::read(_gyro_fd, &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
         gyro_report.timestamp != _last_gyro_timestamp) {        
         _gyro_in = Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
-        _gyro_count++;
         _last_gyro_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();
+    uint64_t tnow = hrt_absolute_time();
-    return min(_accel_count, _gyro_count) / _sample_divider;    
+    if (tnow - _last_sample_timestamp > _sample_time_usec) {
+        _num_samples_available++;
+        _last_sample_timestamp = tnow;
+    }
+    return _num_samples_available;
 }
 
 #endif // CONFIG_HAL_BOARD

libraries/AP_InertialSensor/AP_InertialSensor_PX4.h

@@ -22,24 +22,21 @@ public:
     /* Concrete implementation of AP_InertialSensor functions: */
     bool            update();
     float        	get_delta_time();
-    uint32_t        get_last_sample_time_micros();
     float           get_gyro_drift_rate();
     uint16_t        num_samples_available();
 
 private:
-    uint16_t        _init_sensor( Sample_rate sample_rate );
+    uint16_t _init_sensor( Sample_rate sample_rate );
-    static		    void _ins_timer(uint32_t now);
+    void     _get_sample(void);
-    static          void _accumulate(void);
+    uint64_t _last_update_usec;
-    uint64_t        _last_update_usec;
+    float    _delta_time;
-    float           _delta_time;
+    Vector3f _accel_in;
-    static Vector3f	_accel_in;
+    Vector3f _gyro_in;
-    static uint32_t _accel_count;
+    uint64_t _last_accel_timestamp;
-    static Vector3f	_gyro_in;
+    uint64_t _last_gyro_timestamp;
-    static uint32_t _gyro_count;
+    uint64_t _last_sample_timestamp;
-    static volatile bool _in_accumulate;
+    uint16_t _num_samples_available;
-    static uint64_t _last_accel_timestamp;
+    uint32_t _sample_time_usec;
-    static uint64_t _last_gyro_timestamp;
-    uint8_t  _sample_divider;
 
     // support for updating filter at runtime
     uint8_t _last_filter_hz;
@@ -48,8 +45,8 @@ private:
     void _set_filter_frequency(uint8_t filter_hz);
 
     // accelerometer and gyro driver handles
-    static int _accel_fd;
+    int _accel_fd;
-    static int _gyro_fd;
+    int _gyro_fd;
 };
 #endif
 #endif // __AP_INERTIAL_SENSOR_PX4_H__