mirror of https://github.com/ArduPilot/ardupilot
55 lines
1.8 KiB
C++
55 lines
1.8 KiB
C++
|
#include "AP_Periph.h"
|
||
|
|
|
||
|
|
#ifdef HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
|
||
|
|
|
||
|
|
#include <dronecan_msgs.h>
|
||
|
|
|
||
|
|
// Send temperature message occasionally
|
||
|
|
void AP_Periph_FW::temperature_sensor_update(void)
|
||
|
|
{
|
||
|
|
if (g.temperature_msg_rate <= 0) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
const uint32_t now_ms = AP_HAL::millis();
|
||
|
|
if (now_ms - temperature_last_send_ms < (1000U / g.temperature_msg_rate)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
temperature_last_send_ms = now_ms;
|
||
|
|
|
||
|
|
{
|
||
|
|
const uint8_t num_sensors = temperature_sensor.num_instances();
|
||
|
|
for (uint8_t i = 0; i < num_sensors; i++) {
|
||
|
|
// Send each sensor in turn
|
||
|
|
const uint8_t index = (temperature_last_sent_index + 1 + i) % num_sensors;
|
||
|
|
|
||
|
|
float temp_deg = 0.0;
|
||
|
|
if ((temperature_sensor.get_source(index) != AP_TemperatureSensor_Params::Source::DroneCAN) ||
|
||
|
|
!temperature_sensor.get_temperature(temp_deg, index)) {
|
||
|
|
// not configured to send or Unhealthy
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
|
||
|
|
uavcan_equipment_device_Temperature pkt {};
|
||
|
|
pkt.temperature = C_TO_KELVIN(temp_deg);
|
||
|
|
|
||
|
|
// Use source ID from temperature lib
|
||
|
|
pkt.device_id = temperature_sensor.get_source_id(index);
|
||
|
|
|
||
|
|
uint8_t buffer[UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_MAX_SIZE] {};
|
||
|
|
const uint16_t total_size = uavcan_equipment_device_Temperature_encode(&pkt, buffer, !canfdout());
|
||
|
|
|
||
|
|
canard_broadcast(UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_SIGNATURE,
|
||
|
|
UAVCAN_EQUIPMENT_DEVICE_TEMPERATURE_ID,
|
||
|
|
CANARD_TRANSFER_PRIORITY_LOW,
|
||
|
|
&buffer[0],
|
||
|
|
total_size);
|
||
|
|
|
||
|
|
temperature_last_sent_index = index;
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif // HAL_PERIPH_ENABLE_DEVICE_TEMPERATURE
|