From f76040e55407d3526fb7655ec732304c9ba1dc8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:54:33 +0200 Subject: [PATCH] mavlink: Always send heartbeat and do not require both topics to update --- src/modules/mavlink/mavlink_messages.cpp | 26 +++++++++++++----------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4433377c6d..b295bf35f8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -226,19 +226,21 @@ protected: struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; - if (status_sub->update(&status) && pos_sp_triplet_sub->update(&pos_sp_triplet)) { - uint8_t mavlink_state = 0; - uint8_t mavlink_base_mode = 0; - uint32_t mavlink_custom_mode = 0; - get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + /* always send the heartbeat, independent of the update status of the topics */ + (void)status_sub->update(&status); + (void)pos_sp_triplet_sub->update(&pos_sp_triplet); - mavlink_msg_heartbeat_send(_channel, - mavlink_system.type, - MAV_AUTOPILOT_PX4, - mavlink_base_mode, - mavlink_custom_mode, - mavlink_state); - } + uint8_t mavlink_state = 0; + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + + mavlink_msg_heartbeat_send(_channel, + mavlink_system.type, + MAV_AUTOPILOT_PX4, + mavlink_base_mode, + mavlink_custom_mode, + mavlink_state); } };