From 0d90e0377ae70aa4802b032b5a455aa31f7c9f0f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 4 Jan 2023 22:25:39 +1100 Subject: [PATCH] AP_Airspeed: replace libuavcan with libcanard based driver --- libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp | 50 ++++++-------------- libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h | 7 +-- 2 files changed, 17 insertions(+), 40 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp index deea83c2aa..d905dc132b 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp @@ -4,22 +4,12 @@ #include #include +#include -#include -#if AP_AIRSPEED_HYGROMETER_ENABLE -#include -#endif extern const AP_HAL::HAL& hal; #define LOG_TAG "AirSpeed" -// Frontend Registry Binders -UC_REGISTRY_BINDER(AirspeedCb, uavcan::equipment::air_data::RawAirData); - -#if AP_AIRSPEED_HYGROMETER_ENABLE -UC_REGISTRY_BINDER(HygrometerCb, dronecan::sensors::hygrometer::Hygrometer); -#endif - AP_Airspeed_UAVCAN::DetectedModules AP_Airspeed_UAVCAN::_detected_modules[]; HAL_Semaphore AP_Airspeed_UAVCAN::_sem_registry; @@ -34,22 +24,13 @@ void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) return; } - auto* node = ap_uavcan->get_node(); - - uavcan::Subscriber *airspeed_listener; - airspeed_listener = new uavcan::Subscriber(*node); - - const int airspeed_listener_res = airspeed_listener->start(AirspeedCb(ap_uavcan, &handle_airspeed)); - if (airspeed_listener_res < 0) { - AP_HAL::panic("DroneCAN Airspeed subscriber error \n"); + if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_airspeed, ap_uavcan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("airspeed_sub"); } #if AP_AIRSPEED_HYGROMETER_ENABLE - uavcan::Subscriber *hygrometer_listener; - hygrometer_listener = new uavcan::Subscriber(*node); - const int hygrometer_listener_res = hygrometer_listener->start(HygrometerCb(ap_uavcan, &handle_hygrometer)); - if (hygrometer_listener_res < 0) { - AP_HAL::panic("DroneCAN Hygrometer subscriber error\n"); + if (Canard::allocate_sub_arg_callback(ap_uavcan, &handle_hygrometer, ap_uavcan->get_driver_index()) == nullptr) { + AP_BoardConfig::allocation_error("hygrometer_sub"); } #endif } @@ -128,18 +109,18 @@ AP_Airspeed_UAVCAN* AP_Airspeed_UAVCAN::get_uavcan_backend(AP_UAVCAN* ap_uavcan, return nullptr; } -void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AirspeedCb &cb) +void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_RawAirData &msg) { WITH_SEMAPHORE(_sem_registry); - AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); if (driver != nullptr) { WITH_SEMAPHORE(driver->_sem_airspeed); - driver->_pressure = cb.msg->differential_pressure; - if (!isnan(cb.msg->static_air_temperature) && - cb.msg->static_air_temperature > 0) { - driver->_temperature = KELVIN_TO_C(cb.msg->static_air_temperature); + driver->_pressure = msg.differential_pressure; + if (!isnan(msg.static_air_temperature) && + msg.static_air_temperature > 0) { + driver->_temperature = KELVIN_TO_C(msg.static_air_temperature); driver->_have_temperature = true; } driver->_last_sample_time_ms = AP_HAL::millis(); @@ -147,16 +128,16 @@ void AP_Airspeed_UAVCAN::handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, } #if AP_AIRSPEED_HYGROMETER_ENABLE -void AP_Airspeed_UAVCAN::handle_hygrometer(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HygrometerCb &cb) +void AP_Airspeed_UAVCAN::handle_hygrometer(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const dronecan_sensors_hygrometer_Hygrometer &msg) { WITH_SEMAPHORE(_sem_registry); - AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id); + AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, transfer.source_node_id); if (driver != nullptr) { WITH_SEMAPHORE(driver->_sem_airspeed); - driver->_hygrometer.temperature = KELVIN_TO_C(cb.msg->temperature); - driver->_hygrometer.humidity = cb.msg->humidity; + driver->_hygrometer.temperature = KELVIN_TO_C(msg.temperature); + driver->_hygrometer.humidity = msg.humidity; driver->_hygrometer.last_sample_ms = AP_HAL::millis(); } } @@ -213,5 +194,4 @@ bool AP_Airspeed_UAVCAN::get_hygrometer(uint32_t &last_sample_ms, float &tempera return true; } #endif // AP_AIRSPEED_HYGROMETER_ENABLE - #endif // AP_AIRSPEED_UAVCAN_ENABLED diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h index ae67d77c61..83d99568b7 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h +++ b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h @@ -12,9 +12,6 @@ #include -class AirspeedCb; -class HygrometerCb; - class AP_Airspeed_UAVCAN : public AP_Airspeed_Backend { public: AP_Airspeed_UAVCAN(AP_Airspeed &_frontend, uint8_t _instance); @@ -38,8 +35,8 @@ public: private: - static void handle_airspeed(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AirspeedCb &cb); - static void handle_hygrometer(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HygrometerCb &cb); + static void handle_airspeed(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const uavcan_equipment_air_data_RawAirData &msg); + static void handle_hygrometer(AP_UAVCAN *ap_uavcan, const CanardRxTransfer& transfer, const dronecan_sensors_hygrometer_Hygrometer &msg); static AP_Airspeed_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);