CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/AP_Periph/airspeed.cpp
Views: 1798
1
#include "AP_Periph.h"

2


3
#ifdef HAL_PERIPH_ENABLE_AIRSPEED

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
/*

16
  update CAN airspeed

17
 */

18
void AP_Periph_FW::can_airspeed_update(void)

19
{

20
    if (!airspeed.enabled()) {

21
        return;

22
    }

23
#if AP_PERIPH_PROBE_CONTINUOUS

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

25
        uint32_t now = AP_HAL::millis();

26
        static uint32_t last_probe_ms;

27
        if (now - last_probe_ms >= 1000) {

28
            last_probe_ms = now;

29
            airspeed.allocate();

30
        }

31
    }

32
#endif

33
    uint32_t now = AP_HAL::millis();

34
    if (now - last_airspeed_update_ms < 50) {

35
        // max 20Hz data

36
        return;

37
    }

38
    last_airspeed_update_ms = now;

39
    airspeed.update();

40
    if (!airspeed.healthy()) {

41
        // don't send any data

42
        return;

43
    }

44
    const float press = airspeed.get_corrected_pressure();

45
    float temp;

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

47
        temp = nanf("");

48
    } else {

49
        temp = C_TO_KELVIN(temp);

50
    }

51


52
    uavcan_equipment_air_data_RawAirData pkt {};

53


54
    // unfilled elements are NaN

55
    pkt.static_pressure = nanf("");

56
    pkt.static_pressure_sensor_temperature = nanf("");

57
    pkt.differential_pressure_sensor_temperature = nanf("");

58
    pkt.pitot_temperature = nanf("");

59


60
    // populate the elements we have

61
    pkt.differential_pressure = press;

62
    pkt.static_air_temperature = temp;

63


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

65
#if AP_TEMPERATURE_SENSOR_ENABLED

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

67
        float temp_pitot;

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

69
            pkt.pitot_temperature = C_TO_KELVIN(temp_pitot);

70
            break;

71
        }

72
    }

73
#endif

74


75
    uint8_t buffer[UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_MAX_SIZE];

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

77


78
    canard_broadcast(UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_SIGNATURE,

79
                    UAVCAN_EQUIPMENT_AIR_DATA_RAWAIRDATA_ID,

80
                    CANARD_TRANSFER_PRIORITY_LOW,

81
                    &buffer[0],

82
                    total_size);

83
}

84


85
#endif // HAL_PERIPH_ENABLE_AIRSPEED

86


87