Added mag support to the DriverFramework mpu9250 driver. Shortened parameter names for legacy drivers. Added temporary ifdef's in the calibration code for Snapdragon Flight builds.

Signed-off-by: jwilson <jwilson@qti.qualcomm.com>

src/modules/commander/accelerometer_calibration.cpp

@@ -175,7 +175,7 @@ typedef struct  {
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 	int fd;
 #endif
 
@@ -195,7 +195,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = px4_open(str, 0);
@@ -329,7 +329,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 			return ERROR;
 		}
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
 		fd = px4_open(str, 0);
 
@@ -419,7 +419,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
 			break;
 		}
 
-#ifdef __PX4_QURT
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
 		struct accel_report	accel_report;
 		orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);

src/modules/commander/gyro_calibration.cpp

@@ -186,7 +186,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		int fd = px4_open(str, 0);
 		if (fd >= 0) {
@@ -239,9 +239,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 	for (unsigned s = 0; s < gyro_count; s++) {
 
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
-#ifdef __PX4_QURT
+#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
 		// For QURT respectively the driver framework, we need to get the device ID by copying one report.
-		struct gyro_report	gyro_report;
+		struct gyro_report gyro_report;
 		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
 		worker_data.device_id[s] = gyro_report.device_id;
 #endif
@@ -337,7 +337,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
 				(void)sprintf(str, "CAL_GYRO%u_ID", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
 
-#ifndef __PX4_QURT
+#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 				int fd = px4_open(str, 0);

src/modules/sensors/sensors.cpp

@@ -46,9 +46,6 @@
  * @author Anton Babushkin <anton@px4.io>
  */
 
-// TODO-JYW: TESTING-TESTING
-#define DEBUG_BUILD 1
-
 #include <board_config.h>
 
 #include <px4_config.h>

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

@@ -58,6 +58,7 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
 #include <drivers/device/integrator.h>
 
 #include <uORB/topics/parameter_update.h>
@@ -77,7 +78,7 @@ using namespace DriverFramework;
 class DfMpu9250Wrapper : public MPU9250
 {
 public:
-	DfMpu9250Wrapper(/*enum Rotation rotation*/);
+	DfMpu9250Wrapper(bool mag_enabled);
 	~DfMpu9250Wrapper();
 
 
@@ -105,11 +106,13 @@ private:
 
 	void _update_accel_calibration();
 	void _update_gyro_calibration();
+	void _update_mag_calibration();
 
 	//enum Rotation		_rotation;
 
 	orb_advert_t		    _accel_topic;
 	orb_advert_t		    _gyro_topic;
+	orb_advert_t		    _mag_topic;
 
 	orb_advert_t		    _mavlink_log_pub;
 
@@ -133,11 +136,22 @@ private:
 		float z_scale;
 	} _gyro_calibration;
 
+	struct mag_calibration_s {
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
+
 	int			    _accel_orb_class_instance;
 	int			    _gyro_orb_class_instance;
+	int			    _mag_orb_class_instance;
 
 	Integrator		    _accel_int;
 	Integrator		    _gyro_int;
+	Integrator		    _mag_int;
 
 	unsigned		    _publish_count;
 
@@ -147,24 +161,31 @@ private:
 	perf_counter_t		    _fifo_corruption_counter;
 	perf_counter_t		    _gyro_range_hit_counter;
 	perf_counter_t		    _accel_range_hit_counter;
+	perf_counter_t		    _mag_range_hit_counter;
 	perf_counter_t		    _publish_perf;
 
 	hrt_abstime		    _last_accel_range_hit_time;
 	uint64_t		    _last_accel_range_hit_count;
+
+	bool _mag_enabled;
 };
 
-DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
-	MPU9250(IMU_DEVICE_PATH),
+DfMpu9250Wrapper::DfMpu9250Wrapper(bool mag_enabled) :
+	MPU9250(IMU_DEVICE_PATH, mag_enabled),
 	_accel_topic(nullptr),
 	_gyro_topic(nullptr),
+	_mag_topic(nullptr),
 	_mavlink_log_pub(nullptr),
 	_param_update_sub(-1),
 	_accel_calibration{},
 	_gyro_calibration{},
+	_mag_calibration{},
 	_accel_orb_class_instance(-1),
 	_gyro_orb_class_instance(-1),
+	_mag_orb_class_instance(-1),
 	_accel_int(MPU9250_NEVER_AUTOPUBLISH_US, false),
 	_gyro_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
+	_mag_int(MPU9250_NEVER_AUTOPUBLISH_US, true),
 	/*_rotation(rotation)*/
 	_publish_count(0),
 	_read_counter(perf_alloc(PC_COUNT, "mpu9250_reads")),
@@ -173,9 +194,11 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	_fifo_corruption_counter(perf_alloc(PC_COUNT, "mpu9250_fifo_corruptions")),
 	_gyro_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_gyro_range_hits")),
 	_accel_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_accel_range_hits")),
+	_mag_range_hit_counter(perf_alloc(PC_COUNT, "mpu9250_mag_range_hits")),
 	_publish_perf(perf_alloc(PC_ELAPSED, "mpu9250_publish")),
 	_last_accel_range_hit_time(0),
-	_last_accel_range_hit_count(0)
+	_last_accel_range_hit_count(0),
+	_mag_enabled(mag_enabled)
 {
 	// Set sane default calibration values
 	_accel_calibration.x_scale = 1.0f;
@@ -191,6 +214,15 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	_gyro_calibration.x_offset = 0.0f;
 	_gyro_calibration.y_offset = 0.0f;
 	_gyro_calibration.z_offset = 0.0f;
+
+	if (_mag_enabled == true) {
+		_mag_calibration.x_scale = 1.0f;
+		_mag_calibration.y_scale = 1.0f;
+		_mag_calibration.z_scale = 1.0f;
+		_mag_calibration.x_offset = 0.0f;
+		_mag_calibration.y_offset = 0.0f;
+		_mag_calibration.z_offset = 0.0f;
+	}
 }
 
 DfMpu9250Wrapper::~DfMpu9250Wrapper()
@@ -201,6 +233,9 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper()
 	perf_free(_fifo_corruption_counter);
 	perf_free(_gyro_range_hit_counter);
 	perf_free(_accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_free(_mag_range_hit_counter);
+	}
 	perf_free(_publish_perf);
 }
 
@@ -226,6 +261,18 @@ int DfMpu9250Wrapper::start()
 		return -1;
 	}
 
+	if (_mag_enabled == true) {
+		// TODO: don't publish garbage here
+		mag_report mag_report = {};
+		_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
+						  &_mag_orb_class_instance, ORB_PRIO_DEFAULT);
+
+		if (_mag_topic == nullptr) {
+			PX4_ERR("sensor_mag advert fail");
+			return -1;
+		}
+	}
+
 	/* Subscribe to param update topic. */
 	if (_param_update_sub < 0) {
 		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -251,6 +298,7 @@ int DfMpu9250Wrapper::start()
 	/* Force getting the calibration values. */
 	_update_accel_calibration();
 	_update_gyro_calibration();
+	_update_mag_calibration();
 
 	return 0;
 }
@@ -276,6 +324,9 @@ void DfMpu9250Wrapper::info()
 	perf_print_counter(_fifo_corruption_counter);
 	perf_print_counter(_gyro_range_hit_counter);
 	perf_print_counter(_accel_range_hit_counter);
+	if (_mag_enabled == true) {
+		perf_print_counter(_mag_range_hit_counter);
+	}
 	perf_print_counter(_publish_perf);
 }
 
@@ -430,6 +481,83 @@ void DfMpu9250Wrapper::_update_accel_calibration()
 	_accel_calibration.z_offset = 0.0f;
 }
 
+void DfMpu9250Wrapper::_update_mag_calibration()
+{
+	if (_mag_enabled == false) {
+		return;
+	}
+
+	// TODO: replace magic number
+	for (unsigned i = 0; i < 3; ++i) {
+
+		// TODO: remove printfs and add error counter
+
+		char str[30];
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
+		int32_t device_id;
+		int res = param_get(param_find(str), &device_id);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+			continue;
+		}
+
+		if ((uint32_t)device_id != m_id.dev_id) {
+			continue;
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
+		res = param_get(param_find(str), &_mag_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_XOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_YOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", i);
+		res = param_get(param_find(str), &_mag_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+	}
+
+	// Set sane default calibration values
+	_mag_calibration.x_scale = 1.0f;
+	_mag_calibration.y_scale = 1.0f;
+	_mag_calibration.z_scale = 1.0f;
+	_mag_calibration.x_offset = 0.0f;
+	_mag_calibration.y_offset = 0.0f;
+	_mag_calibration.z_offset = 0.0f;
+}
+
 int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 {
 	/* Check if calibration values are still up-to-date. */
@@ -442,6 +570,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 
 		_update_accel_calibration();
 		_update_gyro_calibration();
+		_update_mag_calibration();
 	}
 
 	math::Vector<3> vec_integrated_unused;
@@ -491,13 +620,20 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	perf_set_count(_fifo_corruption_counter, data.fifo_overflow_counter);
 	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 (_mag_enabled == true) {
+		perf_set_count(_mag_range_hit_counter, data.mag_range_hit_counter);
+	}
 
 	perf_begin(_publish_perf);
 
 	accel_report accel_report = {};
 	gyro_report gyro_report = {};
+	mag_report mag_report = {};
 
 	accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
+	if (_mag_enabled == true) {
+		mag_report.timestamp = accel_report.timestamp;
+	}
 
 	// TODO: get these right
 	gyro_report.scaling = -1.0f;
@@ -508,6 +644,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.range_m_s2 = -1.0f;
 	accel_report.device_id = m_id.dev_id;
 
+	if (_mag_enabled == true) {
+		mag_report.scaling = -1.0f;
+		mag_report.range_ga = -1.0f;
+		mag_report.device_id = m_id.dev_id;
+	}
+
 	// TODO: remove these (or get the values)
 	gyro_report.x_raw = NAN;
 	gyro_report.y_raw = NAN;
@@ -517,6 +659,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y_raw = NAN;
 	accel_report.z_raw = NAN;
 
+	if (_mag_enabled == true) {
+		mag_report.x_raw = NAN;
+		mag_report.y_raw = NAN;
+		mag_report.z_raw = NAN;
+	}
+
 	math::Vector<3> gyro_val_filt;
 	math::Vector<3> accel_val_filt;
 
@@ -533,6 +681,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.y = accel_val_filt(1);
 	accel_report.z = accel_val_filt(2);
 
+	if (_mag_enabled == true) {
+		mag_report.x = (data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale;
+		mag_report.y = (data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale;
+		mag_report.z = (data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale;
+	}
+
 	gyro_report.x_integral = gyro_val_integ(0);
 	gyro_report.y_integral = gyro_val_integ(1);
 	gyro_report.z_integral = gyro_val_integ(2);
@@ -552,6 +706,10 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 			orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
 		}
 
+		if ((_mag_topic != nullptr) && (_mag_enabled == true)) {
+			orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
+		}
+
 		/* Notify anyone waiting for data. */
 		DevMgr::updateNotify(*this);
 
@@ -588,9 +746,9 @@ int stop();
 int info();
 void usage();
 
-int start(/*enum Rotation rotation*/)
+int start(bool mag_enabled)
 {
-	g_dev = new DfMpu9250Wrapper(/*rotation*/);
+	g_dev = new DfMpu9250Wrapper(mag_enabled);
 
 	if (g_dev == nullptr) {
 		PX4_ERR("failed instantiating DfMpu9250Wrapper object");
@@ -695,9 +853,12 @@ df_mpu9250_wrapper_main(int argc, char *argv[])
 
 	const char *verb = argv[myoptind];
 
+	if (!strcmp(verb, "start_with_mag")) {
+		ret = df_mpu9250_wrapper::start(true);
+	}
 
-	if (!strcmp(verb, "start")) {
-		ret = df_mpu9250_wrapper::start(/*rotation*/);
+	else if (!strcmp(verb, "start")) {
+		ret = df_mpu9250_wrapper::start(false);
 	}
 
 	else if (!strcmp(verb, "stop")) {