mirror of
https://github.com/OpenSolo/OpenSolo.git
synced 2025-04-29 22:24:32 +02:00
40 lines
874 B
C++
40 lines
874 B
C++
#include "heart.h"
|
|
|
|
#include "util.h"
|
|
#include "la-log.h"
|
|
|
|
void Heart::idle_10Hz()
|
|
{
|
|
uint64_t now_us = clock_gettime_us(CLOCK_MONOTONIC);
|
|
if (last_heartbeat_time + heartbeat_interval < now_us) {
|
|
last_heartbeat_time = now_us;
|
|
beat();
|
|
}
|
|
}
|
|
|
|
bool Heart::configure(INIReader *config)
|
|
{
|
|
if (!MAVLink_Message_Handler::configure(config)) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void Heart::beat()
|
|
{
|
|
mavlink_message_t msg;
|
|
|
|
uint8_t type = MAV_TYPE_GCS;
|
|
uint8_t autopilot = MAV_AUTOPILOT_INVALID;
|
|
uint8_t base_mode = 0;
|
|
uint32_t custom_mode = 0;
|
|
uint8_t system_status = 0;
|
|
|
|
la_log(LOG_DEBUG, "mh-h: sending heartbeat");
|
|
|
|
mavlink_msg_heartbeat_pack(system_id, component_id, &msg, type, autopilot, base_mode,
|
|
custom_mode, system_status);
|
|
|
|
_mavlink_writer->send_message(msg);
|
|
}
|