From 52e1be74a209fe04e4b984cb29d6e18106d2a7d2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 16 Oct 2022 21:52:18 +1100 Subject: [PATCH] AP_Baro: added option to treat MS5611 as MS5607 and add arming check for pressure altitude error --- libraries/AP_Baro/AP_Baro.cpp | 49 ++++++++++++++++++++++++++++ libraries/AP_Baro/AP_Baro.h | 19 +++++++++++ libraries/AP_Baro/AP_Baro_Backend.h | 3 ++ libraries/AP_Baro/AP_Baro_MS5611.cpp | 26 ++++++++++++++- 4 files changed, 96 insertions(+), 1 deletion(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 5ca798c386..9dac72a5aa 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -53,6 +53,8 @@ #include #include #include +#include +#include #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() diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 3d77f8b741..b1c7b5bf79 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/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; diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index d7a402059f..914995529c 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/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: diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index bdce3769f6..295cd2e9e4 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -21,6 +21,7 @@ #include #include +#include extern const AP_HAL::HAL &hal; @@ -94,6 +95,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) { @@ -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) {