Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: support DroneCAN airspeed with hygrometer data 

some DroneCAN airspeed sensors can send hygrometer data, for when they
have de-icing support
This commit is contained in:
Andrew Tridgell 2022-10-17 19:35:08 +11:00
parent 883a37ef37
commit 10587fa751
6 changed files with 136 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -726,7 +726,7 @@ void AP_Airspeed::Log_Airspeed()
{
const uint64_t now = AP_HAL::micros64();
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (!enabled(i)) {
if (!enabled(i) || sensor[i] == nullptr) {
continue;
}
float temperature;
@ -749,6 +749,26 @@ void AP_Airspeed::Log_Airspeed()
primary : get_primary()
};
AP::logger().WriteBlock(&pkt, sizeof(pkt));
#if AP_AIRSPEED_HYGROMETER_ENABLE
struct {
uint32_t sample_ms;
float temperature;
float humidity;
} hygrometer;
if (sensor[i]->get_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 };

29
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -1,29 +1,12 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include "AP_Airspeed_config.h"
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_MSP/msp.h>
#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;

5
libraries/AP_Airspeed/AP_Airspeed_Backend.h
View File

@ -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;

54
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.cpp
View File

@ -6,14 +6,20 @@
#include <AP_UAVCAN/AP_UAVCAN.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"
// 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<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");
}
#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

14
libraries/AP_Airspeed/AP_Airspeed_UAVCAN.h
View File

@ -13,6 +13,7 @@
#include <AP_UAVCAN/AP_UAVCAN.h>
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

25
libraries/AP_Airspeed/AP_Airspeed_config.h Normal file
View File

@ -0,0 +1,25 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL_Boards.h>
#include <AP_MSP/msp.h>
#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