Save and check device id for acc and gyro calibration parameters.

Fix config utility to work with all devices of each type.
Accel, gyro and mag devices correctly set their device_id devtype.
Combo devices (mpu6000 lsm303d) now correctly return their devtype.
config util shows device id for all sensor types.
Add, save during calibration and check during preflight ID parameters for accelerometer and gyro

src/drivers/drv_mag.h

@@ -41,7 +41,6 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
-#include "drv_device.h"
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
@@ -83,11 +82,6 @@ struct mag_scale {
  */
 ORB_DECLARE(sensor_mag);
 
-/*
- * mag device types, for _device_id
- */
-#define DRV_MAG_DEVTYPE_HMC5883 1
-#define DRV_MAG_DEVTYPE_LSM303D 2
 
 /*
  * ioctl() definitions

src/drivers/drv_sensor.h

@@ -43,6 +43,24 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#include "drv_device.h"
+
+/**
+ * Sensor type definitions.
+ *
+ * Used to create a unique device id for redundant and combo sensors
+ */
+
+#define DRV_MAG_DEVTYPE_HMC5883 0x01
+#define DRV_MAG_DEVTYPE_LSM303D 0x02
+#define DRV_ACC_DEVTYPE_LSM303D 0x11
+#define DRV_ACC_DEVTYPE_BMA180  0x12
+#define DRV_ACC_DEVTYPE_MPU6000 0x13
+#define DRV_GYR_DEVTYPE_MPU6000 0x21
+#define DRV_GYR_DEVTYPE_L3GD20  0x22
+#define DRV_RNG_DEVTYPE_MB12XX  0x31
+#define DRV_RNG_DEVTYPE_LL40LS  0x32
+
 /*
  * ioctl() definitions
  *

src/drivers/l3gd20/l3gd20.cpp

@@ -424,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20;
+
 	// default scale factors
 	_gyro_scale.x_offset = 0;
 	_gyro_scale.x_scale  = 1.0f;

src/drivers/lsm303d/lsm303d.cpp

@@ -569,11 +569,18 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota
 	_constant_accel_count(0),
 	_checked_next(0)
 {
-	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
 
 	// enable debug() calls
 	_debug_enabled = true;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_LSM303D;
+	
+	/* Prime _mag with parents devid. */
+	_mag->_device_id.devid = _device_id.devid;
+	_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D;
+
+
 	// default scale factors
 	_accel_scale.x_offset = 0.0f;
 	_accel_scale.x_scale  = 1.0f;
@@ -660,6 +667,7 @@ LSM303D::init()
 		warnx("ADVERT ERR");
 	}
 
+
 	_accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
 
 	/* advertise sensor topic, measure manually to initialize valid report */
@@ -1769,7 +1777,13 @@ LSM303D_mag::read(struct file *filp, char *buffer, size_t buflen)
 int
 LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->mag_ioctl(filp, cmd, arg);
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->mag_ioctl(filp, cmd, arg);
+	}
 }
 
 void

src/drivers/mpu6000/mpu6000.cpp

@@ -523,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
 	// disable debug() calls
 	_debug_enabled = false;
 
+	_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000;
+
+	/* Prime _gyro with parents devid. */
+	_gyro->_device_id.devid = _device_id.devid;
+	_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000;
+
 	// default accel scale factors
 	_accel_scale.x_offset = 0;
 	_accel_scale.x_scale  = 1.0f;
@@ -609,6 +615,7 @@ MPU6000::init()
 	_gyro_scale.z_offset = 0;
 	_gyro_scale.z_scale  = 1.0f;
 
+
 	/* do CDev init for the gyro device node, keep it optional */
 	ret = _gyro->init();
 	/* if probe/setup failed, bail now */
@@ -1848,7 +1855,14 @@ MPU6000_gyro::read(struct file *filp, char *buffer, size_t buflen)
 int
 MPU6000_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
-	return _parent->gyro_ioctl(filp, cmd, arg);
+
+	switch (cmd) {
+		case DEVIOCGDEVICEID:
+			return (int)CDev::ioctl(filp, cmd, arg);
+			break;
+		default:
+			return _parent->gyro_ioctl(filp, cmd, arg);
+	}
 }
 
 /**

src/modules/commander/accelerometer_calibration.cpp

@@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
+	int32_t device_id;
 
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
@@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd)
 
 	/* reset all offsets to zero and all scales to one */
 	fd = open(ACCEL_DEVICE_PATH, 0);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
 	close(fd);
 
@@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			res = ERROR;
 		}
+
+		if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
+				res = ERROR;
+		}
 	}
 
 	if (res == OK) {

src/modules/commander/gyro_calibration.cpp

@@ -62,6 +62,7 @@ static const char *sensor_name = "gyro";
 
 int do_gyro_calibration(int mavlink_fd)
 {
+	int32_t device_id;
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 	mavlink_log_info(mavlink_fd, "HOLD STILL");
 
@@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd)
 
 	/* reset all offsets to zero and all scales to one */
 	int fd = open(GYRO_DEVICE_PATH, 0);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
 	close(fd);
 
@@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd)
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
 			res = ERROR;
 		}
+		if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
+				res = ERROR;
+			}
 	}
 
 	if (res == OK) {

src/modules/sensors/sensor_params.c

@@ -44,6 +44,13 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
+/**
+ * ID of the Gyro that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_GYRO_ID, 0);
+
 /**
  * Gyro X-axis offset
  *
@@ -153,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f);
 
+/**
+ * ID of the Accelerometer that the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_ACC_ID, 0);
 
 /**
  * Accelerometer X-axis offset

src/systemcmds/config/config.c

@@ -192,8 +192,12 @@ do_gyro(int argc, char *argv[])
 		int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
 		int range = ioctl(fd, GYROIOCGRANGE, 0);
+		int id = ioctl(fd, DEVIOCGDEVICEID,0);
+		int32_t calibration_id = 0;
 
-		warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
+		param_get(param_find("SENS_GYRO_ID"), &(calibration_id));
+
+		warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range);
 
 		close(fd);
 	}
@@ -267,9 +271,10 @@ do_mag(int argc, char *argv[])
 		int range = ioctl(fd, MAGIOCGRANGE, 0);
 		int id = ioctl(fd, DEVIOCGDEVICEID,0);
 		int32_t calibration_id = 0;
+
 		param_get(param_find("SENS_MAG_ID"), &(calibration_id));
 
-		warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
+		warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range);
 
 		close(fd);
 	}
@@ -341,8 +346,12 @@ do_accel(int argc, char *argv[])
 		int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
 		int range = ioctl(fd, ACCELIOCGRANGE, 0);
+		int id = ioctl(fd, DEVIOCGDEVICEID,0);
+		int32_t calibration_id = 0;
 
-		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
+		param_get(param_find("SENS_ACC_ID"), &(calibration_id));
+
+		warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range);
 
 		close(fd);
 	}

src/systemcmds/preflight_check/preflight_check.c

@@ -84,7 +84,7 @@ int preflight_check_main(int argc, char *argv[])
 	/* open text message output path */
 	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	int ret;
-	int32_t mag_devid,mag_calibration_devid;
+	int32_t devid, calibration_devid;
 
 	/* give the system some time to sample the sensors in the background */
 	usleep(150000);
@@ -98,9 +98,9 @@ int preflight_check_main(int argc, char *argv[])
 		goto system_eval;
 	}
 
-	mag_devid = ioctl(fd, DEVIOCGDEVICEID,0);
+	devid = ioctl(fd, DEVIOCGDEVICEID,0);
-	param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid));
+	param_get(param_find("SENS_MAG_ID"), &(calibration_devid));
-	if (mag_devid != mag_calibration_devid){
+	if (devid != calibration_devid){
 		warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
 		system_ok = false;
@@ -120,6 +120,16 @@ int preflight_check_main(int argc, char *argv[])
 
 	close(fd);
 	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
+
+	devid = ioctl(fd, DEVIOCGDEVICEID,0);
+	param_get(param_find("SENS_ACC_ID"), &(calibration_devid));
+	if (devid != calibration_devid){
+		warnx("accelerometer calibration is for a different device - calibrate accelerometer first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID");
+		system_ok = false;
+		goto system_eval;
+	}
+
 	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 
 	if (ret != OK) {
@@ -156,6 +166,16 @@ int preflight_check_main(int argc, char *argv[])
 
 	close(fd);
 	fd = open(GYRO_DEVICE_PATH, 0);
+
+	devid = ioctl(fd, DEVIOCGDEVICEID,0);
+	param_get(param_find("SENS_GYRO_ID"), &(calibration_devid));
+	if (devid != calibration_devid){
+		warnx("gyro calibration is for a different device - calibrate gyro first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID");
+		system_ok = false;
+		goto system_eval;
+	}
+
 	ret = ioctl(fd, GYROIOCSELFTEST, 0);
 
 	if (ret != OK) {