df_mpu9250_wrapper: use FIFO time offsets

- Make use of the FIFO time offsets provided by the MPU9250 driver.
  It allows to use proper dt for the integration.
- Got rid of the unnecessary perf_counters for now.
- Properly use the changed integrator lib.
- Provide integral_dt for ekf2.

src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp

@@ -51,9 +51,9 @@
 #include <px4_getopt.h>
 #include <errno.h>
 
-#include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
+#include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 #include <drivers/device/integrator.h>
@@ -63,9 +63,8 @@
 #include <mpu9250/MPU9250.hpp>
 #include <DevMgr.hpp>
 
-
-// publish frequency of 250 Hz
-#define MPU9250_PUBLISH_INTERVAL_US 4000
+// We don't want to auto publish, therefore set this to 0.
+#define MPU9250_NEVER_AUTOPUBLISH_US 0
 
 
 extern "C" { __EXPORT int df_mpu9250_wrapper_main(int argc, char *argv[]); }
@@ -131,9 +130,8 @@ private:
 	Integrator		    _accel_int;
 	Integrator		    _gyro_int;
 
-	perf_counter_t		    _accel_sample_perf;
-	perf_counter_t		    _gyro_sample_perf;
-
+	unsigned		    _publish_count;
+	hrt_abstime		    _first_sample_time;
 };
 
 DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
@@ -145,11 +143,11 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	_gyro_calibration{},
 	_accel_orb_class_instance(-1),
 	_gyro_orb_class_instance(-1),
-	_accel_int(MPU9250_PUBLISH_INTERVAL_US, false),
-	_gyro_int(MPU9250_PUBLISH_INTERVAL_US, true),
-	_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")),
-	_gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read"))
+	_accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false),
+	_gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
 	/*_rotation(rotation)*/
+	_publish_count(0),
+	_first_sample_time(0)
 {
 	// Set sane default calibration values
 	_accel_calibration.x_scale = 1.0f;
@@ -169,8 +167,6 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 
 DfMpu9250Wrapper::~DfMpu9250Wrapper()
 {
-	perf_free(_accel_sample_perf);
-	perf_free(_gyro_sample_perf);
 }
 
 int DfMpu9250Wrapper::start()
@@ -390,8 +386,6 @@ void DfMpu9250Wrapper::_update_accel_calibration()
 
 int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 {
-	bool should_notify = false;
-
 	/* Check if calibration values are still up-to-date. */
 	bool updated;
 	orb_check(_param_update_sub, &updated);
@@ -404,16 +398,17 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 		_update_gyro_calibration();
 	}
 
-	/* Publish accel first. */
-	perf_begin(_accel_sample_perf);
-
 	accel_report accel_report = {};
-	accel_report.timestamp = hrt_absolute_time();
+	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;
 
-	// TODO: remove these (or get the values)
-	accel_report.x_raw = NAN;
-	accel_report.y_raw = NAN;
-	accel_report.z_raw = NAN;
 	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;
@@ -421,86 +416,86 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	math::Vector<3> accel_val(accel_report.x,
 				  accel_report.y,
 				  accel_report.z);
-	math::Vector<3> accel_val_integrated;
-
-	const bool should_publish_accel = _accel_int.put(accel_report.timestamp,
-					  accel_val,
-					  accel_val_integrated,
-					  accel_report.integral_dt);
-
-	accel_report.x_integral = accel_val_integrated(0);
-	accel_report.y_integral = accel_val_integrated(1);
-	accel_report.z_integral = accel_val_integrated(2);
-
+	math::Vector<3> accel_val_integrated_unused;
 
-	// TODO: get these right
-	accel_report.scaling = -1.0f;
-	accel_report.range_m_s2 = -1.0f;
+	_accel_int.put(accel_report.timestamp,
+		       accel_val,
+		       accel_val_integrated_unused,
+		       accel_report.integral_dt);
 
-	accel_report.device_id = m_id.dev_id;
+	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;
+	gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
 
-	// TODO: when is this ever blocked?
-	if (!(m_pub_blocked) && should_publish_accel) {
+	math::Vector<3> gyro_val(gyro_report.x,
+				 gyro_report.y,
+				 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);
+
+	/* 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;
+	}
 
-		if (_accel_topic != nullptr) {
-			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
-		}
+	/* reset the first sample time to the reset value */
+	_first_sample_time = 0;
 
-		should_notify = true;
+	// Bail out if it's not the 4th time yet.
+	++_publish_count;
+	if (_publish_count < 4) {
+		return 0;
 	}
 
-	perf_end(_accel_sample_perf);
-
+	_publish_count = 0;
 
-	/* Then publish gyro. */
-	perf_begin(_gyro_sample_perf);
+	// TODO: get these right
+	gyro_report.scaling = -1.0f;
+	gyro_report.range_rad_s = -1.0f;
+	gyro_report.device_id = m_id.dev_id;
 
-	gyro_report gyro_report = {};
-	gyro_report.timestamp = hrt_absolute_time();
+	accel_report.scaling = -1.0f;
+	accel_report.range_m_s2 = -1.0f;
+	accel_report.device_id = m_id.dev_id;
 
 	// TODO: remove these (or get the values)
 	gyro_report.x_raw = NAN;
 	gyro_report.y_raw = NAN;
 	gyro_report.z_raw = NAN;
-	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;
-	gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
 
-	math::Vector<3> gyro_val(gyro_report.x,
-				 gyro_report.y,
-				 gyro_report.z);
-	math::Vector<3> gyro_val_integrated(gyro_report.x,
-					    gyro_report.y,
-					    gyro_report.z);
+	accel_report.x_raw = NAN;
+	accel_report.y_raw = NAN;
+	accel_report.z_raw = NAN;
 
-	const bool should_publish_gyro = _gyro_int.put(gyro_report.timestamp,
-					 gyro_val,
-					 gyro_val_integrated,
-					 gyro_report.integral_dt);
+	// Read and reset.
+	math::Vector<3> gyro_val_integrated = _gyro_int.get(true, gyro_report.integral_dt);
+	math::Vector<3> accel_val_integrated = _accel_int.get(true, accel_report.integral_dt);
 
 	gyro_report.x_integral = gyro_val_integrated(0);
 	gyro_report.y_integral = gyro_val_integrated(1);
 	gyro_report.z_integral = gyro_val_integrated(2);
 
-	// TODO: get these right
-	gyro_report.scaling = -1.0f;
-	gyro_report.range_rad_s = -1.0f;
-
-	gyro_report.device_id = m_id.dev_id;
+	accel_report.x_integral = accel_val_integrated(0);
+	accel_report.y_integral = accel_val_integrated(1);
+	accel_report.z_integral = accel_val_integrated(2);
 
 	// TODO: when is this ever blocked?
-	if (!(m_pub_blocked) && should_publish_gyro) {
+	if (!(m_pub_blocked)) {
 
 		if (_gyro_topic != nullptr) {
 			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
 		}
 
-		should_notify = true;
-	}
-
-	perf_end(_gyro_sample_perf);
+		if (_accel_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
+		}
 
-	if (should_notify) {
 		/* Notify anyone waiting for data. */
 		DevMgr::updateNotify(*this);
 	}