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

mavlink: REQUEST_DATA_STREAM hadling implemented

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-07 22:36:28 +04:00
parent e4628fbed6
commit 662a7403b2
5 changed files with 135 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+21
View File
@@ -152,6 +152,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_manual_control(msg);
break;
case MAVLINK_MSG_ID_REQUEST_DATA_STREAM:
handle_message_request_data_stream(msg);
break;
default:
break;
}
@@ -457,6 +461,23 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
}
}
void
MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
{
mavlink_request_data_stream_t req;
mavlink_msg_request_data_stream_decode(msg, &req);
if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) {
float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f;
for (unsigned int i = 0; streams_list[i] != nullptr; i++) {
if (req.req_stream_id == 0 || req.req_stream_id == streams_list[i]->get_id()) {
_mavlink->configure_stream_threadsafe(streams_list[i]->get_name(), rate);
}
}
}
}
void
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{