diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-04-07 22:36:28 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-04-07 22:36:28 +0400 |
commit | 662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf (patch) | |
tree | d4d54523bcd96e76e65026b91e82b7f7736e10dd /src/modules/mavlink/mavlink_receiver.cpp | |
parent | e4628fbed641496704011b0c488a36ed05b45d5f (diff) | |
download | px4-firmware-662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf.tar.gz px4-firmware-662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf.tar.bz2 px4-firmware-662a7403b2ef00018d6c1b38265ec0ba4a9ae6bf.zip |
mavlink: REQUEST_DATA_STREAM hadling implemented
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 21 |
1 files changed, 21 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3ec40ee0a..cf8b23bc3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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; } @@ -458,6 +462,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) { mavlink_hil_sensor_t imu; |