commander : warn if more than 3 mags are connected and start only 3 o…

…n Pixhawk 2

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -135,10 +135,13 @@ then
 		then
 		fi
 
+		# We don't start this mag on the Pixhawk 2, because we do not realistically
+		# need more than 3 mags online (2 internal + 1 external in the GPS unit)
+
 		# external LSM303D is rotated 270 degrees yaw
-		if lsm303d -X -R 6 start
-		then
-		fi
+		# if lsm303d -X -R 6 start
+		# then
+		# fi
 
 		# internal MPU6000 is rotated 180 deg roll, 270 deg yaw
 		if mpu6000 -R 14 start

src/modules/commander/mag_calibration.cpp

@@ -565,7 +565,12 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub)
 		// We should not try to subscribe if the topic doesn't actually exist and can be counted.
 		const unsigned mag_count = orb_group_count(ORB_ID(sensor_mag));
 
-		for (unsigned cur_mag = 0; cur_mag < mag_count; cur_mag++) {
+		// Warn that we will not calibrate more than max_mags magnetometers
+		if (mag_count > max_mags) {
+			calibration_log_critical(mavlink_log_pub, "[cal] Detected %u mags, but will calibrate only %u", mag_count, max_mags);
+		}
+
+		for (unsigned cur_mag = 0; cur_mag < mag_count && cur_mag < max_mags; cur_mag++) {
 			// Mag in this slot is available
 			worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);