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

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

2 files changed, 42 insertions, 13 deletions

diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 9f13edb18..f54342f06 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -81,7 +81,7 @@ config_main(int argc, char *argv[])
 			do_device(argc - 1, argv + 1);
 		}
 	}
-	
+
 	errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'");
 }
 
@@ -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;
+
+		param_get(param_find("SENS_GYRO_ID"), &(calibration_id));
 
-		warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range);
+		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;
+
+		param_get(param_find("SENS_ACC_ID"), &(calibration_id));
 
-		warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range);
+		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);
 	}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index bbd90b961..3e1f76714 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/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);
-	param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid));
-	if (mag_devid != mag_calibration_devid){
+	devid = ioctl(fd, DEVIOCGDEVICEID,0);
+	param_get(param_find("SENS_MAG_ID"), &(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;
@@ -108,7 +108,7 @@ int preflight_check_main(int argc, char *argv[])
 	}
 
 	ret = ioctl(fd, MAGIOCSELFTEST, 0);
-	
+
 	if (ret != OK) {
 		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
@@ -120,8 +120,18 @@ 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) {
 		warnx("accel self test failed");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
@@ -156,8 +166,18 @@ 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) {
 		warnx("gyro self test failed");
 		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
@@ -183,10 +203,10 @@ int preflight_check_main(int argc, char *argv[])
 	system_ok &= rc_ok;
 
 
-		
+
 
 system_eval:
-	
+
 	if (system_ok) {
 		/* all good, exit silently */
 		exit(0);