df_imu: added gyro publishing, cleanup

src/platforms/posix/drivers/df_imu/df_imu.cpp

@@ -33,7 +33,7 @@
 
 /**
  * @file df_imu.cpp
- * Lightweight driver to access IMU driver of DriverFramework.
+ * Lightweight driver to access an IMU driver of the DriverFramework.
  *
  * @author Julian Oes <julian@oes.ch>
  */
@@ -55,17 +55,14 @@
 #include <systemlib/err.h>
 
 #include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
 
 #include <board_config.h>
 //#include <mathlib/math/filter/LowPassFilter2p.hpp>
 //#include <lib/conversion/rotation.h>
-//
+
 #include <mpu9250/MPU9250.hpp>
 #include <DevMgr.hpp>
-#include <VirtDevObj.hpp>
-
-
-#define ACCELSIM_DEVICE_PATH_ACCEL	"/dev/df_accel"
 
 
 extern "C" { __EXPORT int df_imu_main(int argc, char *argv[]); }
@@ -79,7 +76,6 @@ public:
 	DfImu(/*enum Rotation rotation*/);
 	~DfImu();
 
-	//enum Rotation		_rotation;
 
 	/**
 	 * Start automatic measurement.
@@ -96,44 +92,60 @@ public:
 	int		stop();
 
 private:
-	virtual int _publish_callback(struct imu_sensor_data &data);
+	int _publish(struct imu_sensor_data &data);
+
+	//enum Rotation		_rotation;
 
 	orb_advert_t		_accel_topic;
+	orb_advert_t		_gyro_topic;
+
 	int			_accel_orb_class_instance;
+	int			_gyro_orb_class_instance;
 
 	perf_counter_t		_accel_sample_perf;
+	perf_counter_t		_gyro_sample_perf;
 
 };
 
 DfImu::DfImu(/*enum Rotation rotation*/) :
 	MPU9250(IMU_DEVICE_PATH),
 	_accel_topic(nullptr),
+	_gyro_topic(nullptr),
 	_accel_orb_class_instance(-1),
-	_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read"))
+	_gyro_orb_class_instance(-1),
+	_accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")),
+	_gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read"))
 	/*_rotation(rotation)*/
 {
-	m_id.dev_id_s.bus = 1;
-	m_id.dev_id_s.devtype = DRV_ACC_DEVTYPE_MPU9250;
 }
 
 DfImu::~DfImu()
 {
+	perf_free(_accel_sample_perf);
+	perf_free(_gyro_sample_perf);
 }
 
 int DfImu::start()
 {
-	struct accel_report arp = {};
+	// TODO: don't publish garbage here
-
+	accel_report accel_report = {};
-	/* measurement will have generated a report, publish */
+	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report,
-	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
 					   &_accel_orb_class_instance, ORB_PRIO_DEFAULT);
-
 	if (_accel_topic == nullptr) {
-		PX4_WARN("ADVERT ERR");
+		PX4_ERR("sensor_accel advert fail");
 		return -1;
 	}
 
-	/* init device and start sensor */
+	// TODO: don't publish garbage here
+	gyro_report gyro_report = {};
+	_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report,
+					  &_gyro_orb_class_instance, ORB_PRIO_DEFAULT);
+	if (_gyro_topic == nullptr) {
+		PX4_ERR("sensor_gyro advert fail");
+		return -1;
+	}
+
+	/* Init device and start sensor. */
 	int ret = init();
 
 	if (ret != 0) {
@@ -153,21 +165,29 @@ int DfImu::start()
 
 int DfImu::stop()
 {
-	/* stop sensor */
+	/* Stop sensor. */
+	int ret = MPU9250::stop();
+
+	if (ret != 0) {
+		PX4_ERR("MPU9250 stop fail: %d", ret);
+		return ret;
+	}
+
 	return 0;
 }
 
-int DfImu::_publish_callback(struct imu_sensor_data &data)
+int DfImu::_publish(struct imu_sensor_data &data)
 {
-	accel_report accel_report = {};
+	/* Publish accel first. */
+	perf_begin(_accel_sample_perf);
 
+	accel_report accel_report = {};
 	accel_report.timestamp = data.last_read_time_usec;
 
 	// 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_report.y = data.accel_m_s2_y;
 	accel_report.z = data.accel_m_s2_z;
@@ -176,15 +196,43 @@ int DfImu::_publish_callback(struct imu_sensor_data &data)
 	accel_report.scaling = -1.0f;
 	accel_report.range_m_s2 = -1.0f;
 
+	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
-		/* publish it */
 
 		if (_accel_topic != nullptr) {
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 	}
+	perf_end(_accel_sample_perf);
 
-	/* notify anyone waiting for data */
+	/* Then publish gyro. */
+	perf_begin(_gyro_sample_perf);
+
+	gyro_report gyro_report = {};
+	gyro_report.timestamp = data.last_read_time_usec;
+
+	// 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_report.y = data.gyro_rad_s_y;
+	gyro_report.z = data.gyro_rad_s_z;
+
+	// TODO: get these right
+	gyro_report.scaling = -1.0f;
+	gyro_report.range_rad_s = -1.0f;
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_gyro_topic != nullptr) {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
+		}
+	}
+	perf_end(_gyro_sample_perf);
+
+	/* Notify anyone waiting for data. */
 	DevMgr::updateNotify(*this);
 
 	return 0;
@@ -274,7 +322,8 @@ usage()
 	//PX4_WARN("    -R rotation");
 }
 
-} // namespace
+} // namespace df_imu
+
 
 int
 df_imu_main(int argc, char *argv[])