From 4ef4be2d7012231a070a65d6783949a339d8e3f7 Mon Sep 17 00:00:00 2001 From: Chris Lovett Date: Tue, 14 Jun 2016 18:07:24 -0700 Subject: [PATCH] MavlinkReceiver::handle_message_request_data_stream walks into deleted memory when you send the "stop" bit on a stream. It also fails to restart the stream because it deletes the stream when you send the stop command, so restart needs to use stream_list to find the stream again. --- src/modules/mavlink/mavlink_receiver.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a769cb0315..9d33711b9e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1376,8 +1376,28 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) LL_FOREACH(_mavlink->get_streams(), stream) { if (req.req_stream_id == stream->get_id()) { _mavlink->configure_stream_threadsafe(stream->get_name(), rate); + // if the command here was to unsubscribe then the stream will be deleted + // in which case we cannot continue this LL_FOREACH loop. We have to return. + // Besides there shouldn't be more than one stream with the same id, right? + return; } } + + // If we get here then the stream may have been deleted, in which case we need to recreate it. + // todo: we could simplify this code by adding a get_id_static (get_name_static) so that + // we could do both subscribe and unsubscribe using the streams_list. + for (unsigned int i = 0; streams_list[i] != nullptr; i++) { + MavlinkStream *new_instance = streams_list[i]->new_instance(_mavlink); + bool found = false; + if (req.req_stream_id == new_instance->get_id()) { + _mavlink->configure_stream_threadsafe(new_instance->get_name(), rate); + found = true; + } + delete new_instance; + if (found) { + break; + } + } } }