sensors/calibration: use params in DF wrappers

Instead of using a uORB topic with the calibration values published in
sensors and consumed by the DriverFramework driver wrappers, let's just
use the the params directly. This is quite a rough change and needs
definitely some cleanup and refactoring.

msg/accel_calibration.msg

@@ -1,10 +0,0 @@
-uint64 timestamp
-
-int32 device_id
-
-float32 x_offset
-float32 x_scale
-float32 y_offset
-float32 y_scale
-float32 z_offset
-float32 z_scale

msg/gyro_calibration.msg

@@ -1,10 +0,0 @@
-uint64 timestamp
-
-int32 device_id
-
-float32 x_offset
-float32 x_scale
-float32 y_offset
-float32 y_scale
-float32 z_offset
-float32 z_scale

msg/mag_calibration.msg

@@ -1,10 +0,0 @@
-uint64 timestamp
-
-int32 device_id
-
-float32 x_offset
-float32 x_scale
-float32 y_offset
-float32 y_scale
-float32 z_offset
-float32 z_scale

msg/sensor_accel.msg

@@ -15,3 +15,5 @@ int16 x_raw
 int16 y_raw
 int16 z_raw
 int16 temperature_raw
+
+uint32 device_id

msg/sensor_gyro.msg

@@ -15,3 +15,5 @@ int16 x_raw
 int16 y_raw
 int16 z_raw
 int16 temperature_raw
+
+uint32 device_id

msg/sensor_mag.msg

@@ -10,3 +10,5 @@ float32 temperature
 int16 x_raw
 int16 y_raw
 int16 z_raw
+
+uint32 device_id

src/drivers/drv_accel.h

@@ -42,7 +42,6 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
-#include <uORB/topics/accel_calibration.h>
 
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
@@ -55,6 +54,15 @@
 #include <uORB/topics/sensor_accel.h>
 #define accel_report sensor_accel_s
 
+/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
+struct accel_calibration_s {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
 /*
  * ioctl() definitions
  *

src/drivers/drv_gyro.h

@@ -42,7 +42,6 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
-#include <uORB/topics/gyro_calibration.h>
 
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
@@ -55,6 +54,15 @@
 #include <uORB/topics/sensor_gyro.h>
 #define gyro_report sensor_gyro_s
 
+/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct gyro_calibration_s {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
 
 /*
  * ioctl() definitions

src/drivers/drv_mag.h

@@ -40,7 +40,6 @@
 
 #include <stdint.h>
 #include <sys/ioctl.h>
-#include <uORB/topics/mag_calibration.h>
 
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
@@ -53,6 +52,15 @@
 #include <uORB/topics/sensor_mag.h>
 #define mag_report sensor_mag_s
 
+/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */
+struct mag_calibration_s {
+	float	x_offset;
+	float	x_scale;
+	float	y_offset;
+	float	y_scale;
+	float	z_offset;
+	float	z_scale;
+};
 
 /*
  * ioctl() definitions

src/modules/commander/accelerometer_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -157,7 +157,6 @@ static const int ERROR = -1;
 static const char *sensor_name = "accel";
 
 static int32_t device_id[max_accel_sens];
-static int device_prio_max = 0;
 static int32_t device_id_primary = 0;
 
 calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
@@ -193,9 +192,9 @@ int do_accel_calibration(int mavlink_fd)
 
 	char str[30];
 
-#ifndef __PX4_QURT
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
+#ifndef __PX4_QURT
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
 		/* reset all offsets to zero and all scales to one */
 		fd = px4_open(str, 0);
@@ -212,8 +211,39 @@ int do_accel_calibration(int mavlink_fd)
 		if (res != OK) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
 		}
+#else
+		(void)sprintf(str, "CAL_ACC%u_XOFF", s);
+		res = param_set(param_find(str), &accel_scale.x_offset);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_ACC%u_YOFF", s);
+		res = param_set(param_find(str), &accel_scale.y_offset);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_ACC%u_ZOFF", s);
+		res = param_set(param_find(str), &accel_scale.z_offset);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_ACC%u_XSCALE", s);
+		res = param_set(param_find(str), &accel_scale.x_scale);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_ACC%u_YSCALE", s);
+		res = param_set(param_find(str), &accel_scale.y_scale);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_ACC%u_ZSCALE", s);
+		res = param_set(param_find(str), &accel_scale.z_scale);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
 		}
 #endif
+	}
 
 	float accel_offs[max_accel_sens][3];
 	float accel_T[max_accel_sens][3][3];
@@ -267,6 +297,16 @@ int do_accel_calibration(int mavlink_fd)
 
 		failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
 
+
+		PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i,
+				(double)accel_scale.x_offset,
+				(double)accel_scale.y_offset,
+				(double)accel_scale.z_offset);
+		PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", i,
+				(double)accel_scale.x_scale,
+				(double)accel_scale.y_scale,
+				(double)accel_scale.z_scale);
+
 		/* set parameters */
 		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@@ -280,10 +320,8 @@ int do_accel_calibration(int mavlink_fd)
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
 		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
-#ifndef __PX4_QURT
 		(void)sprintf(str, "CAL_ACC%u_ID", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
-#endif
 
 		if (failed) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
@@ -387,10 +425,18 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 		int32_t prio;
 		orb_priority(worker_data.subs[i], &prio);
 
+#ifndef __PX4_QURT
+		int device_prio_max = 0;
 		if (prio > device_prio_max) {
 			device_prio_max = prio;
 			device_id_primary = device_id[i];
 		}
+#else
+		PX4_INFO("found device id: %d", arp.device_id);
+
+		// TODO FIXME: this is hacky but should get the device ID for now
+		device_id[i] = arp.device_id;
+#endif
 	}
 
 	if (result == calibrate_return_ok) {

src/modules/commander/gyro_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -167,12 +167,11 @@ int do_gyro_calibration(int mavlink_fd)
 	gyro_scale_zero.z_offset = 0.0f;
 	gyro_scale_zero.z_scale = 1.0f;
 
-	int device_prio_max = 0;
 	int32_t device_id_primary = 0;
 
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
-#ifndef __PX4_QURT
+
 		// Reset gyro ids to unavailable
 		worker_data.device_id[s] = 0;
 		(void)sprintf(str, "CAL_GYRO%u_ID", s);
@@ -181,14 +180,6 @@ int do_gyro_calibration(int mavlink_fd)
 			mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
 			return ERROR;
 		}
-#else
-		(void)sprintf(str, "CAL_GYRO%u_ID", s);
-		res = param_get(param_find(str), &(worker_data.device_id[s]));
-		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "[cal] Unable to get CAL_GYRO%u_ID", s);
-			return ERROR;
-		}
-#endif
 
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
@@ -205,6 +196,37 @@ int do_gyro_calibration(int mavlink_fd)
 				return ERROR;
 			}
 		}
+#else
+		(void)sprintf(str, "CAL_GYRO%u_XOFF", s);
+		res = param_set(param_find(str), &gyro_scale_zero.x_offset);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_GYRO%u_YOFF", s);
+		res = param_set(param_find(str), &gyro_scale_zero.y_offset);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
+		res = param_set(param_find(str), &gyro_scale_zero.z_offset);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_GYRO%u_XSCALE", s);
+		res = param_set(param_find(str), &gyro_scale_zero.x_scale);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_GYRO%u_YSCALE", s);
+		res = param_set(param_find(str), &gyro_scale_zero.y_scale);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", s);
+		res = param_set(param_find(str), &gyro_scale_zero.z_scale);
+		if (res != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
 #endif
 
 	}
@@ -216,10 +238,20 @@ int do_gyro_calibration(int mavlink_fd)
 		int32_t prio;
 		orb_priority(worker_data.gyro_sensor_sub[s], &prio);
 
+#ifndef __PX4_QURT
+		int device_prio_max = 0;
 		if (prio > device_prio_max) {
 			device_prio_max = prio;
 			device_id_primary = worker_data.device_id[s];
 		}
+#else
+		gyro_report report = {};
+		orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &report);
+		//PX4_INFO("found device id: %d", report.device_id);
+
+		// TODO FIXME: this is hacky but should get the device ID for now
+		worker_data.device_id[s] = report.device_id;
+#endif
 	}
 
 	int cancel_sub  = calibrate_cancel_subscribe();
@@ -296,10 +328,10 @@ int do_gyro_calibration(int mavlink_fd)
 				(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
 
-#ifndef __PX4_QURT
 				(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
 				/* apply new scaling and offsets */
 				(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 				int fd = px4_open(str, 0);

src/modules/commander/mag_calibration.cpp

@@ -102,7 +102,6 @@ int do_mag_calibration(int mavlink_fd)
 {
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
-#ifndef __PX4_QURT
 	struct mag_calibration_s mscale_null;
 	mscale_null.x_offset = 0.0f;
 	mscale_null.x_scale = 1.0f;
@@ -110,7 +109,6 @@ int do_mag_calibration(int mavlink_fd)
 	mscale_null.y_scale = 1.0f;
 	mscale_null.z_offset = 0.0f;
 	mscale_null.z_scale = 1.0f;
-#endif
 
 	int result = OK;
 
@@ -132,11 +130,35 @@ int do_mag_calibration(int mavlink_fd)
 			break;
 		}
 #else
-		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
+		(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
-		result = param_get(param_find(str), &(device_ids[cur_mag]));
+		result = param_set(param_find(str), &mscale_null.x_offset);
 		if (result != OK) {
-			mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
+			PX4_ERR("unable to reset %s", str);
-			break;
+		}
+		(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
+		result = param_set(param_find(str), &mscale_null.y_offset);
+		if (result != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
+		result = param_set(param_find(str), &mscale_null.z_offset);
+		if (result != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
+		result = param_set(param_find(str), &mscale_null.x_scale);
+		if (result != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
+		result = param_set(param_find(str), &mscale_null.y_scale);
+		if (result != OK) {
+			PX4_ERR("unable to reset %s", str);
+		}
+		(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
+		result = param_set(param_find(str), &mscale_null.z_scale);
+		if (result != OK) {
+			PX4_ERR("unable to reset %s", str);
 		}
 #endif
 
@@ -326,12 +348,13 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 		px4_pollfd_struct_t fds[max_mags];
 		size_t fd_count = 0;
 		for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
-			if (worker_data->sub_mag[cur_mag] >= 0) {
+			if (worker_data->sub_mag[cur_mag] >= 0 && device_ids[cur_mag] > 0) {
 				fds[fd_count].fd = worker_data->sub_mag[cur_mag];
 				fds[fd_count].events = POLLIN;
 				fd_count++;
 			}
 		}
+
 		int poll_ret = px4_poll(fds, fd_count, 1000);
 
 		if (poll_ret > 0) {
@@ -446,7 +469,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	// Setup subscriptions to mag sensors
 	if (result == calibrate_return_ok) {
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
-			if (device_ids[cur_mag] != 0) {
 			// Mag in this slot is available
 			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
 			if (worker_data.sub_mag[cur_mag] < 0) {
@@ -459,11 +481,19 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 			int32_t prio;
 			orb_priority(worker_data.sub_mag[cur_mag], &prio);
 
+#ifndef __PX4_QURT
 			if (prio > device_prio_max) {
 				device_prio_max = prio;
 				device_id_primary = device_ids[cur_mag];
 			}
-			}
+#else
+			mag_report report = {};
+			orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report);
+
+			// TODO FIXME: this is hacky but should get the device ID for now
+			device_ids[cur_mag] = report.device_id;
+			PX4_INFO("found device id: %d", report.device_id);
+#endif
 		}
 	}
 
@@ -593,11 +623,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
-				int fd_mag = -1;
 				struct mag_calibration_s mscale;
+#ifndef __PX4_QURT
+				int fd_mag = -1;
 
 				// Set new scale
-
 				(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
 				fd_mag = px4_open(str, 0);
 				if (fd_mag < 0) {
@@ -611,32 +641,44 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 						result = calibrate_return_error;
 					}
 				}
+#endif
 
 				if (result == calibrate_return_ok) {
 					mscale.x_offset = sphere_x[cur_mag];
 					mscale.y_offset = sphere_y[cur_mag];
 					mscale.z_offset = sphere_z[cur_mag];
 
+#ifndef __PX4_QURT
 					if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
 						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
 						result = calibrate_return_error;
 					}
+#endif
 				}
 
+#ifndef __PX4_QURT
 				// Mag device no longer needed
 				if (fd_mag >= 0) {
 					px4_close(fd_mag);
 				}
+#endif
 
 				if (result == calibrate_return_ok) {
 					bool failed = false;
 
+					PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
+							(double)mscale.x_offset,
+							(double)mscale.y_offset,
+							(double)mscale.z_offset);
+					PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", cur_mag,
+							(double)mscale.x_scale,
+							(double)mscale.y_scale,
+							(double)mscale.z_scale);
+
 					/* set parameters */
 
-#ifndef __PX4_QURT
 					(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
-#endif
 					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
 					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);
@@ -644,11 +686,12 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					(void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset)));
 					(void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag);
-					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
+					// FIXME: scaling is not used right now
-					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
+					//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
-					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
+					//(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
-					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
+					//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
-					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
+					//(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
+					//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
 
 					if (failed) {
 						mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);

src/modules/sensors/sensors.cpp

@@ -100,9 +100,6 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_parameter_map.h>
-#include <uORB/topics/gyro_calibration.h>
-#include <uORB/topics/accel_calibration.h>
-#include <uORB/topics/mag_calibration.h>
 
 #include <DevMgr.hpp>
 
@@ -1530,17 +1527,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g
 	}
 
 #else
-	/* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */
+	/* On QURT, the params are read directly in the respective wrappers. */
-
-	static orb_advert_t gyro_calibration_pub = nullptr;
-
-	if (gyro_calibration_pub != nullptr) {
-		orb_publish(ORB_ID(gyro_calibration), gyro_calibration_pub, gcal);
-
-	} else {
-		gyro_calibration_pub = orb_advertise(ORB_ID(gyro_calibration), gcal);
-	}
-
 	return true;
 #endif
 }
@@ -1561,17 +1548,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s
 	}
 
 #else
-	/* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */
+	/* On QURT, the params are read directly in the respective wrappers. */
-
-	static orb_advert_t accel_calibration_pub = nullptr;
-
-	if (accel_calibration_pub != nullptr) {
-		orb_publish(ORB_ID(accel_calibration), accel_calibration_pub, acal);
-
-	} else {
-		accel_calibration_pub = orb_advertise(ORB_ID(accel_calibration), acal);
-	}
-
 	return true;
 #endif
 }
@@ -1592,17 +1569,7 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mca
 	}
 
 #else
-	/* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */
+	/* On QURT, the params are read directly in the respective wrappers. */
-
-	static orb_advert_t mag_calibration_pub = nullptr;
-
-	if (mag_calibration_pub != nullptr) {
-		orb_publish(ORB_ID(mag_calibration), mag_calibration_pub, mcal);
-
-	} else {
-		mag_calibration_pub = orb_advertise(ORB_ID(mag_calibration), mcal);
-	}
-
 	return true;
 #endif
 }

src/modules/uORB/objects_common.cpp

@@ -282,12 +282,3 @@ ORB_DEFINE(ekf2_replay, struct ekf2_replay_s);
 
 #include "topics/qshell_req.h"
 ORB_DEFINE(qshell_req, struct qshell_req_s);
-
-#include "topics/gyro_calibration.h"
-ORB_DEFINE(gyro_calibration, struct gyro_calibration_s);
-
-#include "topics/accel_calibration.h"
-ORB_DEFINE(accel_calibration, struct accel_calibration_s);
-
-#include "topics/mag_calibration.h"
-ORB_DEFINE(mag_calibration, struct mag_calibration_s);

src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp

@@ -56,7 +56,7 @@
 
 #include <drivers/drv_mag.h>
 
-#include <uORB/topics/mag_calibration.h>
+#include <uORB/topics/parameter_update.h>
 
 #include <board_config.h>
 //#include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -101,11 +101,16 @@ private:
 
 	orb_advert_t		_mag_topic;
 
-	int			_mag_calibration_sub;
+	int			_param_update_sub;
 
-	struct mag_calibration_s  _mag_calibration;
+	struct mag_calibration_s {
-
+		float x_offset;
-	bool			_mag_calibration_set;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _mag_calibration;
 
 	int			_mag_orb_class_instance;
 
@@ -116,10 +121,19 @@ private:
 DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
 	HMC5883(MAG_DEVICE_PATH),
 	_mag_topic(nullptr),
+	_param_update_sub(-1),
+	_mag_calibration{},
 	_mag_orb_class_instance(-1),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
 	/*_rotation(rotation)*/
 {
+	// 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;
 }
 
 DfHmc9250Wrapper::~DfHmc9250Wrapper()
@@ -139,9 +153,9 @@ int DfHmc9250Wrapper::start()
 		return -1;
 	}
 
-	/* Subscribe to calibration topic. */
+	/* Subscribe to param update topic. */
-	if (_mag_calibration_sub < 0) {
+	if (_param_update_sub < 0) {
-		_mag_calibration_sub = orb_subscribe(ORB_ID(mag_calibration));
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 	}
 
 	/* Init device and start sensor. */
@@ -159,6 +173,9 @@ int DfHmc9250Wrapper::start()
 		return ret;
 	}
 
+	/* Force getting the calibration values. */
+	_update_mag_calibration();
+
 	return 0;
 }
 
@@ -177,17 +194,65 @@ int DfHmc9250Wrapper::stop()
 
 void DfHmc9250Wrapper::_update_mag_calibration()
 {
-	bool updated;
+	// TODO: replace magic number
-	orb_check(_mag_calibration_sub, &updated);
+	for (unsigned i = 0; i < 3; ++i) {
 
-	if (updated) {
+		// TODO: remove printfs and add error counter
-		mag_calibration_s new_calibration;
-		orb_copy(ORB_ID(mag_calibration), _mag_calibration_sub, &new_calibration);
 
-		/* Only accept calibration for this device. */
+		char str[30];
-		if (m_id.dev_id == new_calibration.device_id) {
+		(void)sprintf(str, "CAL_MAG%u_ID", i);
-			_mag_calibration = new_calibration;
+		int32_t device_id;
-			_mag_calibration_set = true;
+		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);
 		}
 	}
 }
@@ -196,11 +261,14 @@ void DfHmc9250Wrapper::_update_mag_calibration()
 int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 {
 	/* Check if calibration values are still up-to-date. */
-	_update_mag_calibration();
+	bool updated;
+	orb_check(_param_update_sub, &updated);
 
-	if (!_mag_calibration_set) {
+	if (updated) {
-		// TODO: check the return codes of this function
+		parameter_update_s parameter_update;
-		return 0;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
+		_update_mag_calibration();
 	}
 
 	/* Publish mag first. */
@@ -220,14 +288,16 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
 	mag_report.x_raw = NAN;
 	mag_report.y_raw = NAN;
 	mag_report.z_raw = NAN;
-	mag_report.x = data.field_x_ga;
+	mag_report.x = (data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale;
-	mag_report.y = data.field_y_ga;
+	mag_report.y = (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale;
-	mag_report.z = data.field_z_ga;
+	mag_report.z = (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale;
 
 	// TODO: get these right
 	//mag_report.scaling = -1.0f;
 	//mag_report.range_m_s2 = -1.0f;
 
+	mag_report.device_id = m_id.dev_id;
+
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 

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

@@ -57,8 +57,7 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_gyro.h>
 
-#include <uORB/topics/gyro_calibration.h>
+#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/accel_calibration.h>
 
 #include <mpu9250/MPU9250.hpp>
 #include <DevMgr.hpp>
@@ -101,14 +100,25 @@ private:
 	orb_advert_t		    _accel_topic;
 	orb_advert_t		    _gyro_topic;
 
-	int			    _accel_calibration_sub;
+	int			    _param_update_sub;
-	int			    _gyro_calibration_sub;
 
-	struct accel_calibration_s  _accel_calibration;
+	struct accel_calibration_s {
-	struct gyro_calibration_s   _gyro_calibration;
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _accel_calibration;
 
-	bool			    _accel_calibration_set;
+	struct gyro_calibration_s {
-	bool			    _gyro_calibration_set;
+		float x_offset;
+		float x_scale;
+		float y_offset;
+		float y_scale;
+		float z_offset;
+		float z_scale;
+	} _gyro_calibration;
 
 	int			    _accel_orb_class_instance;
 	int			    _gyro_orb_class_instance;
@@ -122,18 +132,29 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) :
 	MPU9250(IMU_DEVICE_PATH),
 	_accel_topic(nullptr),
 	_gyro_topic(nullptr),
-	_accel_calibration_sub(-1),
+	_param_update_sub(-1),
-	_gyro_calibration_sub(-1),
 	_accel_calibration{},
 	_gyro_calibration{},
-	_accel_calibration_set(false),
-	_gyro_calibration_set(false),
 	_accel_orb_class_instance(-1),
 	_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)*/
 {
+	// Set sane default calibration values
+	_accel_calibration.x_scale = 1.0f;
+	_accel_calibration.y_scale = 1.0f;
+	_accel_calibration.z_scale = 1.0f;
+	_accel_calibration.x_offset = 0.0f;
+	_accel_calibration.y_offset = 0.0f;
+	_accel_calibration.z_offset = 0.0f;
+
+	_gyro_calibration.x_scale = 1.0f;
+	_gyro_calibration.y_scale = 1.0f;
+	_gyro_calibration.z_scale = 1.0f;
+	_gyro_calibration.x_offset = 0.0f;
+	_gyro_calibration.y_offset = 0.0f;
+	_gyro_calibration.z_offset = 0.0f;
 }
 
 DfMpu9250Wrapper::~DfMpu9250Wrapper()
@@ -164,13 +185,9 @@ int DfMpu9250Wrapper::start()
 		return -1;
 	}
 
-	/* Subscribe to calibration topics. */
+	/* Subscribe to param update topic. */
-	if (_accel_calibration_sub < 0) {
+	if (_param_update_sub < 0) {
-		_accel_calibration_sub = orb_subscribe(ORB_ID(accel_calibration));
+		_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	}
-
-	if (_gyro_calibration_sub < 0) {
-		_gyro_calibration_sub = orb_subscribe(ORB_ID(gyro_calibration));
 	}
 
 	/* Init device and start sensor. */
@@ -188,6 +205,12 @@ int DfMpu9250Wrapper::start()
 		return ret;
 	}
 
+	PX4_INFO("MPU9250 device id is: %d", m_id.dev_id);
+
+	/* Force getting the calibration values. */
+	_update_accel_calibration();
+	_update_gyro_calibration();
+
 	return 0;
 }
 
@@ -206,34 +229,130 @@ int DfMpu9250Wrapper::stop()
 
 void DfMpu9250Wrapper::_update_gyro_calibration()
 {
-	bool updated;
+	// TODO: replace magic number
-	orb_check(_gyro_calibration_sub, &updated);
+	for (unsigned i = 0; i < 3; ++i) {
 
-	if (updated) {
+		// TODO: remove printfs and add error counter
-		gyro_calibration_s new_calibration;
-		orb_copy(ORB_ID(gyro_calibration), _gyro_calibration_sub, &new_calibration);
 
-		/* Only accept calibration for this device. */
+		char str[30];
-		if (m_id.dev_id == new_calibration.device_id) {
+		(void)sprintf(str, "CAL_GYRO%u_ID", i);
-			_gyro_calibration = new_calibration;
+		int32_t device_id;
-			_gyro_calibration_set = true;
+		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_GYRO%u_XSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZSCALE", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_XOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_YOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_GYRO%u_ZOFF", i);
+		res = param_get(param_find(str), &_gyro_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
 		}
 	}
 }
 
 void DfMpu9250Wrapper::_update_accel_calibration()
 {
-	bool updated;
+	// TODO: replace magic number
-	orb_check(_accel_calibration_sub, &updated);
+	for (unsigned i = 0; i < 3; ++i) {
 
-	if (updated) {
+		// TODO: remove printfs and add error counter
-		accel_calibration_s new_calibration;
-		orb_copy(ORB_ID(accel_calibration), _accel_calibration_sub, &new_calibration);
 
-		/* Only accept calibration for this device. */
+		char str[30];
-		if (m_id.dev_id == new_calibration.device_id) {
+		(void)sprintf(str, "CAL_ACC%u_ID", i);
-			_accel_calibration = new_calibration;
+		int32_t device_id;
-			_accel_calibration_set = true;
+		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_ACC%u_XSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.x_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.y_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
+		res = param_get(param_find(str), &_accel_calibration.z_scale);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.x_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_YOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.y_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
+		}
+
+		(void)sprintf(str, "CAL_ACC%u_ZOFF", i);
+		res = param_get(param_find(str), &_accel_calibration.z_offset);
+
+		if (res != OK) {
+			PX4_ERR("Could not access param %s", str);
 		}
 	}
 }
@@ -241,11 +360,16 @@ void DfMpu9250Wrapper::_update_accel_calibration()
 int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 {
 	/* Check if calibration values are still up-to-date. */
+	bool updated;
+	orb_check(_param_update_sub, &updated);
+
+	if (updated) {
+		parameter_update_s parameter_update;
+		orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
+
 		_update_accel_calibration();
 		_update_gyro_calibration();
-
+	}
-	/* Don't publish if we have not received calibration data. */
-	if (_accel_calibration_set) {
 
 	/* Publish accel first. */
 	perf_begin(_accel_sample_perf);
@@ -257,14 +381,16 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	accel_report.x_raw = NAN;
 	accel_report.y_raw = NAN;
 	accel_report.z_raw = NAN;
-		accel_report.x = (data.accel_m_s2_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
+	accel_report.x = (data.accel_m_s2_x - _accel_calibration.x_offset) * _accel_calibration.x_scale;
-		accel_report.y = (data.accel_m_s2_y - _gyro_calibration.y_offset) * _gyro_calibration.y_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 - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
+	accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale;
 
 	// TODO: get these right
 	accel_report.scaling = -1.0f;
 	accel_report.range_m_s2 = -1.0f;
 
+	accel_report.device_id = m_id.dev_id;
+
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
@@ -274,10 +400,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	}
 
 	perf_end(_accel_sample_perf);
-	}
 
-	/* Don't publish if we have not received calibration data. */
-	if (_gyro_calibration_set) {
 
 	/* Then publish gyro. */
 	perf_begin(_gyro_sample_perf);
@@ -289,14 +412,16 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	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.x = (data.gyro_rad_s_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
-		gyro_report.y = data.gyro_rad_s_y;
+	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_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale;
 
 	// TODO: get these right
 	gyro_report.scaling = -1.0f;
 	gyro_report.range_rad_s = -1.0f;
 
+	gyro_report.device_id = m_id.dev_id;
+
 	// TODO: when is this ever blocked?
 	if (!(m_pub_blocked)) {
 
@@ -306,12 +431,9 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
 	}
 
 	perf_end(_gyro_sample_perf);
-	}
 
-	if (_accel_calibration_set || _gyro_calibration_set) {
 	/* Notify anyone waiting for data. */
 	DevMgr::updateNotify(*this);
-	}
 
 	// TODO: check the return codes of this function
 	return 0;