Browse Source

Enabled 921600 and other high baud rates on all platforms

QGC4.4
LM 13 years ago
parent
commit
5348f704f4
  1. 6
      src/comm/MAVLinkProtocol.cc
  2. 68
      src/comm/SerialLink.cc
  3. 4
      src/uas/UAS.cc
  4. 2
      src/ui/HUD.cc
  5. 18
      src/ui/SerialConfigurationWindow.cc
  6. 2
      thirdParty/qserialport/src/posix/termioshelper.cpp

6
src/comm/MAVLinkProtocol.cc

@ -190,16 +190,18 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b)
static int mavlink09Count = 0; static int mavlink09Count = 0;
static bool decodedFirstPacket = false; static bool decodedFirstPacket = false;
static bool warnedUser = false;
for (int position = 0; position < b.size(); position++) { for (int position = 0; position < b.size(); position++) {
unsigned int decodeState = mavlink_parse_char(link->getId(), (uint8_t)(b[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 ((uint8_t)b[position] == 0x55) mavlink09Count++;
if ((mavlink09Count > 100) && !decodedFirstPacket) if ((mavlink09Count > 100) && !decodedFirstPacket && !warnedUser)
{ {
warnedUser = true;
// Obviously the user tries to use a 0.9 autopilot // Obviously the user tries to use a 0.9 autopilot
// with QGroundControl built for version 1.0 // 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."); emit protocolStatusMessage("MAVLink Version or Baud Rate 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 your autopilot is using version 1.0, check if the baud rates of QGroundControl and your autopilot are the same.");
} }
if (decodeState == 1) if (decodeState == 1)

68
src/comm/SerialLink.cc

@ -676,25 +676,6 @@ qint64 SerialLink::getNominalDataRate()
break; break;
#endif #endif
// Baud rates supported only by Linux
#ifdef Q_OS_LINUX
case QPortSettings::BAUDR_230400:
dataRate = 230400;
break;
case QPortSettings::BAUDR_460800:
dataRate = 460800;
break;
case QPortSettings::BAUDR_500000:
dataRate = 500000;
break;
case QPortSettings::BAUDR_576000:
dataRate = 576000;
break;
case QPortSettings::BAUDR_921600:
dataRate = 921600;
break;
#endif
// Baud rates supported only by Windows // Baud rates supported only by Windows
#if defined(Q_OS_WIN32) || defined(Q_OS_WINCE) #if defined(Q_OS_WIN32) || defined(Q_OS_WINCE)
case QPortSettings::BAUDR_14400: case QPortSettings::BAUDR_14400:
@ -743,6 +724,21 @@ qint64 SerialLink::getNominalDataRate()
case QPortSettings::BAUDR_115200: case QPortSettings::BAUDR_115200:
dataRate = 115200; dataRate = 115200;
break; break;
case QPortSettings::BAUDR_230400:
dataRate = 230400;
break;
case QPortSettings::BAUDR_460800:
dataRate = 460800;
break;
case QPortSettings::BAUDR_500000:
dataRate = 500000;
break;
case QPortSettings::BAUDR_576000:
dataRate = 576000;
break;
case QPortSettings::BAUDR_921600:
dataRate = 921600;
break;
// Otherwise do nothing. // Otherwise do nothing.
case QPortSettings::BAUDR_UNKNOWN: case QPortSettings::BAUDR_UNKNOWN:
@ -979,25 +975,6 @@ bool SerialLink::setBaudRate(int rate)
break; break;
#endif #endif
// Supported only by Linux
#ifdef Q_OS_LINUX
case 230400:
portSettings.setBaudRate(QPortSettings::BAUDR_230400);
break;
case 460800:
portSettings.setBaudRate(QPortSettings::BAUDR_460800);
break;
case 500000:
portSettings.setBaudRate(QPortSettings::BAUDR_500000);
break;
case 576000:
portSettings.setBaudRate(QPortSettings::BAUDR_576000);
break;
case 921600:
portSettings.setBaudRate(QPortSettings::BAUDR_921600);
break;
#endif
// Baud rates supported only by windows // Baud rates supported only by windows
#if defined(Q_OS_WIN32) || defined(Q_OS_WINCE) #if defined(Q_OS_WIN32) || defined(Q_OS_WINCE)
case 14400: case 14400:
@ -1048,6 +1025,21 @@ bool SerialLink::setBaudRate(int rate)
case 115200: case 115200:
portSettings.setBaudRate(QPortSettings::BAUDR_115200); portSettings.setBaudRate(QPortSettings::BAUDR_115200);
break; break;
case 230400:
portSettings.setBaudRate(QPortSettings::BAUDR_230400);
break;
case 460800:
portSettings.setBaudRate(QPortSettings::BAUDR_460800);
break;
case 500000:
portSettings.setBaudRate(QPortSettings::BAUDR_500000);
break;
case 576000:
portSettings.setBaudRate(QPortSettings::BAUDR_576000);
break;
case 921600:
portSettings.setBaudRate(QPortSettings::BAUDR_921600);
break;
default: default:
// If none of the above cases matches, there must be an error // If none of the above cases matches, there must be an error
accepted = false; accepted = false;

4
src/uas/UAS.cc

@ -984,7 +984,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// Restart statemachine // Restart statemachine
imagePacketsArrived = 0; imagePacketsArrived = 0;
emit imageReady(this); emit imageReady(this);
qDebug() << "imageReady emitted. all packets arrived"; //qDebug() << "imageReady emitted. all packets arrived";
} }
} }
break; break;
@ -1632,7 +1632,7 @@ QImage UAS::getImage()
{ {
#ifdef MAVLINK_ENABLED_PIXHAWK #ifdef MAVLINK_ENABLED_PIXHAWK
qDebug() << "IMAGE TYPE:" << imageType; // qDebug() << "IMAGE TYPE:" << imageType;
// RAW greyscale // RAW greyscale
if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U) if (imageType == MAVLINK_DATA_STREAM_IMG_RAW8U)

2
src/ui/HUD.cc

@ -1465,7 +1465,7 @@ void HUD::copyImage()
{ {
if (isVisible()) if (isVisible())
{ {
qDebug() << "HUD::copyImage()"; //qDebug() << "HUD::copyImage()";
UAS* u = dynamic_cast<UAS*>(this->uas); UAS* u = dynamic_cast<UAS*>(this->uas);
if (u) if (u)
{ {

18
src/ui/SerialConfigurationWindow.cc

@ -73,14 +73,6 @@ SerialConfigurationWindow::SerialConfigurationWindow(LinkInterface* link, QWidge
supportedBaudRates << 200; supportedBaudRates << 200;
supportedBaudRates << 1800; supportedBaudRates << 1800;
#endif #endif
// Baud rates supported only by Linux
#if defined(Q_OS_LINUX)
supportedBaudRates << 230400;
supportedBaudRates << 460800;
supportedBaudRates << 500000;
supportedBaudRates << 576000;
supportedBaudRates << 921600;
#endif
// Baud rates supported only by Windows // Baud rates supported only by Windows
#if defined(Q_OS_WIN) #if defined(Q_OS_WIN)
@ -102,6 +94,16 @@ SerialConfigurationWindow::SerialConfigurationWindow(LinkInterface* link, QWidge
supportedBaudRates << 38400; supportedBaudRates << 38400;
supportedBaudRates << 57600; supportedBaudRates << 57600;
supportedBaudRates << 115200; supportedBaudRates << 115200;
supportedBaudRates << 230400;
supportedBaudRates << 460800;
#if defined(Q_OS_LINUX)
// Baud rates supported only by Linux
supportedBaudRates << 500000;
supportedBaudRates << 576000;
#endif
supportedBaudRates << 921600;
// Now actually add all of our supported baud rates to the UI. // Now actually add all of our supported baud rates to the UI.
qSort(supportedBaudRates.begin(), supportedBaudRates.end()); qSort(supportedBaudRates.begin(), supportedBaudRates.end());

2
thirdParty/qserialport/src/posix/termioshelper.cpp vendored

@ -327,6 +327,8 @@ void TermiosHelper::setBaudRate(QPortSettings::BaudRate baudRate)
// } // }
//#else //#else
qCritical() << "Baud rate is now: " << baud;
if ( cfsetspeed(currentAttrs_, baud) == -1 ) { if ( cfsetspeed(currentAttrs_, baud) == -1 ) {
qCritical() << QString("TermiosHelper::setBaudRate(file: %1) failed: %2(%3)") qCritical() << QString("TermiosHelper::setBaudRate(file: %1) failed: %2(%3)")
.arg(fileDescriptor_) .arg(fileDescriptor_)

Loading…
Cancel
Save