From 69c9cb1f48c112b998aae62740bac9b3b8633477 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 Mar 2012 09:12:37 +0100 Subject: [PATCH] Added check for old MAVLink versions in decoding routine --- src/comm/MAVLinkProtocol.cc | 22 +++++++++++++--------- src/uas/UAS.cc | 12 +++++++++++- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index dfef6471c..eaa03dc9d 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -188,19 +188,23 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) mavlink_message_t message; mavlink_status_t status; + static int mavlink09Count = 0; + static bool decodedFirstPacket = false; + for (int position = 0; position < b.size(); position++) { - unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b.at(position)), &message, &status); + unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b[position]), &message, &status); + + if ((uint8_t)b[position] == 0x55) mavlink09Count++; + if ((mavlink09Count > 100) && !decodedFirstPacket) + { + // Obviously the user tries to use a 0.9 autopilot + // with QGroundControl built for version 1.0 + emit protocolStatusMessage("MAVLink Version Mismatch", "Your MAVLink device seems to use the deprecated version 0.9, while QGroundControl only supports version 1.0+. Please upgrade the MAVLink version of your autopilot."); + } if (decodeState == 1) { -//#ifdef MAVLINK_MESSAGE_LENGTHS -// const uint8_t message_lengths[] = MAVLINK_MESSAGE_LENGTHS; -// if (message.msgid >= sizeof(message_lengths) || -// message.len != message_lengths[message.msgid]) { -// qDebug() << "MAVLink message " << message.msgid << " length incorrect (was " << message.len << " expected " << message_lengths[message.msgid] << ")"; -// continue; -// } -//#endif + decodedFirstPacket = true; #if defined(QGC_PROTOBUF_ENABLED) if (message.msgid == MAVLINK_MSG_ID_EXTENDED_MESSAGE) diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 073ab4ab9..a830788ab 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -923,7 +923,17 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) int severity = mavlink_msg_statustext_get_severity(&message); //qDebug() << "RECEIVED STATUS:" << text;false //emit statusTextReceived(severity, text); - emit textMessageReceived(uasId, message.compid, severity, text); + + if (text.startsWith("#audio:")) + { + text.remove("#audio:"); + emit textMessageReceived(uasId, message.compid, severity, QString("Audio message: ") + text); + GAudioOutput::instance()->say(text, severity); + } + else + { + emit textMessageReceived(uasId, message.compid, severity, text); + } } break; #ifdef MAVLINK_ENABLED_PIXHAWK -- 2.22.0