fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-13 16:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add support for mavlink message DEBUG

Browse Source
This commit is contained in:
Julien Lecoeur
2017-08-30 14:45:02 +01:00
committed by Nuno Marques
parent b6c30cf9b2
commit eeb966d375
6 changed files with 100 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+25
View File
@@ -138,6 +138,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_collision_report_pub(nullptr),
_control_state_pub(nullptr),
_debug_key_value_pub(nullptr),
_debug_value_pub(nullptr),
_gps_inject_data_pub(nullptr),
_command_ack_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
@@ -312,6 +313,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_named_value_float(msg);
break;
case MAVLINK_MSG_ID_DEBUG:
handle_message_debug(msg);
break;
default:
break;
}
@@ -2384,6 +2389,26 @@ void MavlinkReceiver::handle_message_named_value_float(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_debug(mavlink_message_t *msg)
{
mavlink_debug_t debug_msg;
debug_value_s debug_topic = {};
mavlink_msg_debug_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.timestamp_ms = debug_msg.time_boot_ms;
debug_topic.ind = debug_msg.ind;
debug_topic.value = debug_msg.value;
if (_debug_value_pub == nullptr) {
_debug_value_pub = orb_advertise(ORB_ID(debug_value), &debug_topic);
} else {
orb_publish(ORB_ID(debug_value), _debug_value_pub, &debug_topic);
}
}
/**
* Receive data from UART.
*/