From aafcae7e6f1bf3161b630430edf073d63725783a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 May 2016 17:40:40 +0200 Subject: [PATCH] MPU9250: integrate using the FIFO sample interval Instead of calculating time offsets between samples, it is easier just to assume a constant sampling time. Then all samples can be integrated, and published every forth time the FIFO buffer has been emptied. The sampling in the sensor happens at 8kHz, the driver empties the buffer at 1kHz, and publishes (and resets the integration) at 250 Hz. --- src/lib/DriverFramework | 2 +- .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 62 ++++++++----------- 2 files changed, 27 insertions(+), 37 deletions(-) diff --git a/src/lib/DriverFramework b/src/lib/DriverFramework index 451921ca91..583b606d75 160000 --- a/src/lib/DriverFramework +++ b/src/lib/DriverFramework @@ -1 +1 @@ -Subproject commit 451921ca91c4fc7b28d9d0c453539aecb580e839 +Subproject commit 583b606d75aa096f09a6723c4c7c4adfc3f781c6 diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index 0660f8e340..ae8c9b65ca 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -140,7 +140,6 @@ private: Integrator _gyro_int; unsigned _publish_count; - hrt_abstime _first_sample_time; perf_counter_t _read_counter; perf_counter_t _error_counter; @@ -168,7 +167,6 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : _gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true), /*_rotation(rotation)*/ _publish_count(0), - _first_sample_time(0), _read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")), _error_counter(perf_alloc(PC_COUNT, "mpu9250_errors")), _fifo_overflow_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_overflows")), @@ -449,14 +447,6 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report accel_report = {}; gyro_report gyro_report = {}; - // Only take a timestamp once for a series of consecutive samples. - // 0 is the reset value. - if (_first_sample_time == 0) { - _first_sample_time = hrt_absolute_time(); - } - - accel_report.timestamp = gyro_report.timestamp = _first_sample_time + data.time_offset_us; - accel_report.x = (data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale; accel_report.y = (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale; accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale; @@ -466,10 +456,10 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) accel_report.z); math::Vector<3> accel_val_integrated_unused; - _accel_int.put(accel_report.timestamp, - accel_val, - accel_val_integrated_unused, - accel_report.integral_dt); + _accel_int.put_with_interval(data.fifo_sample_interval_us, + accel_val, + accel_val_integrated_unused, + accel_report.integral_dt); gyro_report.x = (data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale; gyro_report.y = (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; @@ -480,11 +470,28 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) gyro_report.z); math::Vector<3> gyro_val_integrated_unused; - _gyro_int.put(gyro_report.timestamp, - gyro_val, - gyro_val_integrated_unused, - gyro_report.integral_dt); + _gyro_int.put_with_interval(data.fifo_sample_interval_us, + gyro_val, + gyro_val_integrated_unused, + gyro_report.integral_dt); + // If we are not receiving the last sample from the FIFO buffer yet, let's stop here + // and wait for more packets. + if (!data.is_last_fifo_sample) { + return 0; + } + + // The driver empties the FIFO buffer at 1kHz, however we only need to publish at 250Hz. + // Therefore, only publish every forth time. + ++_publish_count; + + if (_publish_count < 4) { + return 0; + } + + _publish_count = 0; + + // Update all the counters. perf_set_count(_read_counter, data.read_counter); perf_set_count(_error_counter, data.error_counter); perf_set_count(_fifo_overflow_counter, data.fifo_overflow_counter); @@ -492,26 +499,9 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) perf_set_count(_gyro_range_hit_counter, data.gyro_range_hit_counter); perf_set_count(_accel_range_hit_counter, data.accel_range_hit_counter); - /* If the time offset is 0, we are receiving the latest sample and can publish, - * at least every 4th time because the driver is running at 1kHz but we should - * only publish at 250 Hz. */ - if (data.time_offset_us != 0) { - return 0; - } - - /* reset the first sample time to the reset value */ - _first_sample_time = 0; - - // Bail out if it's not the 4th time yet. - ++_publish_count; - - if (_publish_count < 4) { - return 0; - } - perf_begin(_publish_perf); - _publish_count = 0; + accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); // TODO: get these right gyro_report.scaling = -1.0f;