Commander: Use indexed sensor names in calibration routines

3 files changed, 32 insertions, 24 deletions

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 13ab966ab..d9e7e21fc 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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
@@ -221,17 +221,17 @@ int do_accel_calibration(int mavlink_fd)
 		accel_scale.z_scale = accel_T_rotated(2, 2);
 
 		/* set parameters */
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
-		    || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
-		    || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
-		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
-		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
-		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
+		if (param_set(param_find("CAL_ACC0_XOFF"), &(accel_scale.x_offset))
+		    || param_set(param_find("CAL_ACC0_YOFF"), &(accel_scale.y_offset))
+		    || param_set(param_find("CAL_ACC0_ZOFF"), &(accel_scale.z_offset))
+		    || param_set(param_find("CAL_ACC0_XSCALE"), &(accel_scale.x_scale))
+		    || param_set(param_find("CAL_ACC0_YSCALE"), &(accel_scale.y_scale))
+		    || param_set(param_find("CAL_ACC0_ZSCALE"), &(accel_scale.z_scale))) {
 			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			res = ERROR;
 		}
 
-		if (param_set(param_find("SENS_ACC_ID"), &(device_id))) {
+		if (param_set(param_find("CAL_ACC0_ID"), &(device_id))) {
 				res = ERROR;
 		}
 	}
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 8410297ef..e941e327c 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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
@@ -51,6 +51,7 @@
 #include <mavlink/mavlink_log.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
 
 /* oddly, ERROR is not defined for c++ */
 #ifdef ERROR
@@ -80,6 +81,13 @@ int do_gyro_calibration(int mavlink_fd)
 
 	int res = OK;
 
+	/* store board ID */
+	uint32_t mcu_id[3];
+	mcu_unique_id(&mcu_id[0]);
+
+	/* store last 32bit number - not unique, but unique in a given set */
+	param_set(param_find("CAL_BOARD_ID"), &mcu_id[2]);
+
 	/* reset all offsets to zero and all scales to one */
 	int fd = open(GYRO_DEVICE_PATH, 0);
 
@@ -149,9 +157,9 @@ int do_gyro_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* set offset parameters to new values */
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
-		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
-		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
+		if (param_set(param_find("CAL_GYRO0_XOFF"), &(gyro_scale.x_offset))
+		    || param_set(param_find("CAL_GYRO0_YOFF"), &(gyro_scale.y_offset))
+		    || param_set(param_find("CAL_GYRO0_ZOFF"), &(gyro_scale.z_offset))) {
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
 			res = ERROR;
 		}
@@ -275,13 +283,13 @@ int do_gyro_calibration(int mavlink_fd)
 
 	if (res == OK) {
 		/* set scale parameters to new values */
-		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
-		    || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
-		    || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
+		if (param_set(param_find("CAL_GYRO0_XSCALE"), &(gyro_scale.x_scale))
+		    || param_set(param_find("CAL_GYRO0_YSCALE"), &(gyro_scale.y_scale))
+		    || param_set(param_find("CAL_GYRO0_ZSCALE"), &(gyro_scale.z_scale))) {
 			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
 			res = ERROR;
 		}
-		if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) {
+		if (param_set(param_find("CAL_GYRO0_ID"), &(device_id))) {
 				res = ERROR;
 			}
 	}
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 2afb9a440..7147fb283 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2015 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
@@ -263,30 +263,30 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (res == OK) {
 			/* set parameters */
-			if (param_set(param_find("SENS_MAG_ID"), &(device_id))) {
+			if (param_set(param_find("CAL_MAG0_ID"), &(device_id))) {
 				res = ERROR;
 			}
-			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
+			if (param_set(param_find("CAL_MAG0_XOFF"), &(mscale.x_offset))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
+			if (param_set(param_find("CAL_MAG0_YOFF"), &(mscale.y_offset))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
+			if (param_set(param_find("CAL_MAG0_ZOFF"), &(mscale.z_offset))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
+			if (param_set(param_find("CAL_MAG0_XSCALE"), &(mscale.x_scale))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
+			if (param_set(param_find("CAL_MAG0_YSCALE"), &(mscale.y_scale))) {
 				res = ERROR;
 			}
 
-			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
+			if (param_set(param_find("CAL_MAG0_ZSCALE"), &(mscale.z_scale))) {
 				res = ERROR;
 			}