diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index c0e2306221da7416ced5f3000523a94bfd29fe94..0b8e3141045ac43abb72b1eea95ffc3cc43dd436 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -384,25 +384,6 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) // Create a new UAS object uas = QGCMAVLinkUASFactory::createUAS(this, link, message.sysid, &heartbeat); - //This is the first instance of this UAS being known, request update rates: - - mavlink_message_t newmsg; - mavlink_msg_request_data_stream_pack(1,255,&newmsg,message.sysid,message.compid,2,2,1); // Extended Status, 2hz - sendMessage(link,newmsg); - mavlink_msg_request_data_stream_pack(1,255,&newmsg,message.sysid,message.compid,6,3,1); //Position 3hz - sendMessage(link,newmsg); - mavlink_msg_request_data_stream_pack(1,255,&newmsg,message.sysid,message.compid,10,10,1); //Extra 1 10hz - sendMessage(link,newmsg); - mavlink_msg_request_data_stream_pack(1,255,&newmsg,message.sysid,message.compid,11,10,1); //Extra 2 10hz - sendMessage(link,newmsg); - mavlink_msg_request_data_stream_pack(1,255,&newmsg,message.sysid,message.compid,12,2,1); //Extra 3 2hz - sendMessage(link,newmsg); - mavlink_msg_request_data_stream_pack(1,255,&newmsg,message.sysid,message.compid,1,2,1); //Raw Sensors 2hz - sendMessage(link,newmsg); - mavlink_msg_request_data_stream_pack(1,255,&newmsg,message.sysid,message.compid,3,2,1); //RC Channels 2hz - sendMessage(link,newmsg); - - } // Only count message if UAS exists for this message diff --git a/src/uas/ArduPilotMegaMAV.cc b/src/uas/ArduPilotMegaMAV.cc index 61dfced8b1a653349db4646f91eebcea8ee3307c..9dedbeaf6ad07a590aef376d952238ee64dfac8a 100644 --- a/src/uas/ArduPilotMegaMAV.cc +++ b/src/uas/ArduPilotMegaMAV.cc @@ -31,8 +31,16 @@ ArduPilotMegaMAV::ArduPilotMegaMAV(MAVLinkProtocol* mavlink, int id) : UAS(mavlink, id)//, // place other initializers here { + //This does not seem to work. Manually request each stream type at a specified rate. // Ask for all streams at 4 Hz - enableAllDataTransmission(4); + //enableAllDataTransmission(4); + enableExtendedSystemStatusTransmission(2); + enablePositionTransmission(3); + enableExtra1Transmission(10); + enableExtra2Transmission(10); + enableExtra3Transmission(2); + enableRawSensorDataTransmission(2); + enableRCChannelDataTransmission(2); } /**