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/temperature.cpp
Views: 1798
1
#include "AP_Periph.h"

2


3
#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE

4


5
#include <dronecan_msgs.h>

6


7
// Send temperature message occasionally

8
void AP_Periph_FW::temperature_sensor_update(void)

9
{

10
    if (g.temperature_msg_rate <= 0) {

11
        return;

12
    }

13


14
    const uint32_t now_ms = AP_HAL::millis();

15
    if (now_ms - temperature_last_send_ms < (1000U / g.temperature_msg_rate)) {

16
        return;

17
    }

18
    temperature_last_send_ms = now_ms;

19


20
    {

21
        const uint8_t num_sensors = temperature_sensor.num_instances();

22
        for (uint8_t i = 0; i < num_sensors; i++) {

23
            // Send each sensor in turn

24
            const uint8_t index = (temperature_last_sent_index + 1 + i) % num_sensors;

25


26
            float temp_deg = 0.0;

27
            if ((temperature_sensor.get_source(index) != AP_TemperatureSensor_Params::Source::DroneCAN) ||

28
                !temperature_sensor.get_temperature(temp_deg, index)) {

29
                // not configured to send or Unhealthy

30
                continue;

31
            }

32


33
            uavcan_equipment_device_Temperature pkt {};

34
            pkt.temperature = C_TO_KELVIN(temp_deg);

35


36
            // Use source ID from temperature lib

37
            pkt.device_id = temperature_sensor.get_source_id(index);

38


39
            uint8_t buffer[UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE];

40
            const uint16_t total_size = uavcan_equipment_device_Temperature_encode(&pkt, buffer, !canfdout());

41


42
            canard_broadcast(UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE,

43
                             UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID,

44
                             CANARD_TRANSFER_PRIORITY_LOW,

45
                             &buffer[0],

46
                             total_size);

47


48
            temperature_last_sent_index = index;

49
            break;

50
        }

51
    }

52
}

53


54
#endif // HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE

55


56