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.

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);
-		result = param_get(param_find(str), &(device_ids[cur_mag]));
+		(void)sprintf(str, "CAL_MAG%u_XOFF", 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);
-			break;
+			PX4_ERR("unable to reset %s", str);
+		}
+		(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,24 +469,31 @@ 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) {
-					mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
-					result = calibrate_return_error;
-					break;
-				}
-
-				// Get priority
-				int32_t prio;
-				orb_priority(worker_data.sub_mag[cur_mag], &prio);
-
-				if (prio > device_prio_max) {
-					device_prio_max = prio;
-					device_id_primary = device_ids[cur_mag];
-				}
+			// 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) {
+				mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
+				result = calibrate_return_error;
+				break;
 			}
+
+			// Get priority
+			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)));
-					(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
-					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale)));
-					(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag);
-					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
+					// FIXME: scaling is not used right now
+					//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale)));
+					//(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag);
+					//failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_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. */
-
-	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);
-	}
-
+	/* On QURT, the params are read directly in the respective wrappers. */
 	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. */
-
-	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);
-	}
-
+	/* On QURT, the params are read directly in the respective wrappers. */
 	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. */
-
-	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);
-	}
-
+	/* On QURT, the params are read directly in the respective wrappers. */
 	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);