From 10b2b3098162baf27a769c040405e5e533938213 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 5 Jun 2017 22:41:07 +0200 Subject: [PATCH] MAVLink: Accept existing MAVLink 2 connection and accept link disconnect command. --- src/Vehicle/Vehicle.cc | 64 +++++++++++++++++++++++++++++++++--- src/Vehicle/Vehicle.h | 1 + src/comm/LinkManager.cc | 1 + src/comm/MAVLinkProtocol.cc | 37 ++++++++++++++++++--- src/comm/MAVLinkProtocol.h | 3 +- src/comm/ProtocolInterface.h | 2 +- src/comm/SerialLink.h | 1 + 7 files changed, 97 insertions(+), 12 deletions(-) diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index 72655ad23..3f1ce0e5c 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -484,6 +484,12 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes if (!_containsLink(link)) { _addLink(link); + + // if the minimum supported version of MAVLink is already 2.0 + // set our max proto version to it. + if (_mavlink->getCurrentVersion() >= 200) { + _maxProtoVersion = _mavlink->getCurrentVersion(); + } } //-- Check link status @@ -513,8 +519,11 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes } - // Mark this vehicle as active - _connectionActive(); + // Mark this vehicle as active - but only if the traffic is coming from + // the actual vehicle + if (message.sysid == _id) { + _connectionActive(); + } // Give the plugin a change to adjust the message contents if (!_firmwarePlugin->adjustIncomingMavlinkMessage(this, &message)) { @@ -569,6 +578,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes case MAVLINK_MSG_ID_COMMAND_ACK: _handleCommandAck(message); break; + case MAVLINK_MSG_ID_COMMAND_LONG: + _handleCommandLong(message); + break; case MAVLINK_MSG_ID_AUTOPILOT_VERSION: _handleAutopilotVersion(link, message); break; @@ -853,7 +865,9 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) if (ack.command == MAV_CMD_REQUEST_PROTOCOL_VERSION && ack.result != MAV_RESULT_ACCEPTED) { // The autopilot does not understand the request and consequently is likely handling only // MAVLink 1 - _setMaxProtoVersion(100); + if (_maxProtoVersion == 0) { + _setMaxProtoVersion(100); + } } if (_mavCommandQueue.count() && ack.command == _mavCommandQueue[0].command) { @@ -889,6 +903,29 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) _sendNextQueuedMavCommand(); } +void Vehicle::_handleCommandLong(mavlink_message_t& message) +{ + mavlink_command_long_t cmd; + mavlink_msg_command_long_decode(&message, &cmd); + + switch (cmd.command) { + // Other component on the same system + // requests that QGC frees up the serial port of the autopilot + case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (cmd.param6 > 0) { + // disconnect the USB link if its a direct connection to a Pixhawk + for (int i = 0; i < _links.length(); i++) { + SerialLink *sl = qobject_cast(_links.at(i)); + if (sl && sl->getSerialConfig()->usbDirect()) { + qDebug() << "Disconnecting:" << _links.at(i)->getName(); + qgcApp()->toolbox()->linkManager()->disconnectLink(_links.at(i)); + } + } + } + break; + } +} + void Vehicle::_handleExtendedSysState(mavlink_message_t& message) { mavlink_extended_sys_state_t extendedState; @@ -1076,9 +1113,11 @@ void Vehicle::_handleHeartbeat(mavlink_message_t& message) void Vehicle::_handleRadioStatus(mavlink_message_t& message) { + //-- Process telemetry status message mavlink_radio_status_t rstatus; mavlink_msg_radio_status_decode(&message, &rstatus); + int rssi = rstatus.rssi; int remrssi = rstatus.remrssi; int lnoise = (int)(int8_t)rstatus.noise; @@ -1958,9 +1997,15 @@ void Vehicle::_connectionLostTimeout(void) if (_connectionLostEnabled && !_connectionLost) { _connectionLost = true; _heardFrom = false; + _maxProtoVersion = 0; emit connectionLostChanged(true); _say(QString("%1 communication lost").arg(_vehicleIdSpeech())); if (_autoDisconnect) { + + // Reset link state + for (int i = 0; i < _links.length(); i++) { + _mavlink->resetMetadataForLink(_links.at(i)); + } disconnectInactiveVehicle(); } } @@ -1973,6 +2018,12 @@ void Vehicle::_connectionActive(void) _connectionLost = false; emit connectionLostChanged(false); _say(QString("%1 communication regained").arg(_vehicleIdSpeech())); + + // Re-negotiate protocol version for the link + sendMavCommand(MAV_COMP_ID_ALL, // Don't know default component id yet. + MAV_CMD_REQUEST_PROTOCOL_VERSION, + false, // No error shown if fails + 1); // Request protocol version } } @@ -2289,8 +2340,11 @@ void Vehicle::_sendMavCommandAgain(void) } if (queuedCommand.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) { - // We aren't going to get a response back for the protocol version, so assume v1 is all we can do - _setMaxProtoVersion(100); + // We aren't going to get a response back for the protocol version, so assume v1 is all we can do. + // If the max protocol version is uninitialized, fall back to v1. + if (_maxProtoVersion == 0) { + _setMaxProtoVersion(100); + } } emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index f7343e802..733c4d9b0 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -843,6 +843,7 @@ private: void _handleVibration(mavlink_message_t& message); void _handleExtendedSysState(mavlink_message_t& message); void _handleCommandAck(mavlink_message_t& message); + void _handleCommandLong(mavlink_message_t& message); void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message); void _handleProtocolVersion(LinkInterface* link, mavlink_message_t& message); void _handleHilActuatorControls(mavlink_message_t& message); diff --git a/src/comm/LinkManager.cc b/src/comm/LinkManager.cc index 3bbe7249d..37f308371 100644 --- a/src/comm/LinkManager.cc +++ b/src/comm/LinkManager.cc @@ -191,6 +191,7 @@ void LinkManager::_addLink(LinkInterface* link) connect(link, &LinkInterface::bytesReceived, _mavlinkProtocol, &MAVLinkProtocol::receiveBytes); _mavlinkProtocol->resetMetadataForLink(link); + _mavlinkProtocol->setVersion(_mavlinkProtocol->getCurrentVersion()); connect(link, &LinkInterface::connected, this, &LinkManager::_linkConnected); connect(link, &LinkInterface::disconnected, this, &LinkManager::_linkDisconnected); diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 16a952219..8b6074865 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -56,6 +56,7 @@ MAVLinkProtocol::MAVLinkProtocol(QGCApplication* app, QGCToolbox* toolbox) , versionMismatchIgnore(false) , systemId(255) , _current_version(100) + , _radio_version_mismatch_count(0) , _logSuspendError(false) , _logSuspendReplay(false) , _vehicleWasArmed(false) @@ -152,7 +153,7 @@ void MAVLinkProtocol::storeSettings() // Parameter interface settings } -void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link) +void MAVLinkProtocol::resetMetadataForLink(LinkInterface *link) { int channel = link->mavlinkChannel(); totalReceiveCounter[channel] = 0; @@ -160,6 +161,7 @@ void MAVLinkProtocol::resetMetadataForLink(const LinkInterface *link) totalErrorCounter[channel] = 0; currReceiveCounter[channel] = 0; currLossCounter[channel] = 0; + link->setDecodedFirstMavlinkPacket(false); } /** @@ -194,9 +196,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (decodeState == 0 && !link->decodedFirstMavlinkPacket()) { nonmavlinkCount++; - if (nonmavlinkCount > 2000 && !warnedUserNonMavlink) + if (nonmavlinkCount > 500 && !warnedUserNonMavlink) { - //2000 bytes with no mavlink message. Are we connected to a mavlink capable device? + // 500 bytes with no mavlink message. Are we connected to a mavlink capable device? if (!checkedUserNonMavlink) { link->requestReset(); @@ -217,13 +219,15 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (decodeState == 1) { if (!link->decodedFirstMavlinkPacket()) { + link->setDecodedFirstMavlinkPacket(true); mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); if (!(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) && (mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) { qDebug() << "Switching outbound to mavlink 2.0 due to incoming mavlink 2.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags; mavlinkStatus->flags &= ~MAVLINK_STATUS_FLAG_OUT_MAVLINK1; - _current_version = 200; + + // Set all links to v2 + setVersion(200); } - link->setDecodedFirstMavlinkPacket(true); } // Log data @@ -270,6 +274,26 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) emit vehicleHeartbeatInfo(link, message.sysid, message.compid, heartbeat.mavlink_version, heartbeat.autopilot, heartbeat.type); } + // Detect if we are talking to an old radio not supporting v2 + mavlink_status_t* mavlinkStatus = mavlink_get_channel_status(mavlinkChannel); + if (message.msgid == MAVLINK_MSG_ID_RADIO_STATUS) { + if ((mavlinkStatus->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) + && !(mavlinkStatus->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1)) { + + _radio_version_mismatch_count++; + } + } + + if (_radio_version_mismatch_count == 5) { + // Warn the user if the radio continues to send v1 while the link uses v2 + emit protocolStatusMessage(tr("MAVLink Protocol"), tr("Detected radio still using MAVLink v1.0 on a link with MAVLink v2.0 enabled. Please upgrade the radio firmware.")); + // Ensure the warning can't get stuck + _radio_version_mismatch_count++; + // Flick link back to v1 + qDebug() << "Switching outbound to mavlink 1.0 due to incoming mavlink 1.0 packet:" << mavlinkStatus << mavlinkChannel << mavlinkStatus->flags; + mavlinkStatus->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } + // Increase receive counter totalReceiveCounter[mavlinkChannel]++; currReceiveCounter[mavlinkChannel]++; @@ -358,6 +382,9 @@ void MAVLinkProtocol::_vehicleCountChanged(void) if (count == 0) { // Last vehicle is gone, close out logging _stopLogging(); + // Reset protocol version handling + _current_version = 0; + _radio_version_mismatch_count = 0; } } diff --git a/src/comm/MAVLinkProtocol.h b/src/comm/MAVLinkProtocol.h index e115c327a..5d6311734 100644 --- a/src/comm/MAVLinkProtocol.h +++ b/src/comm/MAVLinkProtocol.h @@ -89,7 +89,7 @@ public: /** * Reset the counters for all metadata for this link. */ - virtual void resetMetadataForLink(const LinkInterface *link); + virtual void resetMetadataForLink(LinkInterface *link); /// Suspend/Restart logging during replay. void suspendLogForReplay(bool suspend); @@ -133,6 +133,7 @@ protected: bool versionMismatchIgnore; int systemId; unsigned _current_version; + unsigned _radio_version_mismatch_count; signals: /// Heartbeat received on link diff --git a/src/comm/ProtocolInterface.h b/src/comm/ProtocolInterface.h index a845ccd57..b5cc9704d 100644 --- a/src/comm/ProtocolInterface.h +++ b/src/comm/ProtocolInterface.h @@ -62,7 +62,7 @@ public: * when reconnecting a link. * @param link The link to reset metadata for. */ - virtual void resetMetadataForLink(const LinkInterface *link) = 0; + virtual void resetMetadataForLink(LinkInterface *link) = 0; public slots: virtual void receiveBytes(LinkInterface *link, QByteArray b) = 0; diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index be46a06e9..a70e27a30 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -137,6 +137,7 @@ public: void requestReset(); bool isConnected() const; qint64 getConnectionSpeed() const; + SerialConfiguration* getSerialConfig() const { return _serialConfig; } // These are left unimplemented in order to cause linker errors which indicate incorrect usage of // connect/disconnect on link directly. All connect/disconnect calls should be made through LinkManager. -- 2.22.0