From a5462fec0b48a823e38946d2f31aa2601b1d0ca9 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 10 Sep 2015 20:24:59 +1000
Subject: [PATCH] AP_Baro: allow selection of primary barometer and add 3rd
 baro

this is useful for external I2C barometers on a PH2
---
 libraries/AP_Baro/AP_Baro.cpp     | 28 +++++++++++++++++++---------
 libraries/AP_Baro/AP_Baro.h       |  3 ++-
 libraries/AP_Baro/AP_Baro_PX4.cpp |  3 ++-
 3 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index b5ea10c02b..c574eb3368 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -55,6 +55,12 @@ const AP_Param::GroupInfo AP_Baro::var_info[] PROGMEM = {
     // @Increment: 0.1
     AP_GROUPINFO("ALT_OFFSET", 5, AP_Baro, _alt_offset, 0),
 
+    // @Param: PRIMARY
+    // @DisplayName: Primary barometer
+    // @Description: This selects which barometer will be the primary if multiple barometers are found
+    // @Values: 0:FirstBaro,1:2ndBaro,2:3rdBaro
+    AP_GROUPINFO("PRIMARY", 6, AP_Baro, _primary_baro, 0),
+    
     AP_GROUPEND
 };
 
@@ -342,19 +348,23 @@ void AP_Baro::update(void)
         }
     }
 
-    // choose primary sensor
-    _primary = 0;
-    for (uint8_t i=0; i<_num_sensors; i++) {
-        if (healthy(i)) {
-            _primary = i;
-            break;
-        }
-    }
-
     // ensure the climb rate filter is updated
     if (healthy()) {
         _climb_rate_filter.update(get_altitude(), get_last_update());
     }
+
+    // choose primary sensor
+    if (_primary_baro >= 0 && _primary_baro < _num_sensors && healthy(_primary_baro)) {
+        _primary = _primary_baro;
+    } else {
+        _primary = 0;
+        for (uint8_t i=0; i<_num_sensors; i++) {
+            if (healthy(i)) {
+                _primary = i;
+                break;
+            }
+        }
+    }
 }
 
 /*
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index 89f2675ab7..5d37f77eb6 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -12,7 +12,7 @@
 #if HAL_CPU_CLASS == HAL_CPU_CLASS_16
 #define BARO_MAX_INSTANCES 1
 #else
-#define BARO_MAX_INSTANCES 2
+#define BARO_MAX_INSTANCES 3
 #endif
 
 // maximum number of drivers. Note that a single driver can provide
@@ -151,6 +151,7 @@ private:
     } sensors[BARO_MAX_INSTANCES];
 
     AP_Float                            _alt_offset;
+    AP_Int8                             _primary_baro; // primary chosen by user
     float                               _last_altitude_EAS2TAS;
     float                               _EAS2TAS;
     float                               _external_temperature;
diff --git a/libraries/AP_Baro/AP_Baro_PX4.cpp b/libraries/AP_Baro/AP_Baro_PX4.cpp
index 607230da77..924bdd5f60 100644
--- a/libraries/AP_Baro/AP_Baro_PX4.cpp
+++ b/libraries/AP_Baro/AP_Baro_PX4.cpp
@@ -26,8 +26,9 @@ AP_Baro_PX4::AP_Baro_PX4(AP_Baro &baro) :
     memset(instances, 0, sizeof(instances));
     instances[0].fd = open(BARO_BASE_DEVICE_PATH"0", O_RDONLY);
     instances[1].fd = open(BARO_BASE_DEVICE_PATH"1", O_RDONLY);
+    instances[2].fd = open(BARO_BASE_DEVICE_PATH"2", O_RDONLY);
 
-    for (uint8_t i=0; i<2; i++) {
+    for (uint8_t i=0; i<3; i++) {
         if (instances[i].fd != -1) {
             _num_instances = i+1;
         } else {