diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 7388c31cd6..e71a0dd284 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -46,6 +46,7 @@ #if HAL_ENABLE_LIBUAVCAN_DRIVERS #include "AP_Baro_UAVCAN.h" #endif +#include "AP_Baro_MSP.h" #include #include @@ -162,12 +163,12 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = { // @Increment: 1 AP_GROUPINFO("FLTR_RNG", 13, AP_Baro, _filter_range, HAL_BARO_FILTER_DEFAULT), -#ifdef HAL_PROBE_EXTERNAL_I2C_BAROS +#if defined(HAL_PROBE_EXTERNAL_I2C_BAROS) || defined(HAL_MSP_BARO_ENABLED) // @Param: PROBE_EXT // @DisplayName: External barometers to probe // @Description: This sets which types of external i2c barometer to look for. It is a bitmask of barometer types. The I2C buses to probe is based on GND_EXT_BUS. If GND_EXT_BUS is -1 then it will probe all external buses, otherwise it will probe just the bus number given in GND_EXT_BUS. - // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06 - // @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller,512:MS5837,1024:BMP388,2048:SPL06 + // @Bitmask: 0:BMP085,1:BMP280,2:MS5611,3:MS5607,4:MS5637,5:FBM320,6:DPS280,7:LPS25H,8:Keller,9:MS5837,10:BMP388,11:SPL06,12:MSP + // @Values: 1:BMP085,2:BMP280,4:MS5611,8:MS5607,16:MS5637,32:FBM320,64:DPS280,128:LPS25H,256:Keller,512:MS5837,1024:BMP388,2048:SPL06,4096:MSP // @User: Advanced AP_GROUPINFO("PROBE_EXT", 14, AP_Baro, _baro_probe_ext, HAL_BARO_PROBE_EXT_DEFAULT), #endif @@ -493,6 +494,8 @@ bool AP_Baro::_add_backend(AP_Baro_Backend *backend) */ void AP_Baro::init(void) { + init_done = true; + // ensure that there isn't a previous ground temperature saved if (!is_zero(_user_ground_temperature)) { _user_ground_temperature.set_and_save(0.0f); @@ -641,6 +644,18 @@ void AP_Baro::init(void) _probe_i2c_barometers(); #endif +#if HAL_MSP_BARO_ENABLED + if ((_baro_probe_ext.get() & PROBE_MSP) && msp_instance_mask == 0) { + // allow for late addition of MSP sensor + msp_instance_mask |= 1; + } + for (uint8_t i=0; i<8; i++) { + if (msp_instance_mask & (1U< 7) { + return; + } + if (!init_done) { + msp_instance_mask |= 1U<handle_msp(pkt); + } + } +} +#endif + + namespace AP { AP_Baro &baro() diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 5b3d74c679..12934fbf4e 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -4,6 +4,11 @@ #include #include #include +#include + +#ifndef HAL_MSP_BARO_ENABLED +#define HAL_MSP_BARO_ENABLED HAL_MSP_ENABLED && !HAL_MINIMIZE_FEATURES +#endif // maximum number of sensor instances #define BARO_MAX_INSTANCES 3 @@ -188,7 +193,11 @@ public: HAL_Semaphore &get_semaphore(void) { return _rsem; } - + +#if HAL_MSP_BARO_ENABLED + void handle_msp(const MSP::msp_baro_data_message_t &pkt); +#endif + private: // singleton static AP_Baro *_singleton; @@ -205,6 +214,12 @@ private: uint32_t _log_baro_bit = -1; + bool init_done; + +#if HAL_MSP_BARO_ENABLED + uint8_t msp_instance_mask; +#endif + // bitmask values for GND_PROBE_EXT enum { PROBE_BMP085=(1<<0), @@ -218,7 +233,8 @@ private: PROBE_KELLER=(1<<8), PROBE_MS5837=(1<<9), PROBE_BMP388=(1<<10), - PROBE_SPL06=(1<<11), + PROBE_SPL06 =(1<<11), + PROBE_MSP =(1<<12), }; struct sensor { diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index c35c8757a9..df00d59146 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -24,6 +24,10 @@ public: bool pressure_ok(float press); uint32_t get_error_count() const { return _error_count; } +#if HAL_MSP_BARO_ENABLED + virtual void handle_msp(const MSP::msp_baro_data_message_t &pkt) {} +#endif + /* device driver IDs. These are used to fill in the devtype field of the device ID, which shows up as GND_BARO_ID* parameters to @@ -43,6 +47,7 @@ public: DEVTYPE_BARO_MS5611 = 0x0B, DEVTYPE_BARO_SPL06 = 0x0C, DEVTYPE_BARO_UAVCAN = 0x0D, + DEVTYPE_BARO_MSP = 0x0E, }; protected: diff --git a/libraries/AP_Baro/AP_Baro_MSP.cpp b/libraries/AP_Baro/AP_Baro_MSP.cpp new file mode 100644 index 0000000000..128995148a --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_MSP.cpp @@ -0,0 +1,36 @@ +#include "AP_Baro_MSP.h" + +#if HAL_MSP_BARO_ENABLED + +AP_Baro_MSP::AP_Baro_MSP(AP_Baro &baro, uint8_t _msp_instance) : + AP_Baro_Backend(baro) +{ + msp_instance = _msp_instance; + instance = _frontend.register_sensor(); + set_bus_id(instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_MSP,0,msp_instance,0)); +} + +// Read the sensor +void AP_Baro_MSP::update(void) +{ + if (count) { + WITH_SEMAPHORE(_sem); + _copy_to_frontend(instance, sum_pressure/count, sum_temp/count); + sum_pressure = sum_temp = 0; + count = 0; + } +} + +void AP_Baro_MSP::handle_msp(const MSP::msp_baro_data_message_t &pkt) +{ + if (pkt.instance != msp_instance) { + // not for us + return; + } + WITH_SEMAPHORE(_sem); + sum_pressure += pkt.pressure_pa; + sum_temp += pkt.temp*0.01; + count++; +} + +#endif // HAL_MSP_BARO_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_MSP.h b/libraries/AP_Baro/AP_Baro_MSP.h new file mode 100644 index 0000000000..1ddba3ece4 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_MSP.h @@ -0,0 +1,27 @@ +/* + MSP backend barometer + */ +#pragma once + +#include "AP_Baro_Backend.h" + +#define MOVING_AVERAGE_WEIGHT 0.20f // a 5 samples moving average + +#if HAL_MSP_BARO_ENABLED + +class AP_Baro_MSP : public AP_Baro_Backend +{ +public: + AP_Baro_MSP(AP_Baro &baro, uint8_t msp_instance); + void update(void) override; + void handle_msp(const MSP::msp_baro_data_message_t &pkt) override; + +private: + uint8_t instance; + uint8_t msp_instance; + float sum_pressure; + float sum_temp; + uint16_t count; +}; + +#endif // HAL_MSP_BARO_ENABLED