AP_Compass: added COMPASS_PRIMARY parameter

this allows selection of which compass is the primary. Useful if the
first compass starts giving spurious data (as happened in our plane)

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -149,6 +149,9 @@ void AP_Compass_PX4::accumulate(void)
 
 uint8_t AP_Compass_PX4::_get_primary(void) const
 {
+    if (_primary < _num_instances && _healthy[_primary]) {
+        return _primary;
+    }
     for (uint8_t i=0; i<_num_instances; i++) {
         if (_healthy[i]) return i;
     }    

libraries/AP_Compass/AP_Compass_VRBRAIN.cpp

@@ -146,6 +146,9 @@ void AP_Compass_VRBRAIN::accumulate(void)
 
 uint8_t AP_Compass_VRBRAIN::_get_primary(void) const
 {
+    if (_primary < _num_instances && _healthy[_primary]) {
+        return _primary;
+    }
     for (uint8_t i=0; i<_num_instances; i++) {
         if (_healthy[i]) return i;
     }    

libraries/AP_Compass/Compass.cpp

@@ -101,6 +101,13 @@ const AP_Param::GroupInfo Compass::var_info[] PROGMEM = {
 #if COMPASS_MAX_INSTANCES > 1
     AP_GROUPINFO("OFS2",    10, Compass, _offset[1], 0),
     AP_GROUPINFO("MOT2",    11, Compass, _motor_compensation[1], 0),
+
+    // @Param: PRIMARY
+    // @DisplayName: Choose primary compass
+    // @Description: If more than one compass is available this selects which compass is the primary.
+    // @Values: 0:FirstCompas,1:SecondCompass
+    // @User: Advanced
+    AP_GROUPINFO("PRIMARY", 12, Compass, _primary, 0),
 #endif
 
     AP_GROUPEND

libraries/AP_Compass/Compass.h

@@ -231,6 +231,9 @@ protected:
     AP_Int8 _use_for_yaw;                       ///<enable use for yaw calculation
     AP_Int8 _auto_declination;                  ///<enable automatic declination code
     AP_Int8 _external;                          ///<compass is external
+#if COMPASS_MAX_INSTANCES > 1
+    AP_Int8 _primary;                           ///primary instance
+#endif
 
     bool _null_init_done;                           ///< first-time-around flag used by offset nulling