diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 11027c43d7..789371f681 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -47,6 +47,7 @@ #include "AP_Baro_UAVCAN.h" #endif #include "AP_Baro_MSP.h" +#include "AP_Baro_ExternalAHRS.h" #include #include @@ -538,6 +539,12 @@ void AP_Baro::init(void) } #endif +#if HAL_EXTERNAL_AHRS_ENABLED + if (int8_t serial_port = AP::externalAHRS().get_port() >= 0) { + ADD_BACKEND(new AP_Baro_ExternalAHRS(*this, serial_port)); + } +#endif + // macro for use by HAL_INS_PROBE_LIST #define GET_I2C_DEVICE(bus, address) hal.i2c_mgr->get_device(bus, address) @@ -956,6 +963,18 @@ void AP_Baro::handle_msp(const MSP::msp_baro_data_message_t &pkt) } #endif +#if HAL_EXTERNAL_AHRS_ENABLED +/* + handle ExternalAHRS barometer data + */ +void AP_Baro::handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt) +{ + for (uint8_t i=0; i<_num_drivers; i++) { + drivers[i]->handle_external(pkt); + } +} +#endif + namespace AP { AP_Baro &baro() diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 341fa80e56..caab63c901 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -5,6 +5,7 @@ #include #include #include +#include #ifndef HAL_MSP_BARO_ENABLED #define HAL_MSP_BARO_ENABLED HAL_MSP_SENSORS_ENABLED @@ -205,6 +206,10 @@ public: void handle_msp(const MSP::msp_baro_data_message_t &pkt); #endif +#if HAL_EXTERNAL_AHRS_ENABLED + void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt); +#endif + private: // singleton static AP_Baro *_singleton; diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 76ef78cc22..8f8d1acb43 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -28,6 +28,10 @@ public: virtual void handle_msp(const MSP::msp_baro_data_message_t &pkt) {} #endif +#if HAL_EXTERNAL_AHRS_ENABLED + virtual void handle_external(const AP_ExternalAHRS::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 BARO_DEVID* parameters to diff --git a/libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp b/libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp new file mode 100644 index 0000000000..79ca28b810 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_ExternalAHRS.cpp @@ -0,0 +1,35 @@ +#include "AP_Baro_ExternalAHRS.h" + +#if HAL_EXTERNAL_AHRS_ENABLED + +AP_Baro_ExternalAHRS::AP_Baro_ExternalAHRS(AP_Baro &baro, uint8_t port) : + AP_Baro_Backend(baro) +{ + instance = _frontend.register_sensor(); + set_bus_id(instance, AP_HAL::Device::make_bus_id(AP_HAL::Device::BUS_TYPE_SERIAL,port,0,0)); +} + +// Read the sensor +void AP_Baro_ExternalAHRS::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_ExternalAHRS::handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt) +{ + if (pkt.instance != 0) { + // not for us + return; + } + WITH_SEMAPHORE(_sem); + sum_pressure += pkt.pressure_pa; + sum_temp += pkt.temperature; + count++; +} + +#endif // HAL_EXTERNAL_AHRS_ENABLED diff --git a/libraries/AP_Baro/AP_Baro_ExternalAHRS.h b/libraries/AP_Baro/AP_Baro_ExternalAHRS.h new file mode 100644 index 0000000000..3594b71c82 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_ExternalAHRS.h @@ -0,0 +1,25 @@ +/* + ExternalAHRS backend barometer + */ +#pragma once + +#include "AP_Baro_Backend.h" + +#if HAL_EXTERNAL_AHRS_ENABLED + +class AP_Baro_ExternalAHRS : public AP_Baro_Backend +{ +public: + AP_Baro_ExternalAHRS(AP_Baro &baro, uint8_t serial_port); + void update(void) override; + void handle_external(const AP_ExternalAHRS::baro_data_message_t &pkt) override; + +private: + uint8_t instance; + float sum_pressure; + float sum_temp; + uint16_t count; +}; + +#endif // HAL_EXTERNAL_AHRS_ENABLED + diff --git a/libraries/AP_HAL/Device.h b/libraries/AP_HAL/Device.h index f8a658b798..3b6a96a0ae 100644 --- a/libraries/AP_HAL/Device.h +++ b/libraries/AP_HAL/Device.h @@ -33,6 +33,7 @@ public: BUS_TYPE_UAVCAN = 3, BUS_TYPE_SITL = 4, BUS_TYPE_MSP = 5, + BUS_TYPE_SERIAL = 6, }; enum Speed {