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

Add support for mavlink message DEBUG_FLOAT_ARRAY

Browse Source
This commit is contained in:
Julien Lecoeur
2018-09-27 12:33:12 -04:00
committed by Daniel Agar
parent 7f665a70e7
commit cb3d86a609
6 changed files with 116 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+29
View File
@@ -159,6 +159,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_debug_key_value_pub(nullptr),
_debug_value_pub(nullptr),
_debug_vect_pub(nullptr),
_debug_array_pub(nullptr),
_gps_inject_data_pub(nullptr),
_command_ack_pub(nullptr),
_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
@@ -343,6 +344,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_debug_vect(msg);
break;
case MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY:
handle_message_debug_float_array(msg);
break;
default:
break;
}
@@ -2562,6 +2567,30 @@ void MavlinkReceiver::handle_message_debug_vect(mavlink_message_t *msg)
}
}
void MavlinkReceiver::handle_message_debug_float_array(mavlink_message_t *msg)
{
mavlink_debug_float_array_t debug_msg;
debug_array_s debug_topic = {};
mavlink_msg_debug_float_array_decode(msg, &debug_msg);
debug_topic.timestamp = hrt_absolute_time();
debug_topic.id = debug_msg.array_id;
memcpy(debug_topic.name, debug_msg.name, sizeof(debug_topic.name));
debug_topic.name[sizeof(debug_topic.name) - 1] = '\0'; // enforce null termination
for (size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
debug_topic.data[i] = debug_msg.data[i];
}
if (_debug_array_pub == nullptr) {
_debug_array_pub = orb_advertise(ORB_ID(debug_array), &debug_topic);
} else {
orb_publish(ORB_ID(debug_array), _debug_array_pub, &debug_topic);
}
}
/**
* Receive data from UART/UDP
*/