mirror of https://github.com/ArduPilot/ardupilot
86 lines
2.3 KiB
C++
86 lines
2.3 KiB
C++
|
#include "AP_Periph.h"
|
||
|
|
|
||
|
|
#ifdef HAL_PERIPH_ENABLE_AIRSPEED
|
||
|
|
|
||
|
|
/*
|
||
|
|
airspeed support
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include <dronecan_msgs.h>
|
||
|
|
|
||
|
|
#ifndef AP_PERIPH_PROBE_CONTINUOUS
|
||
|
|
#define AP_PERIPH_PROBE_CONTINUOUS 0
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/*
|
||
|
|
update CAN airspeed
|
||
|
|
*/
|
||
|
|
void AP_Periph_FW::can_airspeed_update(void)
|
||
|
|
{
|
||
|
|
if (!airspeed.enabled()) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
#if AP_PERIPH_PROBE_CONTINUOUS
|
||
|
|
if (!airspeed.healthy()) {
|
||
|
|
uint32_t now = AP_HAL::millis();
|
||
|
|
static uint32_t last_probe_ms;
|
||
|
|
if (now - last_probe_ms >= 1000) {
|
||
|
|
last_probe_ms = now;
|
||
|
|
airspeed.allocate();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
uint32_t now = AP_HAL::millis();
|
||
|
|
if (now - last_airspeed_update_ms < 50) {
|
||
|
|
// max 20Hz data
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
last_airspeed_update_ms = now;
|
||
|
|
airspeed.update();
|
||
|
|
if (!airspeed.healthy()) {
|
||
|
|
// don't send any data
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
const float press = airspeed.get_corrected_pressure();
|
||
|
|
float temp;
|
||
|
|
if (!airspeed.get_temperature(temp)) {
|
||
|
|
temp = nanf("");
|
||
|
|
} else {
|
||
|
|
temp = C_TO_KELVIN(temp);
|
||
|
|
}
|
||
|
|
|
||
|
|
uavcan_equipment_air_data_RawAirData pkt {};
|
||
|
|
|
||
|
|
// unfilled elements are NaN
|
||
|
|
pkt.static_pressure = nanf("");
|
||
|
|
pkt.static_pressure_sensor_temperature = nanf("");
|
||
|
|
pkt.differential_pressure_sensor_temperature = nanf("");
|
||
|
|
pkt.pitot_temperature = nanf("");
|
||
|
|
|
||
|
|
// populate the elements we have
|
||
|
|
pkt.differential_pressure = press;
|
||
|
|
pkt.static_air_temperature = temp;
|
||
|
|
|
||
|
|
// if a Pitot tube temperature sensor is available, use it
|
||
|
|
#if AP_TEMPERATURE_SENSOR_ENABLED
|
||
|
|
for (uint8_t i=0; i<temperature_sensor.num_instances(); i++) {
|
||
|
|
float temp_pitot;
|
||
|
|
if (temperature_sensor.get_source(i) == AP_TemperatureSensor_Params::Source::Pitot_tube && temperature_sensor.get_temperature(temp_pitot, i)) {
|
||
|
|
pkt.pitot_temperature = C_TO_KELVIN(temp_pitot);
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE] {};
|
||
|
|
uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout());
|
||
|
|
|
||
|
|
canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE,
|
||
|
|
UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID,
|
||
|
|
CANARD_TRANSFER_PRIORITY_LOW,
|
||
|
|
&buffer[0],
|
||
|
|
total_size);
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif // HAL_PERIPH_ENABLE_AIRSPEED
|