AP_Baro: added option to treat MS5611 as MS5607

and add arming check for pressure altitude error

libraries/AP_Baro/AP_Baro.cpp

@@ -53,6 +53,8 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Arming/AP_Arming.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_GPS/AP_GPS.h>
+#include <AP_Vehicle/AP_Vehicle.h>
 
 #define INTERNAL_TEMPERATURE_CLAMP 35.0f
 
@@ -232,6 +234,25 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     AP_GROUPINFO("_FIELD_ELV", 22, AP_Baro, _field_elevation, 0),
 #endif
 #endif
+
+#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    // @Param: _ALTERR_MAX
+    // @DisplayName: Altitude error maximum
+    // @Description: This is the maximum acceptable altitude discrepancy between GPS altitude and barometric presssure altitude calculated against a standard atmosphere for arming checks to pass. If you are getting an arming error due to this parameter then you may have a faulty or substituted barometer. A common issue is vendors replacing a MS5611 in a "Pixhawk" with a MS5607. If you have that issue then please see BARO_OPTIONS parameter to force the MS5611 to be treated as a MS5607. This check is disabled if the value is zero.
+    // @Units: m
+    // @Increment: 1
+    // @Range: 0 5000
+    // @User: Advanced
+    AP_GROUPINFO("_ALTERR_MAX", 23, AP_Baro, _alt_error_max, 2000),
+
+    // @Param: _OPTIONS
+    // @DisplayName: Barometer options
+    // @Description: Barometer options
+    // @Bitmask: 0:Treat MS5611 as MS5607
+    // @User: Advanced
+    AP_GROUPINFO("_OPTIONS", 24, AP_Baro, _options, 0),
+#endif
+    
     AP_GROUPEND
 };
 
@@ -1050,6 +1071,34 @@ void AP_Baro::handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt)
 }
 #endif  // AP_BARO_EXTERNALAHRS_ENABLED
 
+// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
+bool AP_Baro::arming_checks(size_t buflen, char *buffer) const
+{
+    if (!all_healthy()) {
+        hal.util->snprintf(buffer, buflen, "not healthy");
+        return false;
+    }
+
+#if APM_BUILD_COPTER_OR_HELI || APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    /*
+      check for a pressure altitude discrepancy between GPS alt and
+      baro alt this catches bad barometers, such as when a MS5607 has
+      been substituted for a MS5611
+     */
+    const auto &gps = AP::gps();
+    if (_alt_error_max > 0 && gps.status() >= AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
+        const float alt_amsl = gps.location().alt*0.01;
+        const float alt_pressure = get_altitude_difference(SSL_AIR_PRESSURE, get_pressure());
+        const float error = fabsf(alt_amsl - alt_pressure);
+        if (error > _alt_error_max) {
+            hal.util->snprintf(buffer, buflen, "GPS alt error %.0fm (see BARO_ALTERR_MAX)", error);
+            return false;
+        }
+    }
+#endif
+    return true;
+}
+
 namespace AP {
 
 AP_Baro &baro()

libraries/AP_Baro/AP_Baro.h

@@ -79,6 +79,9 @@ public:
     // check if all baros are healthy - used for SYS_STATUS report
     bool all_healthy(void) const;
 
+    // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
+    bool arming_checks(size_t buflen, char *buffer) const;
+
     // get primary sensor
     uint8_t get_primary(void) const { return _primary; }
 
@@ -205,6 +208,16 @@ public:
     void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt);
 #endif
 
+    enum Options : uint16_t {
+        TreatMS5611AsMS5607     = (1U << 0U),
+    };
+
+    // check if an option is set
+    bool option_enabled(const Options option) const
+    {
+        return (uint16_t(_options.get()) & uint16_t(option)) != 0;
+    }
+
 private:
     // singleton
     static AP_Baro *_singleton;
@@ -299,6 +312,12 @@ private:
     AP_Int8                            _filter_range;  // valid value range from mean value
     AP_Int32                           _baro_probe_ext;
 
+#ifndef HAL_BUILD_AP_PERIPH
+    AP_Float                           _alt_error_max;
+#endif
+
+    AP_Int16                           _options;
+
     // semaphore for API access from threads
     HAL_Semaphore                      _rsem;
 

libraries/AP_Baro/AP_Baro_Backend.h

@@ -58,6 +58,9 @@ public:
         DEVTYPE_BARO_MSP      = 0x0E,
         DEVTYPE_BARO_ICP101XX = 0x0F,
         DEVTYPE_BARO_ICP201XX = 0x10,
+        DEVTYPE_BARO_MS5607   = 0x11,
+        DEVTYPE_BARO_MS5837   = 0x12,
+        DEVTYPE_BARO_MS5637   = 0x13,
     };
     
 protected:

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -21,6 +21,7 @@
 
 #include <AP_Math/AP_Math.h>
 #include <AP_Math/crc.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
 
 extern const AP_HAL::HAL &hal;
 
@@ -95,6 +96,13 @@ bool AP_Baro_MS56XX::_init()
     _dev->transfer(&CMD_MS56XX_RESET, 1, nullptr, 0);
     hal.scheduler->delay(4);
 
+    /*
+      cope with vendors substituting a MS5607 for a MS5611 on Pixhawk1 'clone' boards
+     */
+    if (_ms56xx_type == BARO_MS5611 && _frontend.option_enabled(AP_Baro::Options::TreatMS5611AsMS5607)) {
+        _ms56xx_type = BARO_MS5607;
+    }
+    
     const char *name = "MS5611";
     switch (_ms56xx_type) {
     case BARO_MS5607:
@@ -136,7 +144,23 @@ bool AP_Baro_MS56XX::_init()
 
     _instance = _frontend.register_sensor();
 
-    _dev->set_device_type(DEVTYPE_BARO_MS5611);
+    enum DevTypes devtype = DEVTYPE_BARO_MS5611;
+    switch (_ms56xx_type) {
+    case BARO_MS5607:
+        devtype = DEVTYPE_BARO_MS5607;
+        break;
+    case BARO_MS5611:
+        devtype = DEVTYPE_BARO_MS5611;
+        break;
+    case BARO_MS5837:
+        devtype = DEVTYPE_BARO_MS5837;
+        break;
+    case BARO_MS5637:
+        devtype = DEVTYPE_BARO_MS5637;
+        break;
+    }
+
+    _dev->set_device_type(devtype);
     set_bus_id(_instance, _dev->get_bus_id());
 
     if (_ms56xx_type == BARO_MS5837) {