Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
Ardupilot
GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/AP_Periph/airspeed.cpp
9367 views
1
#include "AP_Periph.h"

2


3
#if AP_PERIPH_AIRSPEED_ENABLED

4


5
/*

6
  airspeed support

7
 */

8


9
#include <dronecan_msgs.h>

10


11
#ifndef AP_PERIPH_PROBE_CONTINUOUS

12
#define AP_PERIPH_PROBE_CONTINUOUS 0

13
#endif

14


15
extern const AP_HAL::HAL &hal;

16


17
/*

18
  update CAN airspeed

19
 */

20
void AP_Periph_FW::can_airspeed_update(void)

21
{

22
    if (!airspeed.enabled()) {

23
        return;

24
    }

25
#if AP_PERIPH_PROBE_CONTINUOUS

26
    if (option_is_set(PeriphOptions::PROBE_CONTINUOUS) && !hal.util->get_soft_armed() && !airspeed.healthy()) {

27
        uint32_t now = AP_HAL::millis();

28
        static uint32_t last_probe_ms;

29
        if (now - last_probe_ms >= 1000) {

30
            last_probe_ms = now;

31
            airspeed.allocate();

32
        }

33
    }

34
#endif

35
    uint32_t now = AP_HAL::millis();

36
    if (now - last_airspeed_update_ms < 50) {

37
        // max 20Hz data

38
        return;

39
    }

40
    last_airspeed_update_ms = now;

41
    airspeed.update();

42
    if (!airspeed.healthy()) {

43
        // don't send any data

44
        return;

45
    }

46
    const float press = airspeed.get_corrected_pressure();

47
    float temp;

48
    if (!airspeed.get_temperature(temp)) {

49
        temp = nanf("");

50
    } else {

51
        temp = C_TO_KELVIN(temp);

52
    }

53


54
    uavcan_equipment_air_data_RawAirData pkt {};

55


56
    // unfilled elements are NaN

57
    pkt.static_pressure = nanf("");

58
    pkt.static_pressure_sensor_temperature = nanf("");

59
    pkt.differential_pressure_sensor_temperature = nanf("");

60
    pkt.pitot_temperature = nanf("");

61


62
    // populate the elements we have

63
    pkt.differential_pressure = press;

64
    pkt.static_air_temperature = temp;

65


66
    // if a Pitot tube temperature sensor is available, use it

67
#if AP_TEMPERATURE_SENSOR_ENABLED

68
    for (uint8_t i=0; i<temperature_sensor.num_instances(); i++) {

69
        float temp_pitot;

70
        if (temperature_sensor.get_source(i) == AP_TemperatureSensor_Params::Source::Pitot_tube && temperature_sensor.get_temperature(temp_pitot, i)) {

71
            pkt.pitot_temperature = C_TO_KELVIN(temp_pitot);

72
            break;

73
        }

74
    }

75
#endif

76


77
    uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE];

78
    uint16_t total_size = uavcan_equipment_air_data_RawAirData_encode(&pkt, buffer, !periph.canfdout());

79


80
    canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE,

81
                    UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID,

82
                    CANARD_TRANSFER_PRIORITY_LOW,

83
                    &buffer[0],

84
                    total_size);

85
}

86


87
#endif // AP_PERIPH_AIRSPEED_ENABLED

88


89