diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 066478eba2cd61953e81ae2b8a26a0d18518692d..f08b83f9b10fd9af294a0a705913790817e44cd9 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -175,25 +175,12 @@ void LinkManager::_addLink(LinkInterface* link) } if (!containsLink(link)) { - bool channelSet = false; - - // Find a mavlink channel to use for this link, Channel 0 is reserved for internal use. - for (int i=1; i<32; i++) { - if (!(_mavlinkChannelsUsedBitMask & 1 << i)) { - mavlink_reset_channel_status(i); - link->_setMavlinkChannel(i); - // Start the channel on Mav 1 protocol - mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(i); - mavlinkStatus->flags = mavlink_get_channel_status(i)->flags | MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - qDebug() << "LinkManager mavlinkStatus:channel:flags" << mavlinkStatus << i << mavlinkStatus->flags; - _mavlinkChannelsUsedBitMask |= 1 << i; - channelSet = true; - break; - } - } - - if (!channelSet) { + int mavlinkChannel = _reserveMavlinkChannel(); + if (mavlinkChannel != 0) { + link->_setMavlinkChannel(mavlinkChannel); + } else { qWarning() << "Ran out of mavlink channels"; + return; } _sharedLinks.append(SharedLinkInterfacePointer(link)); @@ -266,7 +253,7 @@ void LinkManager::_deleteLink(LinkInterface* link) } // Free up the mavlink channel associated with this link - _mavlinkChannelsUsedBitMask &= ~(1 << link->mavlinkChannel()); + _freeMavlinkChannel(link->mavlinkChannel()); for (int i=0; i<_sharedLinks.count(); i++) { if (_sharedLinks[i].data() == link) { @@ -947,3 +934,25 @@ void LinkManager::startAutoConnectedLinks(void) createConnectedLink(conf); } } + +int LinkManager::_reserveMavlinkChannel(void) +{ + // Find a mavlink channel to use for this link, Channel 0 is reserved for internal use. + for (int mavlinkChannel=1; mavlinkChannel<32; mavlinkChannel++) { + if (!(_mavlinkChannelsUsedBitMask & 1 << mavlinkChannel)) { + mavlink_reset_channel_status(mavlinkChannel); + // Start the channel on Mav 1 protocol + mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); + mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + _mavlinkChannelsUsedBitMask |= 1 << mavlinkChannel; + return mavlinkChannel; + } + } + + return 0; // All channels reserved +} + +void LinkManager::_freeMavlinkChannel(int channel) +{ + _mavlinkChannelsUsedBitMask &= ~(1 << channel); +} diff --git a/src/comm/LinkManager.h b/src/comm/LinkManager.h index 7a44cb0eb1602e33b9026a27b845d13f9fc3d12e..fc2a9c74e879daea42a5aa730f626b3e3e3b3138 100644 --- a/src/comm/LinkManager.h +++ b/src/comm/LinkManager.h @@ -153,6 +153,13 @@ public: void startAutoConnectedLinks(void); + /// Reserves a mavlink channel for use + /// @return Mavlink channel index, 0 for no channels available + int _reserveMavlinkChannel(void); + + /// Free the specified mavlink channel for re-use + void _freeMavlinkChannel(int channel); + static const char* settingsGroup; signals: diff --git a/src/comm/MockLink.cc b/src/comm/MockLink.cc index e5d4226dccc81cfbd428f35c344baea52c45f1a1..93d14200371b4cb5a60ad50a466eed93fe722e63 100644 --- a/src/comm/MockLink.cc +++ b/src/comm/MockLink.cc @@ -49,6 +49,7 @@ MockLink::MockLink(SharedLinkConfigurationPointer& config) , _missionItemHandler(this, qgcApp()->toolbox()->mavlinkProtocol()) , _name("MockLink") , _connected(false) + , _mavlinkChannel(0) , _vehicleSystemId(_nextVehicleSystemId++) , _vehicleComponentId(200) , _inNSH(false) @@ -100,6 +101,14 @@ bool MockLink::_connect(void) { if (!_connected) { _connected = true; + _mavlinkChannel = qgcApp()->toolbox()->linkManager()->_reserveMavlinkChannel(); + if (_mavlinkChannel == 0) { + qWarning() << "No mavlink channels available"; + return false; + } + // MockLinks use Mavlink 2.0 + mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(_mavlinkChannel); + mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; start(); emit connected(); } @@ -110,6 +119,9 @@ bool MockLink::_connect(void) void MockLink::_disconnect(void) { if (_connected) { + if (_mavlinkChannel != 0) { + qgcApp()->toolbox()->linkManager()->_freeMavlinkChannel(_mavlinkChannel); + } _connected = false; quit(); wait(); @@ -261,7 +273,7 @@ void MockLink::_sendHeartBeat(void) mavlink_msg_heartbeat_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, _vehicleType, // MAV_TYPE _firmwareType, // MAV_AUTOPILOT @@ -278,7 +290,7 @@ void MockLink::_sendVibration(void) mavlink_msg_vibration_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, 0, // time_usec 50.5, // vibration_x, @@ -347,7 +359,7 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes) for (qint64 i=0; iseverity, status->msg); @@ -1114,7 +1126,7 @@ void MockLink::_sendRCChannels(void) mavlink_msg_rc_channels_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, 0, // time_boot_ms 8, // chancount @@ -1156,7 +1168,7 @@ void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request mavlink_message_t msg; mavlink_msg_statustext_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &msg, MAV_SEVERITY_INFO, pCalMessage); @@ -1177,7 +1189,7 @@ void MockLink::_handleLogRequestList(const mavlink_message_t& msg) mavlink_message_t responseMsg; mavlink_msg_log_entry_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &responseMsg, _logDownloadLogId, // log id 1, // num_logs @@ -1233,7 +1245,7 @@ void MockLink::_logDownloadWorker(void) mavlink_message_t responseMsg; mavlink_msg_log_data_pack_chan(_vehicleSystemId, _vehicleComponentId, - mavlinkChannel(), + _mavlinkChannel, &responseMsg, _logDownloadLogId, _logDownloadCurrentOffset, diff --git a/src/comm/MockLink.h b/src/comm/MockLink.h index 15e8a5eebdff9b9e37ced799ab0185bf93c24379..da7fe70557328cb8e2cce7860f90d540cbd732b0 100644 --- a/src/comm/MockLink.h +++ b/src/comm/MockLink.h @@ -199,6 +199,7 @@ private: QString _name; bool _connected; + int _mavlinkChannel; uint8_t _vehicleSystemId; uint8_t _vehicleComponentId;