FINALLY fix mag rotation issues. (#7366)

* sensors : second cut at fixing mag calibration - remove old problematic code

* sensors : use more intuitive naming for loop variables

src/modules/sensors/voted_sensors_update.cpp

@@ -230,9 +230,9 @@ void VotedSensorsUpdate::parameters_update()
 	unsigned accel_cal_found_count = 0;
 
 	/* run through all gyro sensors */
-	for (unsigned s = 0; s < GYRO_COUNT_MAX; s++) {
+	for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) {
 
-		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
+		(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, driver_index);
 
 		DevHandle h;
 		DevMgr::getHandle(str, h);
@@ -258,7 +258,7 @@ void VotedSensorsUpdate::parameters_update()
 				continue;
 			}
 
-			if (s == 0 && device_id > 0) {
+			if (driver_index == 0 && device_id > 0) {
 				gyro_cal_found_count++;
 			}
 
@@ -313,9 +313,9 @@ void VotedSensorsUpdate::parameters_update()
 	}
 
 	/* run through all accel sensors */
-	for (unsigned s = 0; s < ACCEL_COUNT_MAX; s++) {
+	for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) {
 
-		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
+		(void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, driver_index);
 
 		DevHandle h;
 		DevMgr::getHandle(str, h);
@@ -341,7 +341,7 @@ void VotedSensorsUpdate::parameters_update()
 				continue;
 			}
 
-			if (s == 0 && device_id > 0) {
+			if (driver_index == 0 && device_id > 0) {
 				accel_cal_found_count++;
 			}
 
@@ -396,15 +396,9 @@ void VotedSensorsUpdate::parameters_update()
 	}
 
 	/* run through all mag sensors */
-	for (unsigned s = 0; s < MAG_COUNT_MAX; s++) {
+	for (unsigned driver_index = 0; driver_index < MAG_COUNT_MAX; driver_index++) {
 
-		/* set a valid default rotation (same as board).
-		 * if the mag is configured, this might be replaced
-		 * in the section below.
-		 */
-		_mag_rotation[s] = _board_rotation;
-
-		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
+		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, driver_index);
 
 		DevHandle h;
 		DevMgr::getHandle(str, h);
@@ -414,7 +408,7 @@ void VotedSensorsUpdate::parameters_update()
 			continue;
 		}
 
-		_mag_device_id[s] = h.ioctl(DEVIOCGDEVICEID, 0);
+		_mag_device_id[driver_index] = h.ioctl(DEVIOCGDEVICEID, 0);
 		bool config_ok = false;
 
 		/* run through all stored calibrations */
@@ -425,16 +419,14 @@ void VotedSensorsUpdate::parameters_update()
 			(void)sprintf(str, "CAL_MAG%u_ID", i);
 			int device_id;
 			failed = failed || (OK != param_get(param_find(str), &device_id));
-			(void)sprintf(str, "CAL_MAG%u_ROT", i);
-			(void)param_find(str);
 
 			if (failed) {
 				DevMgr::releaseHandle(h);
 				continue;
 			}
 
 			/* if the calibration is for this device, apply it */
-			if (device_id == _mag_device_id[s]) {
+			if (device_id == _mag_device_id[driver_index]) {
 				struct mag_calibration_s mscale = {};
 				(void)sprintf(str, "CAL_MAG%u_XOFF", i);
 				failed = failed || (OK != param_get(param_find(str), &mscale.x_offset));
@@ -452,9 +444,7 @@ void VotedSensorsUpdate::parameters_update()
 				(void)sprintf(str, "CAL_MAG%u_ROT", i);
 
 				if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) {
-					/* mag is internal */
-					_mag_rotation[s] = _board_rotation;
-					/* reset param to -1 to indicate internal mag */
+					/* mag is internal - reset param to -1 to indicate internal mag */
 					int32_t minus_one;
 					param_get(param_find(str), &minus_one);
 
@@ -464,11 +454,11 @@ void VotedSensorsUpdate::parameters_update()
 					}
 
 				} else {
-
+					/* mag is external */
 					int32_t mag_rot;
 					param_get(param_find(str), &mag_rot);
 
-					/* check if this mag is still set as internal */
+					/* check if this mag is still set as internal, otherwise leave untouched */
 					if (mag_rot < 0) {
 						/* it was marked as internal, change to external with no rotation */
 						mag_rot = 0;
@@ -751,7 +741,7 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
 							_mag_rotation[uorb_index] = _board_rotation;
 
 						} else {
-							// Set external magnetometers to use the paramter value
+							// Set external magnetometers to use the parameter value
 							get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[uorb_index]);
 						}
 					}