[WIP] sensor calibration cleanup and debug

msg/CMakeLists.txt

@@ -40,6 +40,8 @@ set(msg_files
 	airspeed.msg
 	att_pos_mocap.msg
 	battery_status.msg
+	calibration_accel.msg
+	calibration_gyro.msg
 	camera_capture.msg
 	camera_trigger.msg
 	collision_report.msg

msg/calibration_accel.msg

@@ -0,0 +1,10 @@
+
+# scaling factors; Vout = (Vin * Vscale) + Voffset
+
+float32 x_offset
+float32 y_offset
+float32 z_offset
+
+float32 x_scale
+float32 y_scale
+float32 z_scale

msg/calibration_gyro.msg

@@ -0,0 +1,10 @@
+
+# scaling factors; Vout = (Vin * Vscale) + Voffset
+
+float32 x_offset
+float32 y_offset
+float32 z_offset
+
+float32 x_scale
+float32 y_scale
+float32 z_scale

src/drivers/drv_accel.h

@@ -51,18 +51,6 @@
 #define ACCEL1_DEVICE_PATH	"/dev/accel1"
 #define ACCEL2_DEVICE_PATH	"/dev/accel2"
 
-#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

@@ -51,19 +51,6 @@
 #define GYRO1_DEVICE_PATH	"/dev/gyro1"
 #define GYRO2_DEVICE_PATH	"/dev/gyro2"
 
-#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/imu/adis16448/adis16448.cpp

@@ -81,6 +81,11 @@
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
 #include <lib/conversion/rotation.h>
 
+#include <uORB/topics/calibration_accel.h>
+#include <uORB/topics/calibration_gyro.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_gyro.h>
+
 #define DIR_READ				0x00
 #define DIR_WRITE				0x80
 
@@ -227,15 +232,15 @@ class ADIS16448 : public device::SPI
 	struct hrt_call		_call;
 	unsigned			_call_interval;
 
-	ringbuffer::RingBuffer			*_gyro_reports;
+	ringbuffer::RingBuffer			*_sensor_gyro_ss;
 
-	struct gyro_calibration_s	_gyro_scale;
+	calibration_gyro_s	_gyro_scale;
 	float				_gyro_range_scale;
 	float				_gyro_range_rad_s;
 
-	ringbuffer::RingBuffer			*_accel_reports;
+	ringbuffer::RingBuffer			*_sensor_accel_ss;
 
-	struct accel_calibration_s	_accel_scale;
+	calibration_accel_s	_accel_scale;
 	float				_accel_range_scale;
 	float				_accel_range_m_s2;
 	orb_advert_t		_accel_topic;
@@ -490,11 +495,11 @@ ADIS16448::ADIS16448(int bus, const char *path_accel, const char *path_gyro, con
 	_product(0),
 	_call{},
 	_call_interval(0),
-	_gyro_reports(nullptr),
+	_sensor_gyro_ss(nullptr),
 	_gyro_scale{},
 	_gyro_range_scale(0.0f),
 	_gyro_range_rad_s(0.0f),
-	_accel_reports(nullptr),
+	_sensor_accel_ss(nullptr),
 	_accel_scale{},
 	_accel_range_scale(0.0f),
 	_accel_range_m_s2(0.0f),
@@ -572,12 +577,12 @@ ADIS16448::~ADIS16448()
 	delete _mag;
 
 	/* free any existing reports */
-	if (_gyro_reports != nullptr) {
-		delete _gyro_reports;
+	if (_sensor_gyro_ss != nullptr) {
+		delete _sensor_gyro_ss;
 	}
 
-	if (_accel_reports != nullptr) {
-		delete _accel_reports;
+	if (_sensor_accel_ss != nullptr) {
+		delete _sensor_accel_ss;
 	}
 
 	if (_mag_reports != nullptr) {
@@ -611,15 +616,15 @@ ADIS16448::init()
 	}
 
 	/* allocate basic report buffers */
-	_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report));
+	_sensor_gyro_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
 
-	if (_gyro_reports == nullptr) {
+	if (_sensor_gyro_ss == nullptr) {
 		goto out;
 	}
 
-	_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
+	_sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
 
-	if (_accel_reports == nullptr) {
+	if (_sensor_accel_ss == nullptr) {
 		goto out;
 	}
 
@@ -679,8 +684,8 @@ ADIS16448::init()
 	measure();
 
 	/* advertise sensor topic, measure manually to initialize valid report */
-	struct accel_report arp;
-	_accel_reports->get(&arp);
+	sensor_accel_s arp;
+	_sensor_accel_ss->get(&arp);
 
 	/* measurement will have generated a report, publish */
 	_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
@@ -690,9 +695,9 @@ ADIS16448::init()
 		warnx("ADVERT FAIL");
 	}
 
-	struct gyro_report grp;
+	sensor_gyro_s grp;
 
-	_gyro_reports->get(&grp);
+	_sensor_gyro_ss->get(&grp);
 
 	_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
 			     &_gyro->_gyro_orb_class_instance, ORB_PRIO_MAX);
@@ -855,7 +860,7 @@ ADIS16448::_set_gyro_dyn_range(uint16_t desired_gyro_dyn_range)
 ssize_t
 ADIS16448::read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(accel_report);
+	unsigned count = buflen / sizeof(sensor_accel_s);
 
 	/* buffer must be large enough */
 	if (count < 1) {
@@ -864,23 +869,23 @@ ADIS16448::read(struct file *filp, char *buffer, size_t buflen)
 
 	/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
 	if (_call_interval == 0) {
-		_accel_reports->flush();
+		_sensor_accel_ss->flush();
 		measure();
 	}
 
 	/* if no data, error (we could block here) */
-	if (_accel_reports->empty()) {
+	if (_sensor_accel_ss->empty()) {
 		return -EAGAIN;
 	}
 
 	perf_count(_accel_reads);
 
 	/* copy reports out of our buffer to the caller */
-	accel_report *arp = reinterpret_cast<accel_report *>(buffer);
+	sensor_accel_s *arp = reinterpret_cast<sensor_accel_s *>(buffer);
 	int transferred = 0;
 
 	while (count--) {
-		if (!_accel_reports->get(arp)) {
+		if (!_sensor_accel_ss->get(arp)) {
 			break;
 		}
 
@@ -889,7 +894,7 @@ ADIS16448::read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* return the number of bytes transferred */
-	return (transferred * sizeof(accel_report));
+	return (transferred * sizeof(sensor_accel_s));
 }
 
 int
@@ -926,7 +931,7 @@ ADIS16448::gyro_self_test()
 ssize_t
 ADIS16448::gyro_read(struct file *filp, char *buffer, size_t buflen)
 {
-	unsigned count = buflen / sizeof(gyro_report);
+	unsigned count = buflen / sizeof(sensor_gyro_s);
 
 	/* buffer must be large enough */
 	if (count < 1) {
@@ -935,23 +940,23 @@ ADIS16448::gyro_read(struct file *filp, char *buffer, size_t buflen)
 
 	/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
 	if (_call_interval == 0) {
-		_gyro_reports->flush();
+		_sensor_gyro_ss->flush();
 		measure();
 	}
 
 	/* if no data, error (we could block here) */
-	if (_gyro_reports->empty()) {
+	if (_sensor_gyro_ss->empty()) {
 		return -EAGAIN;
 	}
 
 	perf_count(_gyro_reads);
 
 	/* copy reports out of our buffer to the caller */
-	gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
+	sensor_gyro_s *grp = reinterpret_cast<sensor_gyro_s *>(buffer);
 	int transferred = 0;
 
 	while (count--) {
-		if (!_gyro_reports->get(grp)) {
+		if (!_sensor_gyro_ss->get(grp)) {
 			break;
 		}
 
@@ -960,7 +965,7 @@ ADIS16448::gyro_read(struct file *filp, char *buffer, size_t buflen)
 	}
 
 	/* return the number of bytes transferred */
-	return (transferred * sizeof(gyro_report));
+	return (transferred * sizeof(sensor_gyro_s));
 }
 
 ssize_t
@@ -1095,7 +1100,7 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			irqstate_t flags = px4_enter_critical_section();
 
-			if (!_accel_reports->resize(arg)) {
+			if (!_sensor_accel_ss->resize(arg)) {
 				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
@@ -1114,7 +1119,7 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSCALE: {
 			/* copy scale, but only if off by a few percent */
-			struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
+			calibration_accel_s *s = (calibration_accel_s *) arg;
 			float sum = s->x_scale + s->y_scale + s->z_scale;
 
 			if (sum > 2.0f && sum < 4.0f) {
@@ -1128,7 +1133,7 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale));
+		memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale));
 		return OK;
 
 	case ACCELIOCSRANGE:
@@ -1168,7 +1173,7 @@ ADIS16448::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			irqstate_t flags = px4_enter_critical_section();
 
-			if (!_gyro_reports->resize(arg)) {
+			if (!_sensor_gyro_ss->resize(arg)) {
 				px4_leave_critical_section(flags);
 				return -ENOMEM;
 			}
@@ -1187,12 +1192,12 @@ ADIS16448::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSCALE:
 		/* copy scale in */
-		memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
+		memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCGSCALE:
 		/* copy scale out */
-		memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
+		memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale));
 		return OK;
 
 	case GYROIOCSRANGE:
@@ -1343,8 +1348,8 @@ ADIS16448::start()
 	_call_interval = last_call_interval;
 
 	/* discard any stale data in the buffers */
-	_gyro_reports->flush();
-	_accel_reports->flush();
+	_sensor_gyro_ss->flush();
+	_sensor_accel_ss->flush();
 	_mag_reports->flush();
 
 	/* start polling at the specified rate */
@@ -1423,8 +1428,8 @@ ADIS16448::measure()
 	/*
 	 * Report buffers.
 	 */
-	accel_report	arb;
-	gyro_report		grb;
+	sensor_accel_s	arb;
+	sensor_gyro_s		grb;
 	mag_report		mrb;
 
 	grb.timestamp = arb.timestamp = mrb.timestamp = hrt_absolute_time();
@@ -1548,8 +1553,8 @@ ADIS16448::measure()
 	grb.device_id = _gyro->_device_id.devid;
 	mrb.device_id = _mag->_device_id.devid;
 
-	_gyro_reports ->force(&grb);
-	_accel_reports->force(&arb);
+	_sensor_gyro_ss ->force(&grb);
+	_sensor_accel_ss->force(&arb);
 	_mag_reports  ->force(&mrb);
 
 	/* notify anyone waiting for data */
@@ -1622,8 +1627,8 @@ ADIS16448::print_info()
 	perf_print_counter(_gyro_reads);
 	perf_print_counter(_mag_reads);
 	perf_print_counter(_bad_transfers);
-	_accel_reports->print_info("accel queue");
-	_gyro_reports->print_info("gyro queue");
+	_sensor_accel_ss->print_info("accel queue");
+	_sensor_gyro_ss->print_info("gyro queue");
 	_mag_reports->print_info("mag queue");
 
 	printf("DEVICE ID:\nACCEL:\t%d\nGYRO:\t%d\nMAG:\t%d\n", _device_id.devid, _gyro->_device_id.devid,
@@ -1826,8 +1831,8 @@ start(enum Rotation rotation)
 void
 test()
 {
-	accel_report a_report;
-	gyro_report  g_report;
+	sensor_accel_s a_report;
+	sensor_gyro_s  g_report;
 	mag_report 	 m_report;
 
 	ssize_t sz;