AP_Compass: support 3 mags on PX4

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -43,18 +43,20 @@ extern const AP_HAL::HAL& hal;
 bool AP_Compass_PX4::init(void)
 {
 	_mag_fd[0] = open(MAG_DEVICE_PATH, O_RDONLY);
-	if (_mag_fd[0] < 0) {
+	_mag_fd[1] = open(MAG_DEVICE_PATH "1", O_RDONLY);
+	_mag_fd[2] = open(MAG_DEVICE_PATH "2", O_RDONLY);
+
+    _num_instances = 0;
+    for (uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
+        if (_mag_fd[i] >= 0) {
+            _num_instances = i+1;
+        }
+    }    
+	if (_num_instances == 0) {
         hal.console->printf("Unable to open " MAG_DEVICE_PATH "\n");
         return false;
 	}
 
-	_mag_fd[1] = open(MAG_DEVICE_PATH "1", O_RDONLY);
-	if (_mag_fd[1] >= 0) {
-        _num_instances = 2;
-	} else {
-        _num_instances = 1;
-    }
-
     for (uint8_t i=0; i<_num_instances; i++) {
         // average over up to 20 samples
         if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {

libraries/AP_Compass/Compass.cpp

@@ -110,6 +110,11 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
     AP_GROUPINFO("PRIMARY", 12, Compass, _primary, 0),
 #endif
 
+#if COMPASS_MAX_INSTANCES > 2
+    AP_GROUPINFO("OFS3",    13, Compass, _offset[2], 0),
+    AP_GROUPINFO("MOT3",    14, Compass, _motor_compensation[2], 0),
+#endif
+
     AP_GROUPEND
 };
 

libraries/AP_Compass/Compass.h

@@ -45,7 +45,7 @@
    than 1 then redundent sensors may be available
  */
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
-#define COMPASS_MAX_INSTANCES 2
+#define COMPASS_MAX_INSTANCES 3
 #elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 #define COMPASS_MAX_INSTANCES 2
 #else