From 48b4ca6962b4140429bfbd4767639d4460febc3d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 17 Oct 2022 19:35:08 +1100 Subject: [PATCH] AP_Airspeed: support DroneCAN airspeed with hygrometer data some DroneCAN airspeed sensors can send hygrometer data, for when they have de-icing support --- libraries/AP_Airspeed/AP_Airspeed.cpp | 32 +++++++++++- libraries/AP_Airspeed/AP_Airspeed.h | 29 ++++------- libraries/AP_Airspeed/AP_Airspeed_Backend.h | 5 ++ libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp | 54 ++++++++++++++++++-- libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h | 14 +++++ libraries/AP_Airspeed/AP_Airspeed_config.h | 25 +++++++++ 6 files changed, 136 insertions(+), 23 deletions(-) create mode 100644 libraries/AP_Airspeed/AP_Airspeed_config.h diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 388873bbcf..b05bba0808 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -726,7 +726,7 @@ void AP_Airspeed::Log_Airspeed() { const uint64_t now = AP_HAL::micros64(); for (uint8_t i=0; iget_hygrometer(hygrometer.sample_ms, hygrometer.temperature, hygrometer.humidity) && + hygrometer.sample_ms != state[i].last_hygrometer_log_ms) { + AP::logger().WriteStreaming("HYGR", + "TimeUS,Humidity,Temp", + "s%O", + "F--", + "Qff", + AP_HAL::micros64(), + hygrometer.humidity, + hygrometer.temperature); + state[i].last_hygrometer_log_ms = hygrometer.sample_ms; + } +#endif } } @@ -836,6 +856,16 @@ float AP_Airspeed::get_corrected_pressure(uint8_t i) const { return state[i].corrected_pressure; } +#if AP_AIRSPEED_HYGROMETER_ENABLE +bool AP_Airspeed::get_hygrometer(uint8_t i, uint32_t &last_sample_ms, float &temperature, float &humidity) const +{ + if (!enabled(i) || sensor[i] == nullptr) { + return false; + } + return sensor[i]->get_hygrometer(last_sample_ms, temperature, humidity); +} +#endif // AP_AIRSPEED_HYGROMETER_ENABLE + #else // build type is not appropriate; provide a dummy implementation: const AP_Param::GroupInfo AP_Airspeed::var_info[] = { AP_GROUPEND }; diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index befefdbdc1..b860cfe137 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -1,29 +1,12 @@ #pragma once -#include -#include +#include "AP_Airspeed_config.h" + #include #include -#include - -#ifndef AP_AIRSPEED_ENABLED -#define AP_AIRSPEED_ENABLED 1 -#endif - -#ifndef AP_AIRSPEED_MSP_ENABLED -#define AP_AIRSPEED_MSP_ENABLED (AP_AIRSPEED_ENABLED && HAL_MSP_SENSORS_ENABLED) -#endif class AP_Airspeed_Backend; -#ifndef AIRSPEED_MAX_SENSORS -#define AIRSPEED_MAX_SENSORS 2 -#endif - -#ifndef AP_AIRSPEED_AUTOCAL_ENABLE -#define AP_AIRSPEED_AUTOCAL_ENABLE AP_AIRSPEED_ENABLED -#endif - class Airspeed_Calibration { public: friend class AP_Airspeed; @@ -126,6 +109,10 @@ public: uint32_t last_update_ms(uint8_t i) const { return state[i].last_update_ms; } uint32_t last_update_ms(void) const { return last_update_ms(primary); } +#if AP_AIRSPEED_HYGROMETER_ENABLE + bool get_hygrometer(uint8_t i, uint32_t &last_sample_ms, float &temperature, float &humidity) const; +#endif + static const struct AP_Param::GroupInfo var_info[]; enum pitot_tube_order { PITOT_TUBE_ORDER_POSITIVE = 0, @@ -231,6 +218,10 @@ private: int8_t param_use_backup; uint32_t last_warn_ms; } failures; + +#if AP_AIRSPEED_HYGROMETER_ENABLE + uint32_t last_hygrometer_log_ms; +#endif } state[AIRSPEED_MAX_SENSORS]; bool calibration_enabled; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Backend.h b/libraries/AP_Airspeed/AP_Airspeed_Backend.h index 2d369cd8dd..4bacf9adbd 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Backend.h +++ b/libraries/AP_Airspeed/AP_Airspeed_Backend.h @@ -45,6 +45,11 @@ public: virtual void handle_msp(const MSP::msp_airspeed_data_message_t &pkt) {} +#if AP_AIRSPEED_HYGROMETER_ENABLE + // optional hygrometer support + virtual bool get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) { return false; } +#endif + protected: int8_t get_pin(void) const; float get_psi_range(void) const; diff --git a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp index 2c232648d3..deea83c2aa 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp @@ -6,14 +6,20 @@ #include #include - +#if AP_AIRSPEED_HYGROMETER_ENABLE +#include +#endif extern const AP_HAL::HAL& hal; #define LOG_TAG "AirSpeed" -// UAVCAN Frontend Registry Binder +// 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; @@ -35,8 +41,17 @@ void AP_Airspeed_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan) const int airspeed_listener_res = airspeed_listener->start(AirspeedCb(ap_uavcan, &handle_airspeed)); if (airspeed_listener_res < 0) { - AP_HAL::panic("UAVCAN Airspeed subscriber start problem\n"); + AP_HAL::panic("DroneCAN Airspeed subscriber error \n"); } + +#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"); + } +#endif } AP_Airspeed_Backend* AP_Airspeed_UAVCAN::probe(AP_Airspeed &_frontend, uint8_t _instance, uint32_t previous_devid) @@ -131,6 +146,22 @@ 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) +{ + WITH_SEMAPHORE(_sem_registry); + + AP_Airspeed_UAVCAN* driver = get_uavcan_backend(ap_uavcan, 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.last_sample_ms = AP_HAL::millis(); + } +} +#endif // AP_AIRSPEED_HYGROMETER_ENABLE + bool AP_Airspeed_UAVCAN::init() { // always returns true @@ -166,4 +197,21 @@ bool AP_Airspeed_UAVCAN::get_temperature(float &temperature) return true; } +#if AP_AIRSPEED_HYGROMETER_ENABLE +/* + return hygrometer data if available + */ +bool AP_Airspeed_UAVCAN::get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) +{ + if (_hygrometer.last_sample_ms == 0) { + return false; + } + WITH_SEMAPHORE(_sem_airspeed); + last_sample_ms = _hygrometer.last_sample_ms; + temperature = _hygrometer.temperature; + humidity = _hygrometer.humidity; + 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 e4115c32ef..ae67d77c61 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h +++ b/libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h @@ -13,6 +13,7 @@ #include class AirspeedCb; +class HygrometerCb; class AP_Airspeed_UAVCAN : public AP_Airspeed_Backend { public: @@ -26,6 +27,11 @@ public: // temperature not available via analog backend bool get_temperature(float &temperature) override; +#if AP_AIRSPEED_HYGROMETER_ENABLE + // get hygrometer data + bool get_hygrometer(uint32_t &last_sample_ms, float &temperature, float &humidity) override; +#endif + static void subscribe_msgs(AP_UAVCAN* ap_uavcan); static AP_Airspeed_Backend* probe(AP_Airspeed &_fronted, uint8_t _instance, uint32_t previous_devid); @@ -33,6 +39,7 @@ 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 AP_Airspeed_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id); @@ -40,6 +47,13 @@ private: float _temperature; // Celcius uint32_t _last_sample_time_ms; + // hygrometer data + struct { + float temperature; + float humidity; + uint32_t last_sample_ms; + } _hygrometer; + HAL_Semaphore _sem_airspeed; // Module Detection Registry diff --git a/libraries/AP_Airspeed/AP_Airspeed_config.h b/libraries/AP_Airspeed/AP_Airspeed_config.h new file mode 100644 index 0000000000..3df9e79328 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_config.h @@ -0,0 +1,25 @@ +#pragma once + +#include +#include +#include + +#ifndef AP_AIRSPEED_ENABLED +#define AP_AIRSPEED_ENABLED 1 +#endif + +#ifndef AP_AIRSPEED_MSP_ENABLED +#define AP_AIRSPEED_MSP_ENABLED (AP_AIRSPEED_ENABLED && HAL_MSP_SENSORS_ENABLED) +#endif + +#ifndef AIRSPEED_MAX_SENSORS +#define AIRSPEED_MAX_SENSORS 2 +#endif + +#ifndef AP_AIRSPEED_AUTOCAL_ENABLE +#define AP_AIRSPEED_AUTOCAL_ENABLE AP_AIRSPEED_ENABLED +#endif + +#ifndef AP_AIRSPEED_HYGROMETER_ENABLE +#define AP_AIRSPEED_HYGROMETER_ENABLE (AP_AIRSPEED_ENABLED && BOARD_FLASH_SIZE > 1024) +#endif