Fix MAVLink not responding regression on TELEM2

This commit is contained in:
Lorenz Meier
2016-04-10 12:11:26 -07:00
parent d2c98a98e9
commit d97f32cca1
2 changed files with 3 additions and 7 deletions

View File

@@ -1758,9 +1758,7 @@ Mavlink::task_main(int argc, char *argv[])
mavlink_update_system();
/* start the MAVLink receiver */
if (_mode != MAVLINK_MODE_OSD) {
MavlinkReceiver::receive_start(&_receive_thread, this);
}
MavlinkReceiver::receive_start(&_receive_thread, this);
MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
uint64_t param_time = 0;
@@ -2125,9 +2123,7 @@ Mavlink::task_main(int argc, char *argv[])
}
/* first wait for threads to complete before tearing down anything */
if (_mode != MAVLINK_MODE_OSD) {
pthread_join(_receive_thread, NULL);
}
pthread_join(_receive_thread, NULL);
delete _subscribe_to_stream;
_subscribe_to_stream = nullptr;