diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 870c41735f..399d8ef69f 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -34,6 +34,9 @@ #include "AP_Baro_PX4.h" #include "AP_Baro_qflight.h" #include "AP_Baro_QURT.h" +#if HAL_WITH_UAVCAN +#include "AP_Baro_UAVCAN.h" +#endif #define C_TO_KELVIN 273.15f // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers @@ -474,6 +477,19 @@ void AP_Baro::init(void) #endif } +#if HAL_WITH_UAVCAN + // If there is place left - allocate one UAVCAN based baro + if ((AP_BoardConfig::get_can_enable() != 0) && (hal.can_mgr != nullptr)) + { + if(_num_drivers < BARO_MAX_DRIVERS && _num_sensors < BARO_MAX_INSTANCES) + { + printf("Creating AP_Baro_UAVCAN\n\r"); + drivers[_num_drivers] = new AP_Baro_UAVCAN(*this); + _num_drivers++; + } + } +#endif + if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == nullptr) { AP_HAL::panic("Baro: unable to initialise driver"); } diff --git a/libraries/AP_Baro/AP_Baro_Backend.h b/libraries/AP_Baro/AP_Baro_Backend.h index 758f771631..c5aa5df2e7 100644 --- a/libraries/AP_Baro/AP_Baro_Backend.h +++ b/libraries/AP_Baro/AP_Baro_Backend.h @@ -6,6 +6,7 @@ class AP_Baro_Backend { public: AP_Baro_Backend(AP_Baro &baro); + virtual ~AP_Baro_Backend(void) {}; // each driver must provide an update method to copy accumulated // data to the frontend @@ -16,6 +17,9 @@ public: // trigger them to read the sensor virtual void accumulate(void) {} + // callback for UAVCAN messages + virtual void handle_baro_msg(float pressure, float temperature) {} + protected: // reference to frontend object AP_Baro &_frontend; diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.cpp b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp new file mode 100644 index 0000000000..7a56fb4503 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.cpp @@ -0,0 +1,74 @@ +#include + +#if HAL_WITH_UAVCAN + +#include "AP_Baro_UAVCAN.h" +#include + +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#define debug_baro_uavcan(level, fmt, args...) do { if ((level) <= AP_BoardConfig::get_can_debug()) { hal.console->printf(fmt, ##args); }} while (0) + +// There is limitation to use only one UAVCAN barometer now. + +/* + constructor - registers instance at top Baro driver + */ +AP_Baro_UAVCAN::AP_Baro_UAVCAN(AP_Baro &baro) : + AP_Baro_Backend(baro) +{ + _instance = _frontend.register_sensor(); + if (hal.can_mgr != nullptr) { + AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); + if (ap_uavcan != nullptr) { + // Give time to receive some packets from CAN if baro sensor is present + // This way it will get calibrated correctly + hal.scheduler->delay(1000); + ap_uavcan->register_baro_listener(this, 1); + + debug_baro_uavcan(2, "AP_Baro_UAVCAN loaded\n\r"); + } + } + + _sem_baro = hal.util->new_semaphore(); +} + +AP_Baro_UAVCAN::~AP_Baro_UAVCAN() +{ + if (hal.can_mgr != nullptr) { + AP_UAVCAN *ap_uavcan = hal.can_mgr->get_UAVCAN(); + if (ap_uavcan != nullptr) { + ap_uavcan->remove_baro_listener(this); + debug_baro_uavcan(2, "AP_Baro_UAVCAN destructed\n\r"); + } + } +} + +// Read the sensor +void AP_Baro_UAVCAN::update(void) +{ + if (_sem_baro->take(0)) { + _copy_to_frontend(_instance, _pressure, _temperature); + + _frontend.set_external_temperature(_temperature); + _sem_baro->give(); + } +} + +void AP_Baro_UAVCAN::handle_baro_msg(float pressure, float temperature) +{ + if (_sem_baro->take(0)) { + _pressure = pressure; + _temperature = temperature - 273.15f; + _last_timestamp = AP_HAL::micros64(); + _sem_baro->give(); + } +} + +#endif // HAL_WITH_UAVCAN + diff --git a/libraries/AP_Baro/AP_Baro_UAVCAN.h b/libraries/AP_Baro/AP_Baro_UAVCAN.h new file mode 100644 index 0000000000..4d81897f9f --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_UAVCAN.h @@ -0,0 +1,23 @@ +#pragma once + +#include "AP_Baro_Backend.h" +#include + +class AP_Baro_UAVCAN : public AP_Baro_Backend { +public: + AP_Baro_UAVCAN(AP_Baro &); + ~AP_Baro_UAVCAN() override; + + void update() override; + + // This method is called from UAVCAN thread + virtual void handle_baro_msg(float pressure, float temperature) override; + +private: + uint8_t _instance; + float _pressure; + float _temperature; + uint64_t _last_timestamp; + + AP_HAL::Semaphore *_sem_baro; +};