Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: replace libuavcan with libcanard based driver

Browse Source
This commit is contained in:
bugobliterator 2023-01-04 22:25:39 +11:00 committed by Andrew Tridgell
parent 54df802d59
commit 0d90e0377a
2 changed files with 17 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp
View File

@ -4,22 +4,12 @@
#include <AP_CANManager/AP_CANManager.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <uavcan/equipment/air_data/RawAirData.hpp>
#if AP_AIRSPEED_HYGROMETER_ENABLE
#include <dronecan/sensors/hygrometer/Hygrometer.hpp>
#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<uavcan::equipment::air_data::RawAirData, AirspeedCb> *airspeed_listener;
airspeed_listener = new uavcan::Subscriber<uavcan::equipment::air_data::RawAirData, AirspeedCb>(*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<dronecan::sensors::hygrometer::Hygrometer, HygrometerCb> *hygrometer_listener;
hygrometer_listener = new uavcan::Subscriber<dronecan::sensors::hygrometer::Hygrometer, HygrometerCb>(*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

7
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h
View File

@ -12,9 +12,6 @@
#include <AP_UAVCAN/AP_UAVCAN.h>
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);