Add SENS_MAG_ID parameter

Record devid to SENS_MAG_ID during mag calibration
Verify devid matches SENS_MAG_ID during preflight_check

5 files changed, 31 insertions, 1 deletions

diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h
index 5ddf5d08e..d341e8947 100644
--- a/src/drivers/drv_mag.h
+++ b/src/drivers/drv_mag.h
@@ -41,9 +41,11 @@
 #include <stdint.h>
 #include <sys/ioctl.h>
 
+#include "drv_device.h"
 #include "drv_sensor.h"
 #include "drv_orb_dev.h"
 
+
 #define MAG_DEVICE_PATH		"/dev/mag"
 
 /**
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 7be8de9c6..9d2abd3ce 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -65,6 +65,7 @@ static const char *sensor_name = "mag";
 
 int do_mag_calibration(int mavlink_fd)
 {
+	int32_t device_id;
 	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 	mavlink_log_info(mavlink_fd, "don't move system");
 
@@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd)
 
 	/* erase old calibration */
 	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+
+	device_id = ioctl(fd, DEVIOCGDEVICEID, 0);
+
 	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
 	if (res != OK) {
@@ -253,6 +257,9 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (res == OK) {
 			/* set parameters */
+			if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
+				res = ERROR;
+			}
 			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
 				res = ERROR;
 			}
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index a065541b9..bf5708e0b 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -98,6 +98,12 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
 
+/**
+ * ID of Magnetometer the calibration is for.
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_MAG_ID, 0);
 
 /**
  * Magnetometer X-axis offset
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 4a97d328c..077bc47c9 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -58,6 +58,7 @@
 
 #include "systemlib/systemlib.h"
 #include "systemlib/err.h"
+#include "systemlib/param/param.h"
 
 __EXPORT int config_main(int argc, char *argv[]);
 
@@ -264,8 +265,11 @@ do_mag(int argc, char *argv[])
 		int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
 		int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0);
 		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\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", srate, prate, range);
+		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);
 
 		close(fd);
 	}
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 86e4ff545..bbd90b961 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -84,6 +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;
 
 	/* give the system some time to sample the sensors in the background */
 	usleep(150000);
@@ -96,6 +97,16 @@ int preflight_check_main(int argc, char *argv[])
 		system_ok = false;
 		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){
+		warnx("magnetometer calibration is for a different device - calibrate magnetometer first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
+		system_ok = false;
+		goto system_eval;
+	}
+
 	ret = ioctl(fd, MAGIOCSELFTEST, 0);
 	
 	if (ret != OK) {