diff --git a/deploy/mac_create_dmg.sh b/deploy/mac_create_dmg.sh index b35a041869d3121e99887eecde6e642b055ce22a..bcdfe4381a653e1ee7b0af1901be6fa6dfe49cd9 100644 --- a/deploy/mac_create_dmg.sh +++ b/deploy/mac_create_dmg.sh @@ -12,6 +12,7 @@ cp -r ../bin/mac/qgroundcontrol.app mac/. cp -r ../audio mac/qgroundcontrol.app/Contents/MacOs/. mkdir -p mac/qgroundcontrol.app/Contents/Frameworks/ +# SDL is not copied by Qt - for whatever reason cp -r SDL.framework mac/qgroundcontrol.app/Contents/Frameworks/. echo -e '\n\nStarting to create disk image. This may take a while..\n' macdeployqt mac/qgroundcontrol.app -dmg diff --git a/qgroundcontrol.pri b/qgroundcontrol.pri index d21ef7c5ccc5e0e7de617740558e9d6ef1c47a3b..d3be98e82ed0bcfce2c5965c290637039163134d 100644 --- a/qgroundcontrol.pri +++ b/qgroundcontrol.pri @@ -29,6 +29,7 @@ # $$BASEDIR/lib/openjaus/libopenJaus/include message(Qt version $$[QT_VERSION]) +message(Using Qt from $QTDIR) release { # DEFINES += QT_NO_DEBUG_OUTPUT @@ -50,6 +51,7 @@ macx { contains( HARDWARE_PLATFORM, 9.6.0 ) || contains( HARDWARE_PLATFORM, 9.7.0 ) || contains( HARDWARE_PLATFORM, 9.8.0 ) || contains( HARDWARE_PLATFORM, 9.9.0 ) { # x86 Mac OS X Leopard 10.5 and earlier CONFIG += x86 cocoa phonon + CONFIG -= x86_64 message(Building for Mac OS X 32bit/Leopard 10.5 and earlier) # Enable function-profiling with the OS X saturn tool diff --git a/src/QGC.h b/src/QGC.h index c4e4fed4b4c0b33d2dd61fd6cde677b51bee80bc..e21a17d105b80dc8839f1bd2f83cb0eabc8fbaf6 100644 --- a/src/QGC.h +++ b/src/QGC.h @@ -11,6 +11,7 @@ namespace QGC const QColor colorGreen(20, 200, 20); const QColor colorYellow(255, 255, 0); const QColor colorDarkYellow(180, 180, 0); + const QColor colorBackground("#050508"); /** @brief Get the current ground time in microseconds */ quint64 groundTimeUsecs(); diff --git a/src/comm/MAVLinkSimulationLink.cc b/src/comm/MAVLinkSimulationLink.cc index a4606b528d1d5e2d5a549ace97e0034ac296d6a9..9dcd0bc928f5848bcbb3a3bd4ccbe838752e0e01 100644 --- a/src/comm/MAVLinkSimulationLink.cc +++ b/src/comm/MAVLinkSimulationLink.cc @@ -289,16 +289,19 @@ void MAVLinkSimulationLink::mainloop() if (keys.value(i, "") == "Gyro_Phi") { rawImuValues.xgyro = d; + attitude.rollspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65; } if (keys.value(i, "") == "Gyro_Theta") { rawImuValues.ygyro = d; + attitude.pitchspeed = ((d-29.000)/15000.0)*2.7-2.7-2.65; } if (keys.value(i, "") == "Gyro_Psi") { rawImuValues.zgyro = d; + attitude.yawspeed = ((d-29.000)/3000.0)*2.7-2.7-2.65; } #ifdef MAVLINK_ENABLED_PIXHAWK if (keys.value(i, "") == "Pressure") @@ -426,12 +429,12 @@ void MAVLinkSimulationLink::mainloop() memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; - // GLOBAL POSITION VEHICLE 3 - mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); - bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; +// // GLOBAL POSITION VEHICLE 3 +// mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0); +// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret); +// //add data into datastream +// memcpy(stream+streampointer,buffer, bufferlength); +// streampointer += bufferlength; static int rcCounter = 0; if (rcCounter == 2) @@ -609,15 +612,15 @@ void MAVLinkSimulationLink::mainloop() memcpy(stream+streampointer,buffer, bufferlength); streampointer += bufferlength; - // HEARTBEAT VEHICLE 3 +// // HEARTBEAT VEHICLE 3 - // Pack message and get size of encoded byte string - messageSize = mavlink_msg_heartbeat_pack(60, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); - // Allocate buffer with packet data - bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); - //add data into datastream - memcpy(stream+streampointer,buffer, bufferlength); - streampointer += bufferlength; +// // Pack message and get size of encoded byte string +// messageSize = mavlink_msg_heartbeat_pack(60, componentId, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); +// // Allocate buffer with packet data +// bufferlength = mavlink_msg_to_send_buffer(buffer, &msg); +// //add data into datastream +// memcpy(stream+streampointer,buffer, bufferlength); +// streampointer += bufferlength; // STATUS VEHICLE 2 mavlink_sys_status_t status2; diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 236493000159d1368b71a6d825ca9d8509c46f80..afb4da35eb6ec34cbe76e3ab36c6569f81286d67 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -1,24 +1,4 @@ /*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - ======================================================================*/ /** * @file @@ -58,34 +38,16 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P // Set unique ID and add link to the list of links this->id = getNextLinkId(); - // Load defaults from settings - QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); - settings.sync(); - if (settings.contains("SERIALLINK_COMM_PORT")) - { - this->porthandle = settings.value("SERIALLINK_COMM_PORT").toString(); - } - // *nix (Linux, MacOS tested) serial port support port = new QextSerialPort(porthandle, QextSerialPort::Polling); //port = new QextSerialPort(porthandle, QextSerialPort::EventDriven); - if (settings.contains("SERIALLINK_COMM_PORT")) - { - setBaudRate(settings.value("SERIALLINK_COMM_BAUD").toInt()); - setParityType(settings.value("SERIALLINK_COMM_PARITY").toInt()); - setStopBitsType(settings.value("SERIALLINK_COMM_STOPBITS").toInt()); - setDataBitsType(settings.value("SERIALLINK_COMM_DATABITS").toInt()); - } - else - { - this->baudrate = baudrate; - this->flow = flow; - this->parity = parity; - this->dataBits = dataBits; - this->stopBits = stopBits; - this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all. - } + this->baudrate = baudrate; + this->flow = flow; + this->parity = parity; + this->dataBits = dataBits; + this->stopBits = stopBits; + this->timeout = 1; ///< The timeout controls how long the program flow should wait for new serial bytes. As we're polling, we don't want to wait at all. port->setTimeout(timeout); // Timeout of 0 ms, we don't want to wait for data, we just poll again next time port->setBaudRate(baudrate); port->setFlowControl(flow); @@ -96,7 +58,7 @@ SerialLink::SerialLink(QString portname, BaudRateType baudrate, FlowType flow, P // Set the port name if (porthandle == "") { -// name = tr("serial link ") + QString::number(getId()) + tr(" (unconfigured)"); + // name = tr("serial link ") + QString::number(getId()) + tr(" (unconfigured)"); name = tr("Serial Link ") + QString::number(getId()); } else @@ -134,6 +96,33 @@ SerialLink::~SerialLink() port = NULL; } +void SerialLink::loadSettings() +{ + // Load defaults from settings + QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); + settings.sync(); + if (settings.contains("SERIALLINK_COMM_PORT")) + { + setPortName(settings.value("SERIALLINK_COMM_PORT").toString()); + setBaudRateType(settings.value("SERIALLINK_COMM_BAUD").toInt()); + setParityType(settings.value("SERIALLINK_COMM_PARITY").toInt()); + setStopBitsType(settings.value("SERIALLINK_COMM_STOPBITS").toInt()); + setDataBitsType(settings.value("SERIALLINK_COMM_DATABITS").toInt()); + } +} + +void SerialLink::writeSettings() +{ + // Store settings + QSettings settings(QGC::COMPANYNAME, QGC::APPNAME); + settings.setValue("SERIALLINK_COMM_PORT", this->porthandle); + settings.setValue("SERIALLINK_COMM_BAUD", getBaudRateType()); + settings.setValue("SERIALLINK_COMM_PARITY", getParityType()); + settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBitsType()); + settings.setValue("SERIALLINK_COMM_DATABITS", getDataBitsType()); + settings.sync(); +} + /** * @brief Runs the thread @@ -189,13 +178,13 @@ void SerialLink::writeBytes(const char* data, qint64 size) // Increase write counter bitsSentTotal += size * 8; -// int i; -// for (i=0; iporthandle); - settings.setValue("SERIALLINK_COMM_BAUD", getBaudRate()); - settings.setValue("SERIALLINK_COMM_PARITY", getParityType()); - settings.setValue("SERIALLINK_COMM_STOPBITS", getStopBitsType()); - settings.setValue("SERIALLINK_COMM_DATABITS", getDataBitsType()); - settings.sync(); + writeSettings(); } return connectionUp; @@ -376,7 +359,8 @@ void SerialLink::setName(QString name) qint64 SerialLink::getNominalDataRate() { qint64 dataRate = 0; - switch (baudrate) { + switch (baudrate) + { case BAUD50: dataRate = 50; break; @@ -442,6 +426,13 @@ qint64 SerialLink::getNominalDataRate() break; case BAUD256000: dataRate = 256000; + // Windows-specific high-end baudrates + case BAUD230400: + dataRate = 230400; + case BAUD460800: + dataRate = 460800; + case BAUD921600: + dataRate = 921600; break; } return dataRate; @@ -543,7 +534,8 @@ bool SerialLink::setPortName(QString portName) if(portName.trimmed().length() > 0) { bool reconnect = false; - if(isConnected()) { + if(isConnected()) + { disconnect(); reconnect = true; } @@ -650,8 +642,17 @@ bool SerialLink::setBaudRateType(int rateIndex) baudrate = BAUD128000; break; case 21: + baudrate = BAUD230400; + break; + case 22: baudrate = BAUD256000; break; + case 23: + baudrate = BAUD460800; + break; + case 24: + baudrate = BAUD921600; + break; default: // If none of the above cases matches, there must be an error accepted = false; @@ -671,12 +672,14 @@ bool SerialLink::setBaudRate(int rate) { bool reconnect = false; bool accepted = true; // This is changed if none of the data rates matches - if(isConnected()) { + if(isConnected()) + { disconnect(); reconnect = true; } - switch (rate) { + switch (rate) + { case 50: baudrate = BAUD50; break; @@ -740,9 +743,18 @@ bool SerialLink::setBaudRate(int rate) case 128000: baudrate = BAUD128000; break; + case 230400: + baudrate = BAUD230400; + break; case 256000: baudrate = BAUD256000; break; + case 460800: + baudrate = BAUD460800; + break; + case 921600: + baudrate = BAUD921600; + break; default: // If none of the above cases matches, there must be an error accepted = false; @@ -765,12 +777,14 @@ bool SerialLink::setFlowType(int flow) { bool reconnect = false; bool accepted = true; - if(isConnected()) { + if(isConnected()) + { disconnect(); reconnect = true; } - switch (flow) { + switch (flow) + { case FLOW_OFF: this->flow = FLOW_OFF; break; @@ -794,12 +808,14 @@ bool SerialLink::setParityType(int parity) { bool reconnect = false; bool accepted = true; - if(isConnected()) { + if(isConnected()) + { disconnect(); reconnect = true; } - switch (parity) { + switch (parity) + { case PAR_NONE: this->parity = PAR_NONE; break; @@ -826,11 +842,14 @@ bool SerialLink::setParityType(int parity) return accepted; } + +// FIXME Works not as anticipated by user! bool SerialLink::setDataBitsType(int dataBits) { bool accepted = true; - switch (dataBits) { + switch (dataBits) + { case 5: this->dataBits = DATA_5; break; @@ -858,6 +877,7 @@ bool SerialLink::setDataBitsType(int dataBits) return accepted; } +// FIXME WORKS NOT AS ANTICIPATED BY USER! bool SerialLink::setStopBitsType(int stopBits) { bool reconnect = false; @@ -867,7 +887,8 @@ bool SerialLink::setStopBitsType(int stopBits) reconnect = true; } - switch (stopBits) { + switch (stopBits) + { case 1: this->stopBits = STOP_1; break; diff --git a/src/comm/SerialLink.h b/src/comm/SerialLink.h index dce2386e06d5132831a94022c23028fa144a9185..3bac202b8cf753e0510663815de76b464dbbfc17 100644 --- a/src/comm/SerialLink.h +++ b/src/comm/SerialLink.h @@ -91,6 +91,9 @@ public: qint64 getBitsSent(); qint64 getBitsReceived(); + void loadSettings(); + void writeSettings(); + void run(); int getLinkQuality(); diff --git a/src/comm/SerialLinkInterface.h b/src/comm/SerialLinkInterface.h index 22e00a8615a11df20fc7d99606a9c006962911cb..49b5335f9811db78cd9873c505e9b14e0ed89f44 100644 --- a/src/comm/SerialLinkInterface.h +++ b/src/comm/SerialLinkInterface.h @@ -57,6 +57,8 @@ public slots: virtual bool setParityType(int parity) = 0; virtual bool setDataBitsType(int dataBits) = 0; virtual bool setStopBitsType(int stopBits) = 0; + virtual void loadSettings() = 0; + virtual void writeSettings() = 0; }; diff --git a/src/lib/qextserialport/posix_qextserialport.cpp b/src/lib/qextserialport/posix_qextserialport.cpp index 32c6320caf5f6f1907a7c43e18803ce767d18ed6..99247130b041c23ef84584331203465e984ffe2d 100644 --- a/src/lib/qextserialport/posix_qextserialport.cpp +++ b/src/lib/qextserialport/posix_qextserialport.cpp @@ -41,7 +41,7 @@ This constructor assigns the device name to the name of the first port on the sp See the other constructors if you need to open a different port. */ Posix_QextSerialPort::Posix_QextSerialPort(QextSerialBase::QueryMode mode) -: QextSerialBase() + : QextSerialBase() { setQueryMode(mode); init(); @@ -52,9 +52,9 @@ Posix_QextSerialPort::Posix_QextSerialPort(QextSerialBase::QueryMode mode) Copy constructor. */ Posix_QextSerialPort::Posix_QextSerialPort(const Posix_QextSerialPort& s) - : QextSerialBase(s.port) + : QextSerialBase(s.port) { - setOpenMode(s.openMode()); + setOpenMode(s.openMode()); port = s.port; Settings.BaudRate=s.Settings.BaudRate; Settings.DataBits=s.Settings.DataBits; @@ -76,7 +76,7 @@ name is the name of the device, which is windowsystem-specific, e.g."COM1" or "/dev/ttyS0". */ Posix_QextSerialPort::Posix_QextSerialPort(const QString & name, QextSerialBase::QueryMode mode) - : QextSerialBase(name) + : QextSerialBase(name) { setQueryMode(mode); init(); @@ -87,7 +87,7 @@ Posix_QextSerialPort::Posix_QextSerialPort(const QString & name, QextSerialBase: Constructs a port with default name and specified settings. */ Posix_QextSerialPort::Posix_QextSerialPort(const PortSettings& settings, QextSerialBase::QueryMode mode) - : QextSerialBase() + : QextSerialBase() { setBaudRate(settings.BaudRate); setDataBits(settings.DataBits); @@ -105,7 +105,7 @@ Posix_QextSerialPort::Posix_QextSerialPort(const PortSettings& settings, QextSer Constructs a port with specified name and settings. */ Posix_QextSerialPort::Posix_QextSerialPort(const QString & name, const PortSettings& settings, QextSerialBase::QueryMode mode) - : QextSerialBase(name) + : QextSerialBase(name) { setBaudRate(settings.BaudRate); setDataBits(settings.DataBits); @@ -123,8 +123,8 @@ Posix_QextSerialPort::Posix_QextSerialPort(const QString & name, const PortSetti Override the = operator. */ Posix_QextSerialPort& Posix_QextSerialPort::operator=(const Posix_QextSerialPort& s) -{ - setOpenMode(s.openMode()); + { + setOpenMode(s.openMode()); port = s.port; Settings.BaudRate=s.Settings.BaudRate; Settings.DataBits=s.Settings.DataBits; @@ -142,9 +142,9 @@ Posix_QextSerialPort& Posix_QextSerialPort::operator=(const Posix_QextSerialPort void Posix_QextSerialPort::init() { - fd = 0; - if (queryMode() == QextSerialBase::EventDriven) - qWarning("POSIX doesn't have event driven mechanism implemented yet"); + fd = 0; + if (queryMode() == QextSerialBase::EventDriven) + qWarning("POSIX doesn't have event driven mechanism implemented yet"); } /*! @@ -194,7 +194,10 @@ BAUD1800. BAUD76800 57600 76800 *BAUD115200 115200 115200 BAUD128000 128000 115200 + BAUD230400 230400 115200 BAUD256000 256000 115200 + BAUD460800 460800 115200 + BAUD921600 921600 115200 \endverbatim */ void Posix_QextSerialPort::setBaudRate(BaudRateType baudRate) @@ -202,300 +205,306 @@ void Posix_QextSerialPort::setBaudRate(BaudRateType baudRate) LOCK_MUTEX(); if (Settings.BaudRate!=baudRate) { switch (baudRate) { - case BAUD14400: - Settings.BaudRate=BAUD9600; - break; + case BAUD14400: + Settings.BaudRate=BAUD9600; + break; - case BAUD56000: - Settings.BaudRate=BAUD38400; - break; + case BAUD56000: + Settings.BaudRate=BAUD38400; + break; - case BAUD76800: + case BAUD76800: #ifndef B76800 - Settings.BaudRate=BAUD57600; + Settings.BaudRate=BAUD57600; #else - Settings.BaudRate=baudRate; + Settings.BaudRate=baudRate; #endif - break; + break; - case BAUD128000: - case BAUD256000: - Settings.BaudRate=BAUD115200; - break; + case BAUD128000: + case BAUD230400: + case BAUD460800: + case BAUD921600: + case BAUD256000: + Settings.BaudRate=BAUD115200; + break; - default: - Settings.BaudRate=baudRate; - break; + default: + Settings.BaudRate=baudRate; + break; } } if (isOpen()) { switch (baudRate) { /*50 baud*/ - case BAUD50: - TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 50 baud operation."); + case BAUD50: + TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 50 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B50; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B50; #else - cfsetispeed(&Posix_CommConfig, B50); - cfsetospeed(&Posix_CommConfig, B50); + cfsetispeed(&Posix_CommConfig, B50); + cfsetospeed(&Posix_CommConfig, B50); #endif - break; + break; /*75 baud*/ - case BAUD75: - TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 75 baud operation."); + case BAUD75: + TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 75 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B75; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B75; #else - cfsetispeed(&Posix_CommConfig, B75); - cfsetospeed(&Posix_CommConfig, B75); + cfsetispeed(&Posix_CommConfig, B75); + cfsetospeed(&Posix_CommConfig, B75); #endif - break; + break; /*110 baud*/ - case BAUD110: + case BAUD110: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B110; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B110; #else - cfsetispeed(&Posix_CommConfig, B110); - cfsetospeed(&Posix_CommConfig, B110); + cfsetispeed(&Posix_CommConfig, B110); + cfsetospeed(&Posix_CommConfig, B110); #endif - break; + break; /*134.5 baud*/ - case BAUD134: - TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 134.5 baud operation."); + case BAUD134: + TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 134.5 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B134; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B134; #else - cfsetispeed(&Posix_CommConfig, B134); - cfsetospeed(&Posix_CommConfig, B134); + cfsetispeed(&Posix_CommConfig, B134); + cfsetospeed(&Posix_CommConfig, B134); #endif - break; + break; /*150 baud*/ - case BAUD150: - TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 150 baud operation."); + case BAUD150: + TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 150 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B150; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B150; #else - cfsetispeed(&Posix_CommConfig, B150); - cfsetospeed(&Posix_CommConfig, B150); + cfsetispeed(&Posix_CommConfig, B150); + cfsetospeed(&Posix_CommConfig, B150); #endif - break; + break; /*200 baud*/ - case BAUD200: - TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 200 baud operation."); + case BAUD200: + TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows does not support 200 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B200; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B200; #else - cfsetispeed(&Posix_CommConfig, B200); - cfsetospeed(&Posix_CommConfig, B200); + cfsetispeed(&Posix_CommConfig, B200); + cfsetospeed(&Posix_CommConfig, B200); #endif - break; + break; /*300 baud*/ - case BAUD300: + case BAUD300: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B300; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B300; #else - cfsetispeed(&Posix_CommConfig, B300); - cfsetospeed(&Posix_CommConfig, B300); + cfsetispeed(&Posix_CommConfig, B300); + cfsetospeed(&Posix_CommConfig, B300); #endif - break; + break; /*600 baud*/ - case BAUD600: + case BAUD600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B600; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B600; #else - cfsetispeed(&Posix_CommConfig, B600); - cfsetospeed(&Posix_CommConfig, B600); + cfsetispeed(&Posix_CommConfig, B600); + cfsetospeed(&Posix_CommConfig, B600); #endif - break; + break; /*1200 baud*/ - case BAUD1200: + case BAUD1200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B1200; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B1200; #else - cfsetispeed(&Posix_CommConfig, B1200); - cfsetospeed(&Posix_CommConfig, B1200); + cfsetispeed(&Posix_CommConfig, B1200); + cfsetospeed(&Posix_CommConfig, B1200); #endif - break; + break; /*1800 baud*/ - case BAUD1800: - TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows and IRIX do not support 1800 baud operation."); + case BAUD1800: + TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows and IRIX do not support 1800 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B1800; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B1800; #else - cfsetispeed(&Posix_CommConfig, B1800); - cfsetospeed(&Posix_CommConfig, B1800); + cfsetispeed(&Posix_CommConfig, B1800); + cfsetospeed(&Posix_CommConfig, B1800); #endif - break; + break; /*2400 baud*/ - case BAUD2400: + case BAUD2400: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B2400; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B2400; #else - cfsetispeed(&Posix_CommConfig, B2400); - cfsetospeed(&Posix_CommConfig, B2400); + cfsetispeed(&Posix_CommConfig, B2400); + cfsetospeed(&Posix_CommConfig, B2400); #endif - break; + break; /*4800 baud*/ - case BAUD4800: + case BAUD4800: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B4800; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B4800; #else - cfsetispeed(&Posix_CommConfig, B4800); - cfsetospeed(&Posix_CommConfig, B4800); + cfsetispeed(&Posix_CommConfig, B4800); + cfsetospeed(&Posix_CommConfig, B4800); #endif - break; + break; /*9600 baud*/ - case BAUD9600: + case BAUD9600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B9600; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B9600; #else - cfsetispeed(&Posix_CommConfig, B9600); - cfsetospeed(&Posix_CommConfig, B9600); + cfsetispeed(&Posix_CommConfig, B9600); + cfsetospeed(&Posix_CommConfig, B9600); #endif - break; + break; /*14400 baud*/ - case BAUD14400: - TTY_WARNING("Posix_QextSerialPort: POSIX does not support 14400 baud operation. Switching to 9600 baud."); + case BAUD14400: + TTY_WARNING("Posix_QextSerialPort: POSIX does not support 14400 baud operation. Switching to 9600 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B9600; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B9600; #else - cfsetispeed(&Posix_CommConfig, B9600); - cfsetospeed(&Posix_CommConfig, B9600); + cfsetispeed(&Posix_CommConfig, B9600); + cfsetospeed(&Posix_CommConfig, B9600); #endif - break; + break; /*19200 baud*/ - case BAUD19200: + case BAUD19200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B19200; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B19200; #else - cfsetispeed(&Posix_CommConfig, B19200); - cfsetospeed(&Posix_CommConfig, B19200); + cfsetispeed(&Posix_CommConfig, B19200); + cfsetospeed(&Posix_CommConfig, B19200); #endif - break; + break; /*38400 baud*/ - case BAUD38400: + case BAUD38400: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B38400; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B38400; #else - cfsetispeed(&Posix_CommConfig, B38400); - cfsetospeed(&Posix_CommConfig, B38400); + cfsetispeed(&Posix_CommConfig, B38400); + cfsetospeed(&Posix_CommConfig, B38400); #endif - break; + break; /*56000 baud*/ - case BAUD56000: - TTY_WARNING("Posix_QextSerialPort: POSIX does not support 56000 baud operation. Switching to 38400 baud."); + case BAUD56000: + TTY_WARNING("Posix_QextSerialPort: POSIX does not support 56000 baud operation. Switching to 38400 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B38400; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B38400; #else - cfsetispeed(&Posix_CommConfig, B38400); - cfsetospeed(&Posix_CommConfig, B38400); + cfsetispeed(&Posix_CommConfig, B38400); + cfsetospeed(&Posix_CommConfig, B38400); #endif - break; + break; /*57600 baud*/ - case BAUD57600: + case BAUD57600: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B57600; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B57600; #else - cfsetispeed(&Posix_CommConfig, B57600); - cfsetospeed(&Posix_CommConfig, B57600); + cfsetispeed(&Posix_CommConfig, B57600); + cfsetospeed(&Posix_CommConfig, B57600); #endif - break; + break; /*76800 baud*/ - case BAUD76800: - TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows and some POSIX systems do not support 76800 baud operation."); + case BAUD76800: + TTY_PORTABILITY_WARNING("Posix_QextSerialPort Portability Warning: Windows and some POSIX systems do not support 76800 baud operation."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag&=(~CBAUD); #ifdef B76800 - Posix_CommConfig.c_cflag|=B76800; + Posix_CommConfig.c_cflag|=B76800; #else - TTY_WARNING("Posix_QextSerialPort: Posix_QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); - Posix_CommConfig.c_cflag|=B57600; + TTY_WARNING("Posix_QextSerialPort: Posix_QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); + Posix_CommConfig.c_cflag|=B57600; #endif //B76800 #else //CBAUD #ifdef B76800 - cfsetispeed(&Posix_CommConfig, B76800); - cfsetospeed(&Posix_CommConfig, B76800); + cfsetispeed(&Posix_CommConfig, B76800); + cfsetospeed(&Posix_CommConfig, B76800); #else - TTY_WARNING("Posix_QextSerialPort: Posix_QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); - cfsetispeed(&Posix_CommConfig, B57600); - cfsetospeed(&Posix_CommConfig, B57600); + TTY_WARNING("Posix_QextSerialPort: Posix_QextSerialPort was compiled without 76800 baud support. Switching to 57600 baud."); + cfsetispeed(&Posix_CommConfig, B57600); + cfsetospeed(&Posix_CommConfig, B57600); #endif //B76800 #endif //CBAUD - break; + break; /*115200 baud*/ - case BAUD115200: + case BAUD115200: #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; /*128000 baud*/ - case BAUD128000: - TTY_WARNING("Posix_QextSerialPort: POSIX does not support 128000 baud operation. Switching to 115200 baud."); + case BAUD128000: + TTY_WARNING("Posix_QextSerialPort: POSIX does not support 128000 baud operation. Switching to 115200 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; /*256000 baud*/ - case BAUD256000: - TTY_WARNING("Posix_QextSerialPort: POSIX does not support 256000 baud operation. Switching to 115200 baud."); + case BAUD230400: + case BAUD460800: + case BAUD921600: + case BAUD256000: + TTY_WARNING("Posix_QextSerialPort: POSIX does not support baud rates above 115200 baud. Switching to 115200 baud."); #ifdef CBAUD - Posix_CommConfig.c_cflag&=(~CBAUD); - Posix_CommConfig.c_cflag|=B115200; + Posix_CommConfig.c_cflag&=(~CBAUD); + Posix_CommConfig.c_cflag|=B115200; #else - cfsetispeed(&Posix_CommConfig, B115200); - cfsetospeed(&Posix_CommConfig, B115200); + cfsetispeed(&Posix_CommConfig, B115200); + cfsetospeed(&Posix_CommConfig, B115200); #endif - break; + break; } tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); } @@ -536,56 +545,56 @@ void Posix_QextSerialPort::setDataBits(DataBitsType dataBits) switch(dataBits) { /*5 data bits*/ - case DATA_5: - if (Settings.StopBits==STOP_2) { - TTY_WARNING("Posix_QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS5; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + case DATA_5: + if (Settings.StopBits==STOP_2) { + TTY_WARNING("Posix_QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); + } + else { + Settings.DataBits=dataBits; + Posix_CommConfig.c_cflag&=(~CSIZE); + Posix_CommConfig.c_cflag|=CS5; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; /*6 data bits*/ case DATA_6: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Posix_QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS6; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Posix_QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); + } + else { + Settings.DataBits=dataBits; + Posix_CommConfig.c_cflag&=(~CSIZE); + Posix_CommConfig.c_cflag|=CS6; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; /*7 data bits*/ case DATA_7: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Posix_QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS7; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Posix_QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); + } + else { + Settings.DataBits=dataBits; + Posix_CommConfig.c_cflag&=(~CSIZE); + Posix_CommConfig.c_cflag|=CS7; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; /*8 data bits*/ case DATA_8: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Posix_QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); - } - else { - Settings.DataBits=dataBits; - Posix_CommConfig.c_cflag&=(~CSIZE); - Posix_CommConfig.c_cflag|=CS8; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Posix_QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); + } + else { + Settings.DataBits=dataBits; + Posix_CommConfig.c_cflag&=(~CSIZE); + Posix_CommConfig.c_cflag|=CS8; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; } } UNLOCK_MUTEX(); @@ -625,60 +634,60 @@ void Posix_QextSerialPort::setParity(ParityType parity) switch (parity) { /*space parity*/ - case PAR_SPACE: - if (Settings.DataBits==DATA_8) { - TTY_PORTABILITY_WARNING("Posix_QextSerialPort: Space parity is only supported in POSIX with 7 or fewer data bits"); - } - else { - - /*space parity not directly supported - add an extra data bit to simulate it*/ - Posix_CommConfig.c_cflag&=~(PARENB|CSIZE); - switch(Settings.DataBits) { - case DATA_5: - Settings.DataBits=DATA_6; - Posix_CommConfig.c_cflag|=CS6; - break; - - case DATA_6: - Settings.DataBits=DATA_7; - Posix_CommConfig.c_cflag|=CS7; - break; - - case DATA_7: - Settings.DataBits=DATA_8; - Posix_CommConfig.c_cflag|=CS8; - break; - - case DATA_8: - break; - } - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + case PAR_SPACE: + if (Settings.DataBits==DATA_8) { + TTY_PORTABILITY_WARNING("Posix_QextSerialPort: Space parity is only supported in POSIX with 7 or fewer data bits"); + } + else { + + /*space parity not directly supported - add an extra data bit to simulate it*/ + Posix_CommConfig.c_cflag&=~(PARENB|CSIZE); + switch(Settings.DataBits) { + case DATA_5: + Settings.DataBits=DATA_6; + Posix_CommConfig.c_cflag|=CS6; + break; + + case DATA_6: + Settings.DataBits=DATA_7; + Posix_CommConfig.c_cflag|=CS7; + break; + + case DATA_7: + Settings.DataBits=DATA_8; + Posix_CommConfig.c_cflag|=CS8; + break; + + case DATA_8: + break; } - break; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; /*mark parity - WINDOWS ONLY*/ case PAR_MARK: - TTY_WARNING("Posix_QextSerialPort: Mark parity is not supported by POSIX."); - break; + TTY_WARNING("Posix_QextSerialPort: Mark parity is not supported by POSIX."); + break; /*no parity*/ case PAR_NONE: - Posix_CommConfig.c_cflag&=(~PARENB); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + Posix_CommConfig.c_cflag&=(~PARENB); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; /*even parity*/ case PAR_EVEN: - Posix_CommConfig.c_cflag&=(~PARODD); - Posix_CommConfig.c_cflag|=PARENB; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + Posix_CommConfig.c_cflag&=(~PARODD); + Posix_CommConfig.c_cflag|=PARENB; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; /*odd parity*/ case PAR_ODD: - Posix_CommConfig.c_cflag|=(PARENB|PARODD); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + Posix_CommConfig.c_cflag|=(PARENB|PARODD); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; } } UNLOCK_MUTEX(); @@ -713,28 +722,28 @@ void Posix_QextSerialPort::setStopBits(StopBitsType stopBits) switch (stopBits) { /*one stop bit*/ - case STOP_1: - Settings.StopBits=stopBits; - Posix_CommConfig.c_cflag&=(~CSTOPB); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + case STOP_1: + Settings.StopBits=stopBits; + Posix_CommConfig.c_cflag&=(~CSTOPB); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; /*1.5 stop bits*/ - case STOP_1_5: - TTY_WARNING("Posix_QextSerialPort: 1.5 stop bit operation is not supported by POSIX."); - break; + case STOP_1_5: + TTY_WARNING("Posix_QextSerialPort: 1.5 stop bit operation is not supported by POSIX."); + break; /*two stop bits*/ - case STOP_2: - if (Settings.DataBits==DATA_5) { - TTY_WARNING("Posix_QextSerialPort: 2 stop bits cannot be used with 5 data bits"); - } - else { - Settings.StopBits=stopBits; - Posix_CommConfig.c_cflag|=CSTOPB; - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - } - break; + case STOP_2: + if (Settings.DataBits==DATA_5) { + TTY_WARNING("Posix_QextSerialPort: 2 stop bits cannot be used with 5 data bits"); + } + else { + Settings.StopBits=stopBits; + Posix_CommConfig.c_cflag|=CSTOPB; + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + } + break; } } UNLOCK_MUTEX(); @@ -763,24 +772,24 @@ void Posix_QextSerialPort::setFlowControl(FlowType flow) switch(flow) { /*no flow control*/ - case FLOW_OFF: - Posix_CommConfig.c_cflag&=(~CRTSCTS); - Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + case FLOW_OFF: + Posix_CommConfig.c_cflag&=(~CRTSCTS); + Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; /*software (XON/XOFF) flow control*/ - case FLOW_XONXOFF: - Posix_CommConfig.c_cflag&=(~CRTSCTS); - Posix_CommConfig.c_iflag|=(IXON|IXOFF|IXANY); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + case FLOW_XONXOFF: + Posix_CommConfig.c_cflag&=(~CRTSCTS); + Posix_CommConfig.c_iflag|=(IXON|IXOFF|IXANY); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; - case FLOW_HARDWARE: - Posix_CommConfig.c_cflag|=CRTSCTS; - Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); - tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); - break; + case FLOW_HARDWARE: + Posix_CommConfig.c_cflag|=CRTSCTS; + Posix_CommConfig.c_iflag&=(~(IXON|IXOFF|IXANY)); + tcsetattr(fd, TCSAFLUSH, &Posix_CommConfig); + break; } } UNLOCK_MUTEX(); @@ -811,11 +820,11 @@ void Posix_QextSerialPort::setTimeout(long millisec) Posix_Copy_Timeout.tv_usec = millisec % 1000; if (isOpen()) { if (millisec == -1) - fcntl(fd, F_SETFL, O_NDELAY); + fcntl(fd, F_SETFL, O_NDELAY); else - //O_SYNC should enable blocking ::write() - //however this seems not working on Linux 2.6.21 (works on OpenBSD 4.2) - fcntl(fd, F_SETFL, O_SYNC); + //O_SYNC should enable blocking ::write() + //however this seems not working on Linux 2.6.21 (works on OpenBSD 4.2) + fcntl(fd, F_SETFL, O_SYNC); tcgetattr(fd, & Posix_CommConfig); Posix_CommConfig.c_cc[VTIME] = millisec/100; tcsetattr(fd, TCSAFLUSH, & Posix_CommConfig); @@ -969,18 +978,18 @@ Translates a system-specific error code to a QextSerialPort error code. Used in void Posix_QextSerialPort::translateError(ulong error) { switch (error) { - case EBADF: - case ENOTTY: - lastErr=E_INVALID_FD; - break; - - case EINTR: - lastErr=E_CAUGHT_NON_BLOCKED_SIGNAL; - break; - - case ENOMEM: - lastErr=E_NO_MEMORY; - break; + case EBADF: + case ENOTTY: + lastErr=E_INVALID_FD; + break; + + case EINTR: + lastErr=E_CAUGHT_NON_BLOCKED_SIGNAL; + break; + + case ENOMEM: + lastErr=E_NO_MEMORY; + break; } } @@ -1122,7 +1131,7 @@ qint64 Posix_QextSerialPort::writeData(const char * data, qint64 maxSize) int retVal = 0; retVal = ::write(fd, data, maxSize); if (retVal == -1) - lastErr = E_WRITE_FAILED; + lastErr = E_WRITE_FAILED; UNLOCK_MUTEX(); return (qint64)retVal; diff --git a/src/lib/qextserialport/qextserialbase.h b/src/lib/qextserialport/qextserialbase.h index c4656c1d35c69c7478513f70731c6068599fa2c7..0b4a2765185309e8a398658c6907bdee6f9e8a01 100644 --- a/src/lib/qextserialport/qextserialbase.h +++ b/src/lib/qextserialport/qextserialbase.h @@ -92,8 +92,11 @@ enum BaudRateType BAUD57600, BAUD76800, //POSIX ONLY BAUD115200, - BAUD128000, //WINDOWS ONLY - BAUD256000 //WINDOWS ONLY + BAUD128000, // WINDOWS ONLY + BAUD230400, // WINDOWS ONLY + BAUD256000, // WINDOWS ONLY + BAUD460800, // WINDOWS ONLY + BAUD921600 // WINDOWS ONLY }; enum DataBitsType diff --git a/src/lib/qextserialport/win_qextserialport.cpp b/src/lib/qextserialport/win_qextserialport.cpp index 49c5fc8c06a755404d55c3e380ed30f5b7f970e4..39768baf45a54702020ba8f15df4ab5dd0cdb4bb 100644 --- a/src/lib/qextserialport/win_qextserialport.cpp +++ b/src/lib/qextserialport/win_qextserialport.cpp @@ -114,19 +114,19 @@ Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& setFlowControl(settings.FlowControl); setTimeout(settings.Timeout_Millisec); setQueryMode(mode); - init(); + init(); } void Win_QextSerialPort::init() { - _bytesToWrite = 0; - overlap.Internal = 0; - overlap.InternalHigh = 0; - overlap.Offset = 0; - overlap.OffsetHigh = 0; - overlap.hEvent = CreateEvent(NULL, true, false, NULL); - overlapThread = new Win_QextSerialThread(this); - bytesToWriteLock = new QReadWriteLock; + _bytesToWrite = 0; + overlap.Internal = 0; + overlap.InternalHigh = 0; + overlap.Offset = 0; + overlap.OffsetHigh = 0; + overlap.hEvent = CreateEvent(NULL, true, false, NULL); + overlapThread = new Win_QextSerialThread(this); + bytesToWriteLock = new QReadWriteLock; } /*! @@ -178,11 +178,11 @@ if the port associated with the class is already open. The port is also configu settings, as stored in the Settings structure. */ bool Win_QextSerialPort::open(OpenMode mode) { - unsigned long confSize = sizeof(COMMCONFIG); - Win_CommConfig.dwSize = confSize; - DWORD dwFlagsAndAttributes = 0; - if (queryMode() == QextSerialBase::EventDriven) - dwFlagsAndAttributes += FILE_FLAG_OVERLAPPED; + unsigned long confSize = sizeof(COMMCONFIG); + Win_CommConfig.dwSize = confSize; + DWORD dwFlagsAndAttributes = 0; + if (queryMode() == QextSerialBase::EventDriven) + dwFlagsAndAttributes += FILE_FLAG_OVERLAPPED; LOCK_MUTEX(); if (mode == QIODevice::NotOpen) @@ -190,7 +190,7 @@ bool Win_QextSerialPort::open(OpenMode mode) { if (!isOpen()) { /*open the port*/ Win_Handle=CreateFileA(port.toAscii(), GENERIC_READ|GENERIC_WRITE, - FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); + FILE_SHARE_READ|FILE_SHARE_WRITE, NULL, OPEN_EXISTING, dwFlagsAndAttributes, NULL); if (Win_Handle!=INVALID_HANDLE_VALUE) { /*configure port settings*/ GetCommConfig(Win_Handle, &Win_CommConfig, &confSize); @@ -211,24 +211,24 @@ bool Win_QextSerialPort::open(OpenMode mode) { SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); //init event driven approach - if (queryMode() == QextSerialBase::EventDriven) { - Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; - Win_CommTimeouts.ReadTotalTimeoutConstant = 0; - Win_CommTimeouts.WriteTotalTimeoutMultiplier = 0; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; - SetCommTimeouts(Win_Handle, &Win_CommTimeouts); + if (queryMode() == QextSerialBase::EventDriven) { + Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.ReadTotalTimeoutConstant = 0; + Win_CommTimeouts.WriteTotalTimeoutMultiplier = 0; + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + SetCommTimeouts(Win_Handle, &Win_CommTimeouts); if (!SetCommMask( Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { - qWarning("failed to set Comm Mask. Error code: %ld", GetLastError()); - UNLOCK_MUTEX(); - return false; + qWarning("failed to set Comm Mask. Error code: %ld", GetLastError()); + UNLOCK_MUTEX(); + return false; } overlapThread->start(); } - QIODevice::open(mode); + QIODevice::open(mode); } } else { - UNLOCK_MUTEX(); + UNLOCK_MUTEX(); return false; } UNLOCK_MUTEX(); @@ -245,16 +245,16 @@ void Win_QextSerialPort::close() LOCK_MUTEX(); if (isOpen()) { - flush(); - if (overlapThread->isRunning()) { - overlapThread->stop(); - if (QThread::currentThread() != overlapThread) - overlapThread->wait(); - } + flush(); + if (overlapThread->isRunning()) { + overlapThread->stop(); + if (QThread::currentThread() != overlapThread) + overlapThread->wait(); + } if (CloseHandle(Win_Handle)) Win_Handle = INVALID_HANDLE_VALUE; - _bytesToWrite = 0; - QIODevice::close(); + _bytesToWrite = 0; + QIODevice::close(); } UNLOCK_MUTEX(); @@ -353,32 +353,32 @@ is currently open (use isOpen() function to check if port is open). qint64 Win_QextSerialPort::readData(char *data, qint64 maxSize) { DWORD retVal; - + LOCK_MUTEX(); retVal = 0; - if (queryMode() == QextSerialBase::EventDriven) { - OVERLAPPED overlapRead; - overlapRead.Internal = 0; - overlapRead.InternalHigh = 0; - overlapRead.Offset = 0; - overlapRead.OffsetHigh = 0; - overlapRead.hEvent = CreateEvent(NULL, true, false, NULL); - if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapRead)) { - if (GetLastError() == ERROR_IO_PENDING) - GetOverlappedResult(Win_Handle, & overlapRead, & retVal, true); - else { - lastErr = E_READ_FAILED; - retVal = (DWORD)-1; - } - } - CloseHandle(overlapRead.hEvent); - } else if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + if (queryMode() == QextSerialBase::EventDriven) { + OVERLAPPED overlapRead; + overlapRead.Internal = 0; + overlapRead.InternalHigh = 0; + overlapRead.Offset = 0; + overlapRead.OffsetHigh = 0; + overlapRead.hEvent = CreateEvent(NULL, true, false, NULL); + if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapRead)) { + if (GetLastError() == ERROR_IO_PENDING) + GetOverlappedResult(Win_Handle, & overlapRead, & retVal, true); + else { + lastErr = E_READ_FAILED; + retVal = (DWORD)-1; + } + } + CloseHandle(overlapRead.hEvent); + } else if (!ReadFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { lastErr = E_READ_FAILED; retVal = (DWORD)-1; } - UNLOCK_MUTEX(); + UNLOCK_MUTEX(); return (qint64)retVal; } @@ -394,8 +394,8 @@ is currently open (use isOpen() function to check if port is open). */ qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) { - DWORD retVal; - + DWORD retVal; + LOCK_MUTEX(); retVal = 0; @@ -403,21 +403,21 @@ qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) bytesToWriteLock->lockForWrite(); _bytesToWrite += maxSize; bytesToWriteLock->unlock(); - overlapWrite.Internal = 0; - overlapWrite.InternalHigh = 0; - overlapWrite.Offset = 0; - overlapWrite.OffsetHigh = 0; - overlapWrite.hEvent = CreateEvent(NULL, true, false, NULL); - if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapWrite)) { - lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; - } else - retVal = maxSize; + overlapWrite.Internal = 0; + overlapWrite.InternalHigh = 0; + overlapWrite.Offset = 0; + overlapWrite.OffsetHigh = 0; + overlapWrite.hEvent = CreateEvent(NULL, true, false, NULL); + if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, & overlapWrite)) { + lastErr = E_WRITE_FAILED; + retVal = (DWORD)-1; + } else + retVal = maxSize; } else if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { - lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; - } - + lastErr = E_WRITE_FAILED; + retVal = (DWORD)-1; + } + UNLOCK_MUTEX(); return (qint64)retVal; @@ -453,30 +453,30 @@ void Win_QextSerialPort::setFlowControl(FlowType flow) { switch(flow) { /*no flow control*/ - case FLOW_OFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + case FLOW_OFF: + Win_CommConfig.dcb.fOutxCtsFlow=FALSE; + Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX=FALSE; + Win_CommConfig.dcb.fOutX=FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; /*software (XON/XOFF) flow control*/ - case FLOW_XONXOFF: - Win_CommConfig.dcb.fOutxCtsFlow=FALSE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; - Win_CommConfig.dcb.fInX=TRUE; - Win_CommConfig.dcb.fOutX=TRUE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + case FLOW_XONXOFF: + Win_CommConfig.dcb.fOutxCtsFlow=FALSE; + Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_DISABLE; + Win_CommConfig.dcb.fInX=TRUE; + Win_CommConfig.dcb.fOutX=TRUE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; - case FLOW_HARDWARE: - Win_CommConfig.dcb.fOutxCtsFlow=TRUE; - Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_HANDSHAKE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + case FLOW_HARDWARE: + Win_CommConfig.dcb.fOutxCtsFlow=TRUE; + Win_CommConfig.dcb.fRtsControl=RTS_CONTROL_HANDSHAKE; + Win_CommConfig.dcb.fInX=FALSE; + Win_CommConfig.dcb.fOutX=FALSE; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; } } UNLOCK_MUTEX(); @@ -503,33 +503,33 @@ void Win_QextSerialPort::setParity(ParityType parity) { switch (parity) { /*space parity*/ - case PAR_SPACE: - if (Settings.DataBits==DATA_8) { - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); - } - Win_CommConfig.dcb.fParity=TRUE; - break; + case PAR_SPACE: + if (Settings.DataBits==DATA_8) { + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Space parity with 8 data bits is not supported by POSIX systems."); + } + Win_CommConfig.dcb.fParity=TRUE; + break; /*mark parity - WINDOWS ONLY*/ case PAR_MARK: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); - Win_CommConfig.dcb.fParity=TRUE; - break; + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: Mark parity is not supported by POSIX systems"); + Win_CommConfig.dcb.fParity=TRUE; + break; /*no parity*/ case PAR_NONE: - Win_CommConfig.dcb.fParity=FALSE; - break; + Win_CommConfig.dcb.fParity=FALSE; + break; /*even parity*/ case PAR_EVEN: - Win_CommConfig.dcb.fParity=TRUE; - break; + Win_CommConfig.dcb.fParity=TRUE; + break; /*odd parity*/ case PAR_ODD: - Win_CommConfig.dcb.fParity=TRUE; - break; + Win_CommConfig.dcb.fParity=TRUE; + break; } SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); } @@ -570,48 +570,48 @@ void Win_QextSerialPort::setDataBits(DataBitsType dataBits) { switch(dataBits) { /*5 data bits*/ - case DATA_5: - if (Settings.StopBits==STOP_2) { - TTY_WARNING("Win_QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=5; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + case DATA_5: + if (Settings.StopBits==STOP_2) { + TTY_WARNING("Win_QextSerialPort: 5 Data bits cannot be used with 2 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=5; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; /*6 data bits*/ case DATA_6: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Win_QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=6; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Win_QextSerialPort: 6 Data bits cannot be used with 1.5 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=6; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; /*7 data bits*/ case DATA_7: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Win_QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=7; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Win_QextSerialPort: 7 Data bits cannot be used with 1.5 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=7; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; /*8 data bits*/ case DATA_8: - if (Settings.StopBits==STOP_1_5) { - TTY_WARNING("Win_QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); - } - else { - Win_CommConfig.dcb.ByteSize=8; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + if (Settings.StopBits==STOP_1_5) { + TTY_WARNING("Win_QextSerialPort: 8 Data bits cannot be used with 1.5 stop bits."); + } + else { + Win_CommConfig.dcb.ByteSize=8; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; } } UNLOCK_MUTEX(); @@ -649,33 +649,33 @@ void Win_QextSerialPort::setStopBits(StopBitsType stopBits) { switch (stopBits) { /*one stop bit*/ - case STOP_1: - Win_CommConfig.dcb.StopBits=ONESTOPBIT; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - break; + case STOP_1: + Win_CommConfig.dcb.StopBits=ONESTOPBIT; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + break; /*1.5 stop bits*/ - case STOP_1_5: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); - if (Settings.DataBits!=DATA_5) { - TTY_WARNING("Win_QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=ONE5STOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + case STOP_1_5: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: 1.5 stop bit operation is not supported by POSIX."); + if (Settings.DataBits!=DATA_5) { + TTY_WARNING("Win_QextSerialPort: 1.5 stop bits can only be used with 5 data bits"); + } + else { + Win_CommConfig.dcb.StopBits=ONE5STOPBITS; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; /*two stop bits*/ case STOP_2: - if (Settings.DataBits==DATA_5) { - TTY_WARNING("Win_QextSerialPort: 2 stop bits cannot be used with 5 data bits"); - } - else { - Win_CommConfig.dcb.StopBits=TWOSTOPBITS; - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - break; + if (Settings.DataBits==DATA_5) { + TTY_WARNING("Win_QextSerialPort: 2 stop bits cannot be used with 5 data bits"); + } + else { + Win_CommConfig.dcb.StopBits=TWOSTOPBITS; + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + break; } } UNLOCK_MUTEX(); @@ -713,156 +713,175 @@ are speeds that are usable on both Windows and POSIX. *BAUD115200 115200 115200 BAUD128000 128000 115200 BAUD256000 256000 115200 + BAUD230400 230400 115200 + BAUD256000 256000 115200 + BAUD460800 460800 115200 + BAUD921600 921600 115200 \endverbatim */ void Win_QextSerialPort::setBaudRate(BaudRateType baudRate) { LOCK_MUTEX(); if (Settings.BaudRate!=baudRate) { switch (baudRate) { - case BAUD50: - case BAUD75: - case BAUD134: - case BAUD150: - case BAUD200: - Settings.BaudRate=BAUD110; - break; - - case BAUD1800: - Settings.BaudRate=BAUD1200; - break; - - case BAUD76800: - Settings.BaudRate=BAUD57600; - break; - - default: - Settings.BaudRate=baudRate; - break; + case BAUD50: + case BAUD75: + case BAUD134: + case BAUD150: + case BAUD200: + Settings.BaudRate=BAUD110; + break; + + case BAUD1800: + Settings.BaudRate=BAUD1200; + break; + + case BAUD76800: + Settings.BaudRate=BAUD57600; + break; + + default: + Settings.BaudRate=baudRate; + break; } } if (isOpen()) { switch (baudRate) { /*50 baud*/ - case BAUD50: - TTY_WARNING("Win_QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + case BAUD50: + TTY_WARNING("Win_QextSerialPort: Windows does not support 50 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; /*75 baud*/ - case BAUD75: - TTY_WARNING("Win_QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + case BAUD75: + TTY_WARNING("Win_QextSerialPort: Windows does not support 75 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; /*110 baud*/ - case BAUD110: - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + case BAUD110: + Win_CommConfig.dcb.BaudRate=CBR_110; + break; /*134.5 baud*/ - case BAUD134: - TTY_WARNING("Win_QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + case BAUD134: + TTY_WARNING("Win_QextSerialPort: Windows does not support 134.5 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; /*150 baud*/ - case BAUD150: - TTY_WARNING("Win_QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + case BAUD150: + TTY_WARNING("Win_QextSerialPort: Windows does not support 150 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; /*200 baud*/ - case BAUD200: - TTY_WARNING("Win_QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); - Win_CommConfig.dcb.BaudRate=CBR_110; - break; + case BAUD200: + TTY_WARNING("Win_QextSerialPort: Windows does not support 200 baud operation. Switching to 110 baud."); + Win_CommConfig.dcb.BaudRate=CBR_110; + break; /*300 baud*/ - case BAUD300: - Win_CommConfig.dcb.BaudRate=CBR_300; - break; + case BAUD300: + Win_CommConfig.dcb.BaudRate=CBR_300; + break; /*600 baud*/ - case BAUD600: - Win_CommConfig.dcb.BaudRate=CBR_600; - break; + case BAUD600: + Win_CommConfig.dcb.BaudRate=CBR_600; + break; /*1200 baud*/ - case BAUD1200: - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; + case BAUD1200: + Win_CommConfig.dcb.BaudRate=CBR_1200; + break; /*1800 baud*/ - case BAUD1800: - TTY_WARNING("Win_QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); - Win_CommConfig.dcb.BaudRate=CBR_1200; - break; + case BAUD1800: + TTY_WARNING("Win_QextSerialPort: Windows does not support 1800 baud operation. Switching to 1200 baud."); + Win_CommConfig.dcb.BaudRate=CBR_1200; + break; /*2400 baud*/ - case BAUD2400: - Win_CommConfig.dcb.BaudRate=CBR_2400; - break; + case BAUD2400: + Win_CommConfig.dcb.BaudRate=CBR_2400; + break; /*4800 baud*/ - case BAUD4800: - Win_CommConfig.dcb.BaudRate=CBR_4800; - break; + case BAUD4800: + Win_CommConfig.dcb.BaudRate=CBR_4800; + break; /*9600 baud*/ - case BAUD9600: - Win_CommConfig.dcb.BaudRate=CBR_9600; - break; + case BAUD9600: + Win_CommConfig.dcb.BaudRate=CBR_9600; + break; /*14400 baud*/ - case BAUD14400: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_14400; - break; + case BAUD14400: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 14400 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_14400; + break; /*19200 baud*/ - case BAUD19200: - Win_CommConfig.dcb.BaudRate=CBR_19200; - break; + case BAUD19200: + Win_CommConfig.dcb.BaudRate=CBR_19200; + break; /*38400 baud*/ - case BAUD38400: - Win_CommConfig.dcb.BaudRate=CBR_38400; - break; + case BAUD38400: + Win_CommConfig.dcb.BaudRate=CBR_38400; + break; /*56000 baud*/ - case BAUD56000: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_56000; - break; + case BAUD56000: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 56000 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_56000; + break; /*57600 baud*/ - case BAUD57600: - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; + case BAUD57600: + Win_CommConfig.dcb.BaudRate=CBR_57600; + break; /*76800 baud*/ - case BAUD76800: - TTY_WARNING("Win_QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); - Win_CommConfig.dcb.BaudRate=CBR_57600; - break; + case BAUD76800: + TTY_WARNING("Win_QextSerialPort: Windows does not support 76800 baud operation. Switching to 57600 baud."); + Win_CommConfig.dcb.BaudRate=CBR_57600; + break; /*115200 baud*/ - case BAUD115200: - Win_CommConfig.dcb.BaudRate=CBR_115200; - break; + case BAUD115200: + Win_CommConfig.dcb.BaudRate=CBR_115200; + break; /*128000 baud*/ - case BAUD128000: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_128000; - break; + case BAUD128000: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 128000 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_128000; + break; /*256000 baud*/ - case BAUD256000: - TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); - Win_CommConfig.dcb.BaudRate=CBR_256000; - break; + case BAUD256000: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 256000 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_256000; + break; + /*230400 baud*/ + case BAUD230400: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 230400 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_230400; + break; + /*460800 baud*/ + case BAUD460800: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 460800 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_460800; + break; + /*921600 baud*/ + case BAUD921600: + TTY_PORTABILITY_WARNING("Win_QextSerialPort Portability Warning: POSIX does not support 921600 baud operation."); + Win_CommConfig.dcb.BaudRate=CBR_921600; + break; } SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); } @@ -948,55 +967,55 @@ ulong Win_QextSerialPort::lineStatus(void) { bool Win_QextSerialPort::waitForReadyRead(int msecs) { - //@todo implement - return false; + //@todo implement + return false; } qint64 Win_QextSerialPort::bytesToWrite() const { - return _bytesToWrite; + return _bytesToWrite; } void Win_QextSerialPort::monitorCommEvent() { - DWORD eventMask = 0; - - ResetEvent(overlap.hEvent); - if (!WaitCommEvent(Win_Handle, & eventMask, & overlap)) - if (GetLastError() != ERROR_IO_PENDING) - qCritical("WaitCommEvent error %ld\n", GetLastError()); - - if (WaitForSingleObject(overlap.hEvent, INFINITE) == WAIT_OBJECT_0) { - //overlap event occured - DWORD undefined; - if (!GetOverlappedResult(Win_Handle, & overlap, & undefined, false)) { - qWarning("CommEvent overlapped error %ld", GetLastError()); - return; - } - if (eventMask & EV_RXCHAR) { - if (sender() != this) - emit readyRead(); - } - if (eventMask & EV_TXEMPTY) { - DWORD numBytes; - GetOverlappedResult(Win_Handle, & overlapWrite, & numBytes, true); - bytesToWriteLock->lockForWrite(); - if (sender() != this) - emit bytesWritten(bytesToWrite()); - _bytesToWrite = 0; - bytesToWriteLock->unlock(); - } - if (eventMask & EV_DSR) - if (lineStatus() & LS_DSR) - emit dsrChanged(true); - else - emit dsrChanged(false); - } + DWORD eventMask = 0; + + ResetEvent(overlap.hEvent); + if (!WaitCommEvent(Win_Handle, & eventMask, & overlap)) + if (GetLastError() != ERROR_IO_PENDING) + qCritical("WaitCommEvent error %ld\n", GetLastError()); + + if (WaitForSingleObject(overlap.hEvent, INFINITE) == WAIT_OBJECT_0) { + //overlap event occured + DWORD undefined; + if (!GetOverlappedResult(Win_Handle, & overlap, & undefined, false)) { + qWarning("CommEvent overlapped error %ld", GetLastError()); + return; + } + if (eventMask & EV_RXCHAR) { + if (sender() != this) + emit readyRead(); + } + if (eventMask & EV_TXEMPTY) { + DWORD numBytes; + GetOverlappedResult(Win_Handle, & overlapWrite, & numBytes, true); + bytesToWriteLock->lockForWrite(); + if (sender() != this) + emit bytesWritten(bytesToWrite()); + _bytesToWrite = 0; + bytesToWriteLock->unlock(); + } + if (eventMask & EV_DSR) + if (lineStatus() & LS_DSR) + emit dsrChanged(true); + else + emit dsrChanged(false); + } } void Win_QextSerialPort::terminateCommWait() { - SetCommMask(Win_Handle, 0); + SetCommMask(Win_Handle, 0); } @@ -1013,17 +1032,17 @@ void Win_QextSerialPort::setTimeout(long millisec) { LOCK_MUTEX(); Settings.Timeout_Millisec = millisec; - if (millisec == -1) { - Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; - Win_CommTimeouts.ReadTotalTimeoutConstant = 0; - } else { - Win_CommTimeouts.ReadIntervalTimeout = millisec; - Win_CommTimeouts.ReadTotalTimeoutConstant = millisec; - } - Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; - Win_CommTimeouts.WriteTotalTimeoutMultiplier = millisec; - Win_CommTimeouts.WriteTotalTimeoutConstant = 0; - if (queryMode() != QextSerialBase::EventDriven) + if (millisec == -1) { + Win_CommTimeouts.ReadIntervalTimeout = MAXDWORD; + Win_CommTimeouts.ReadTotalTimeoutConstant = 0; + } else { + Win_CommTimeouts.ReadIntervalTimeout = millisec; + Win_CommTimeouts.ReadTotalTimeoutConstant = millisec; + } + Win_CommTimeouts.ReadTotalTimeoutMultiplier = 0; + Win_CommTimeouts.WriteTotalTimeoutMultiplier = millisec; + Win_CommTimeouts.WriteTotalTimeoutConstant = 0; + if (queryMode() != QextSerialBase::EventDriven) SetCommTimeouts(Win_Handle, &Win_CommTimeouts); UNLOCK_MUTEX(); @@ -1033,19 +1052,19 @@ void Win_QextSerialPort::setTimeout(long millisec) { Win_QextSerialThread::Win_QextSerialThread(Win_QextSerialPort * qesp): QThread() { - this->qesp = qesp; - terminate = false; + this->qesp = qesp; + terminate = false; } void Win_QextSerialThread::stop() { - terminate = true; - qesp->terminateCommWait(); + terminate = true; + qesp->terminateCommWait(); } void Win_QextSerialThread::run() { - while (!terminate) - qesp->monitorCommEvent(); - terminate = false; + while (!terminate) + qesp->monitorCommEvent(); + terminate = false; } diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 209838809b435d955968372a4884e106d75ef139..3a3b61782e9ed059ee01cc6507381fbe6594fd7b 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1,24 +1,4 @@ -/*===================================================================== - -QGroundControl Open Source Ground Control Station - -(c) 2009, 2010 QGROUNDCONTROL PROJECT - -This file is part of the QGROUNDCONTROL project - - QGROUNDCONTROL is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - QGROUNDCONTROL is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with QGROUNDCONTROL. If not, see . - +/*=================================================================== ======================================================================*/ /** @@ -48,46 +28,46 @@ This file is part of the QGROUNDCONTROL project UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), - uasId(id), - startTime(MG::TIME::getGroundTimeNow()), - commStatus(COMM_DISCONNECTED), - name(""), - autopilot(-1), - links(new QList()), - unknownPackets(), - mavlink(protocol), - waypointManager(*this), - thrustSum(0), - thrustMax(10), - startVoltage(0), - currentVoltage(12.0f), - lpVoltage(12.0f), - mode(MAV_MODE_UNINIT), - status(MAV_STATE_UNINIT), - onboardTimeOffset(0), - controlRollManual(true), - controlPitchManual(true), - controlYawManual(true), - controlThrustManual(true), - manualRollAngle(0), - manualPitchAngle(0), - manualYawAngle(0), - manualThrust(0), - receiveDropRate(0), - sendDropRate(0), - lowBattAlarm(false), - positionLock(false), - localX(0.0), - localY(0.0), - localZ(0.0), - latitude(0.0), - longitude(0.0), - altitude(0.0), - roll(0.0), - pitch(0.0), - yaw(0.0), - statusTimeout(new QTimer(this)), - paramsOnceRequested(false) +uasId(id), +startTime(MG::TIME::getGroundTimeNow()), +commStatus(COMM_DISCONNECTED), +name(""), +autopilot(-1), +links(new QList()), +unknownPackets(), +mavlink(protocol), +waypointManager(*this), +thrustSum(0), +thrustMax(10), +startVoltage(0), +currentVoltage(12.0f), +lpVoltage(12.0f), +mode(MAV_MODE_UNINIT), +status(MAV_STATE_UNINIT), +onboardTimeOffset(0), +controlRollManual(true), +controlPitchManual(true), +controlYawManual(true), +controlThrustManual(true), +manualRollAngle(0), +manualPitchAngle(0), +manualYawAngle(0), +manualThrust(0), +receiveDropRate(0), +sendDropRate(0), +lowBattAlarm(false), +positionLock(false), +localX(0.0), +localY(0.0), +localZ(0.0), +latitude(0.0), +longitude(0.0), +altitude(0.0), +roll(0.0), +pitch(0.0), +yaw(0.0), +statusTimeout(new QTimer(this)), +paramsOnceRequested(false) { color = UASInterface::getNextColor(); setBattery(LIPOLY, 3); @@ -134,14 +114,14 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) if (!links->contains(link)) { addLink(link); -// qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); + // qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); } -// else -// { -// qDebug() << __FILE__ << __LINE__ << "DID NOT ADD LINK" << link->getName() << "ALREADY IN LIST"; -// } + // else + // { + // qDebug() << __FILE__ << __LINE__ << "DID NOT ADD LINK" << link->getName() << "ALREADY IN LIST"; + // } -// qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq; + // qDebug() << "UAS RECEIVED from" << message.sysid << "component" << message.compid << "msg id" << message.msgid << "seq no" << message.seq; if (message.sysid == uasId) { @@ -188,7 +168,7 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit statusChanged(this, uasState, stateDescription); emit statusChanged(this->status); emit loadChanged(this,state.load/10.0f); - emit UAS::valueChanged(this, "Load", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); + emit UAS::valueChanged(uasId, "Load", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); stateAudio = " changed status to " + uasState; } @@ -289,9 +269,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, "Accel. X", raw.xacc, time); emit valueChanged(uasId, "Accel. Y", raw.yacc, time); emit valueChanged(uasId, "Accel. Z", raw.zacc, time); - emit valueChanged(uasId, "Gyro Phi", raw.xgyro, time); - emit valueChanged(uasId, "Gyro Theta", raw.ygyro, time); - emit valueChanged(uasId, "Gyro Psi", raw.zgyro, time); + emit valueChanged(uasId, "Gyro Phi", static_cast(raw.xgyro), time); + emit valueChanged(uasId, "Gyro Theta", static_cast(raw.ygyro), time); + emit valueChanged(uasId, "Gyro Psi", static_cast(raw.zgyro), time); emit valueChanged(uasId, "Mag. X", raw.xmag, time); emit valueChanged(uasId, "Mag. Y", raw.ymag, time); emit valueChanged(uasId, "Mag. Z", raw.zmag, time); @@ -307,15 +287,24 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) roll = attitude.roll; pitch = attitude.pitch; yaw = attitude.yaw; +// emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); +// emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); +// emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); emit valueChanged(uasId, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); emit valueChanged(uasId, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); emit valueChanged(uasId, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(this, "roll IMU", mavlink_msg_attitude_get_roll(&message), time); - emit valueChanged(this, "pitch IMU", mavlink_msg_attitude_get_pitch(&message), time); - emit valueChanged(this, "yaw IMU", mavlink_msg_attitude_get_yaw(&message), time); emit valueChanged(uasId, "rollspeed IMU", attitude.rollspeed, time); emit valueChanged(uasId, "pitchspeed IMU", attitude.pitchspeed, time); emit valueChanged(uasId, "yawspeed IMU", attitude.yawspeed, time); + + // Emit in angles + emit valueChanged(uasId, "roll (deg)", (attitude.roll/M_PI)*180.0, time); + emit valueChanged(uasId, "pitch (deg)", (attitude.pitch/M_PI)*180.0, time); + emit valueChanged(uasId, "yaw (deg)", (attitude.yaw/M_PI)*180.0, time); + emit valueChanged(uasId, "roll V (deg/s)", (attitude.rollspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "pitch V (deg/s)", (attitude.pitchspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "yaw V (deg/s)", (attitude.yawspeed/M_PI)*180.0, time); + emit attitudeChanged(this, mavlink_msg_attitude_get_roll(&message), mavlink_msg_attitude_get_pitch(&message), mavlink_msg_attitude_get_yaw(&message), time); } break; @@ -338,8 +327,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit localPositionChanged(this, pos.x, pos.y, pos.z, time); emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); -// qDebug()<<"Local Position = "<getUnixTime(0)); + emit valueChanged(uasId, "Diff pressure 1", pressure.press_diff1, this->getUnixTime(0)); + emit valueChanged(uasId, "Diff pressure 2", pressure.press_diff2, this->getUnixTime(0)); + } + break; case MAVLINK_MSG_ID_RC_CHANNELS_RAW: { mavlink_rc_channels_raw_t channels; @@ -612,22 +610,22 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) emit valueChanged(uasId, str+".z", vect.z, time); } break; -//#ifdef MAVLINK_ENABLED_PIXHAWK -// case MAVLINK_MSG_ID_POINT_OF_INTEREST: -// { -// mavlink_point_of_interest_t poi; -// mavlink_msg_point_of_interest_decode(&message, &poi); -// emit poiFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN), poi.x, poi.y, poi.z); -// } -// break; -// case MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION: -// { -// mavlink_point_of_interest_connection_t poi; -// mavlink_msg_point_of_interest_connection_decode(&message, &poi); -// emit poiConnectionFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN), poi.x1, poi.y1, poi.z1, poi.x2, poi.y2, poi.z2); -// } -// break; -//#endif + //#ifdef MAVLINK_ENABLED_PIXHAWK + // case MAVLINK_MSG_ID_POINT_OF_INTEREST: + // { + // mavlink_point_of_interest_t poi; + // mavlink_msg_point_of_interest_decode(&message, &poi); + // emit poiFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN), poi.x, poi.y, poi.z); + // } + // break; + // case MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION: + // { + // mavlink_point_of_interest_connection_t poi; + // mavlink_msg_point_of_interest_connection_decode(&message, &poi); + // emit poiConnectionFound(this, poi.type, poi.color, QString((QChar*)poi.name, MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN), poi.x1, poi.y1, poi.z1, poi.x2, poi.y2, poi.z2); + // } + // break; + //#endif #ifdef MAVLINK_ENABLED_UALBERTA case MAVLINK_MSG_ID_NAV_FILTER_BIAS: { @@ -720,58 +718,58 @@ void UAS::setLocalOriginAtCurrentGPSPosition() void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) { - #ifdef MAVLINK_ENABLED_PIXHAWK +#ifdef MAVLINK_ENABLED_PIXHAWK mavlink_message_t msg; mavlink_msg_position_control_setpoint_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, 0, x, y, z, yaw); sendMessage(msg); - #else +#else Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); - #endif +#endif } void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) { #ifdef MAVLINK_ENABLED_PIXHAWK - mavlink_message_t msg; - mavlink_msg_position_control_offset_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw); - sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_position_control_offset_set_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, x, y, z, yaw); + sendMessage(msg); #else -Q_UNUSED(x); -Q_UNUSED(y); -Q_UNUSED(z); -Q_UNUSED(yaw); + Q_UNUSED(x); + Q_UNUSED(y); + Q_UNUSED(z); + Q_UNUSED(yaw); #endif } void UAS::startRadioControlCalibration() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_RC); - sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_RC); + sendMessage(msg); } void UAS::startDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START); - sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_START); + sendMessage(msg); } void UAS::pauseDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE); - sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_PAUSE); + sendMessage(msg); } void UAS::stopDataRecording() { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); - sendMessage(msg); + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_REC_STOP); + sendMessage(msg); } void UAS::startMagnetometerCalibration() @@ -820,9 +818,9 @@ quint64 UAS::getUnixTime(quint64 time) #ifndef _MSC_VER else if (time < 1261440000000000LLU) #else - else if (time < 1261440000000000) + else if (time < 1261440000000000) #endif - { + { if (onboardTimeOffset == 0) { onboardTimeOffset = MG::TIME::getGroundTimeNow() - time/1000; @@ -1333,15 +1331,15 @@ void UAS::setManualControlCommands(double roll, double pitch, double yaw, double manualYawAngle = yaw * yawScaling; manualThrust = thrust * thrustScaling; -// if(mode == (int)MAV_MODE_MANUAL) -// { - mavlink_message_t message; - mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); - sendMessage(message); - qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; + // if(mode == (int)MAV_MODE_MANUAL) + // { + mavlink_message_t message; + mavlink_msg_manual_control_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &message, this->uasId, (float)manualRollAngle, (float)manualPitchAngle, (float)manualYawAngle, (float)manualThrust, controlRollManual, controlPitchManual, controlYawManual, controlThrustManual); + sendMessage(message); + qDebug() << __FILE__ << __LINE__ << ": SENT MANUAL CONTROL MESSAGE: roll" << manualRollAngle << " pitch: " << manualPitchAngle << " yaw: " << manualYawAngle << " thrust: " << manualThrust; - emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow()); -// } + emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, MG::TIME::getGroundTimeNow()); + // } } int UAS::getSystemType() @@ -1363,7 +1361,7 @@ void UAS::receiveButton(int buttonIndex) break; } -// qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!"; + // qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!"; } diff --git a/src/ui/HDDisplay.cc b/src/ui/HDDisplay.cc index 1192494028880a965092f5a44ac8942f679eeae5..a73bbccc48349e3e5d4e2f7d2911cc4917092453 100644 --- a/src/ui/HDDisplay.cc +++ b/src/ui/HDDisplay.cc @@ -1,24 +1,4 @@ /*===================================================================== - -PIXHAWK Micro Air Vehicle Flying Robotics Toolkit - -(c) 2009, 2010 PIXHAWK PROJECT - -This file is part of the PIXHAWK project - - PIXHAWK is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - PIXHAWK is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with PIXHAWK. If not, see . - ======================================================================*/ /** @@ -33,7 +13,11 @@ This file is part of the PIXHAWK project #include #include #include +#include +#include #include +#include +#include #include "UASManager.h" #include "HDDisplay.h" #include "ui_HDDisplay.h" @@ -44,7 +28,7 @@ This file is part of the PIXHAWK project #endif #include -HDDisplay::HDDisplay(QStringList* plotList, QWidget *parent) : +HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : QGraphicsView(parent), uas(NULL), values(QMap()), @@ -73,52 +57,61 @@ HDDisplay::HDDisplay(QStringList* plotList, QWidget *parent) : strongStrokeWidth(1.5f), normalStrokeWidth(1.0f), fineStrokeWidth(0.5f), - acceptList(plotList), + acceptList(new QStringList()), lastPaintTime(0), + columns(3), m_ui(new Ui::HDDisplay) { + setWindowTitle(title); //m_ui->setupUi(this); - // Check if acceptlist exists - if (!acceptList) + // Add all items in accept list to gauge + if (plotList) { - acceptList = new QStringList(); + for(int i = 0; i < plotList->length(); ++i) + { + addGauge(plotList->at(i)); + } } -// setBackgroundBrush(QBrush(backgroundColor)); -// setDragMode(QGraphicsView::ScrollHandDrag); -// setCacheMode(QGraphicsView::CacheBackground); -// // FIXME Handle full update with care - ressource intensive -// setViewportUpdateMode(QGraphicsView::FullViewportUpdate); -// -// setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform); -// -// //Set-up the scene -// QGraphicsScene* Scene = new QGraphicsScene(this); -// setScene(Scene); -// -// //Populate the scene -// for(int x = 0; x < 1000; x = x + 25) { -// for(int y = 0; y < 1000; y = y + 25) { -// -// if(x % 100 == 0 && y % 100 == 0) { -// Scene->addRect(x, y, 2, 2); -// -// QString pointString; -// QTextStream stream(&pointString); -// stream << "(" << x << "," << y << ")"; -// QGraphicsTextItem* item = Scene->addText(pointString); -// item->setPos(x, y); -// } else { -// Scene->addRect(x, y, 1, 1); -// } -// } -// } -// -// //Set-up the view -// setSceneRect(0, 0, 1000, 1000); -// setCenter(QPointF(500.0, 500.0)); //A modified version of centerOn(), handles special cases -// setCursor(Qt::OpenHandCursor); + restoreState(); + + createActions(); + + // setBackgroundBrush(QBrush(backgroundColor)); + // setDragMode(QGraphicsView::ScrollHandDrag); + // setCacheMode(QGraphicsView::CacheBackground); + // // FIXME Handle full update with care - ressource intensive + // setViewportUpdateMode(QGraphicsView::FullViewportUpdate); + // + // setRenderHints(QPainter::Antialiasing | QPainter::SmoothPixmapTransform); + // + // //Set-up the scene + // QGraphicsScene* Scene = new QGraphicsScene(this); + // setScene(Scene); + // + // //Populate the scene + // for(int x = 0; x < 1000; x = x + 25) { + // for(int y = 0; y < 1000; y = y + 25) { + // + // if(x % 100 == 0 && y % 100 == 0) { + // Scene->addRect(x, y, 2, 2); + // + // QString pointString; + // QTextStream stream(&pointString); + // stream << "(" << x << "," << y << ")"; + // QGraphicsTextItem* item = Scene->addText(pointString); + // item->setPos(x, y); + // } else { + // Scene->addRect(x, y, 1, 1); + // } + // } + // } + // + // //Set-up the view + // setSceneRect(0, 0, 1000, 1000); + // setCenter(QPointF(500.0, 500.0)); //A modified version of centerOn(), handles special cases + // setCursor(Qt::OpenHandCursor); this->setMinimumHeight(125); @@ -147,12 +140,13 @@ HDDisplay::HDDisplay(QStringList* plotList, QWidget *parent) : HDDisplay::~HDDisplay() { + saveState(); delete m_ui; } void HDDisplay::enableGLRendering(bool enable) { - Q_UNUSED(enable) + Q_UNUSED(enable); } void HDDisplay::triggerUpdate() @@ -170,6 +164,172 @@ void HDDisplay::paintEvent(QPaintEvent * event) renderOverlay(); } +void HDDisplay::contextMenuEvent (QContextMenuEvent* event) +{ + QMenu menu(this); + menu.addAction(addGaugeAction); + menu.addActions(getItemRemoveActions()); + menu.addSeparator(); + menu.addAction(setColumnsAction); + menu.addAction(setTitleAction); + menu.exec(event->globalPos()); +} + +void HDDisplay::saveState() +{ + QSettings settings; + + QString instruments; + // Restore instrument settings + for (int i = 0; i < acceptList->count(); i++) + { + QString key = acceptList->at(i); + instruments += "|" + QString::number(minValues.value(key, -1.0))+","+key+","+QString::number(maxValues.value(key, +1.0)); + } + + qDebug() << "Saving" << instruments; + + settings.setValue(windowTitle()+"_gauges", instruments); + settings.sync(); +} + +void HDDisplay::restoreState() +{ + QSettings settings; + settings.sync(); + + QStringList instruments = settings.value(windowTitle()+"_gauges").toString().split('|'); + for (int i = 0; i < instruments.count(); i++) + { + addGauge(instruments.at(i)); + } +} + +QList HDDisplay::getItemRemoveActions() +{ + QList actions; + for(int i = 0; i < acceptList->length(); ++i) + { + QString gauge = acceptList->at(i); + QAction* remove = new QAction(tr("Remove %1 gauge").arg(gauge), this); + remove->setStatusTip(tr("Removes the %1 gauge from the view.").arg(gauge)); + remove->setData(gauge); + connect(remove, SIGNAL(triggered()), this, SLOT(removeItemByAction())); + actions.append(remove); + } + return actions; +} + +void HDDisplay::removeItemByAction() +{ + QAction* trigger = qobject_cast(QObject::sender()); + if (trigger) + { + QString item = trigger->data().toString(); + int index = acceptList->indexOf(item); + acceptList->removeAt(index); + minValues.remove(item); + maxValues.remove(item); + } +} + +void HDDisplay::addGauge() +{ + QStringList items; + for (int i = 0; i < values.count(); ++i) + { + items.append(QString("%1,%2,%3").arg("-180").arg(values.keys().at(i)).arg("+180")); + } + bool ok; + QString item = QInputDialog::getItem(this, tr("Add Gauge Instrument"), + tr("Format: min, curve name, max"), items, 0, true, &ok); + if (ok && !item.isEmpty()) + { + addGauge(item); + } +} + +void HDDisplay::addGauge(const QString& gauge) +{ + if (gauge.length() > 0) + { + QStringList parts = gauge.split(','); + if (parts.count() > 1) + { + double val; + bool ok; + + QString key = parts.at(1); + + if (!acceptList->contains(key)) + { + // Convert min to double number + val = parts.first().toDouble(&ok); + if (ok) minValues.insert(key, val); + // Convert max to double number + val = parts.last().toDouble(&ok); + if (ok) maxValues.insert(key, val); + // Add value to acceptlist + acceptList->append(key); + } + } + else + { + if (!acceptList->contains(gauge)) + { + acceptList->append(parts.first()); + } + } + } +} + +void HDDisplay::createActions() +{ + addGaugeAction = new QAction(tr("New &Gauge"), this); + addGaugeAction->setStatusTip(tr("Add a new gauge to the view by adding its name from the linechart")); + connect(addGaugeAction, SIGNAL(triggered()), this, SLOT(addGauge())); + + setTitleAction = new QAction(tr("Set Widget Title"), this); + setTitleAction->setStatusTip(tr("Set the title caption of this tool widget")); + connect(setTitleAction, SIGNAL(triggered()), this, SLOT(setTitle())); + + setColumnsAction = new QAction(tr("Set Number of Instrument Columns"), this); + setColumnsAction->setStatusTip(tr("Set number of columns to draw")); + connect(setColumnsAction, SIGNAL(triggered()), this, SLOT(setColumns())); +} + + +void HDDisplay::setColumns() +{ + bool ok; + int i = QInputDialog::getInt(this, tr("Number of Instrument Columns"), + tr("Columns:"), columns, 1, 15, 1, &ok); + if (ok) + { + columns = i; + } +} + +void HDDisplay::setColumns(int cols) +{ + columns = cols; +} + +void HDDisplay::setTitle() +{ + QDockWidget* parent = dynamic_cast(this->parentWidget()); + if (parent) + { + bool ok; + QString text = QInputDialog::getText(this, tr("New title"), + tr("Widget title:"), QLineEdit::Normal, + parent->windowTitle(), &ok); + if (ok && !text.isEmpty()) + parent->setWindowTitle(text); + this->setWindowTitle(text); + } +} + void HDDisplay::renderOverlay() { #if (QGC_EVENTLOOP_DEBUG) @@ -188,6 +348,13 @@ void HDDisplay::renderOverlay() // Update scaling factor // adjust scaling to fit both horizontally and vertically scalingFactor = this->width()/vwidth; + + // Adjust vheight dynamically according to the number of rows + float vColWidth = vwidth / columns; + int vRows = ceil(acceptList->length()/(float)columns); + // Assuming square instruments, vheight is column width*row count + vheight = vColWidth * vRows; + double scalingFactorH = this->height()/vheight; if (scalingFactorH < scalingFactor) scalingFactor = scalingFactorH; @@ -195,7 +362,6 @@ void HDDisplay::renderOverlay() painter.setRenderHint(QPainter::Antialiasing, true); painter.setRenderHint(QPainter::HighQualityAntialiasing, true); //painter.fillRect(QRect(0, 0, width(), height()), backgroundColor); - const int columns = 3; const float spacing = 0.4f; // 40% of width const float gaugeWidth = vwidth / (((float)columns) + (((float)columns+1) * spacing + spacing * 0.5f)); const QColor gaugeColor = QColor(200, 200, 200); @@ -216,7 +382,7 @@ void HDDisplay::renderOverlay() drawGauge(xCoord, yCoord, gaugeWidth/2.0f, minValues.value(value, -1.0f), maxValues.value(value, 1.0f), value, values.value(value, minValues.value(value, 0.0f)), gaugeColor, &painter, goodRanges.value(value, qMakePair(0.0f, 0.5f)), critRanges.value(value, qMakePair(0.7f, 1.0f)), true); xCoord += gaugeWidth + leftSpacing; // Move one row down if necessary - if (xCoord + gaugeWidth > vwidth) + if (xCoord + gaugeWidth*0.9f > vwidth) { yCoord += topSpacing + gaugeWidth; xCoord = leftSpacing + gaugeWidth/2.0f; @@ -234,14 +400,14 @@ void HDDisplay::setActiveUAS(UASInterface* uas) if (this->uas != NULL) { // Disconnect any previously connected active MAV - disconnect(this->uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); + disconnect(this->uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(updateValue(int,QString,double,quint64))); } // Now connect the new UAS //qDebug() << "UAS SET!" << "ID:" << uas->getUASID(); // Setup communication - connect(uas, SIGNAL(valueChanged(UASInterface*,QString,double,quint64)), this, SLOT(updateValue(UASInterface*,QString,double,quint64))); + connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(updateValue(int,QString,double,quint64))); this->uas = uas; } @@ -344,9 +510,14 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float // per max. change const float rangeScale = ((2.0f * M_PI) / (max - min)) * 0.72f; - const float nameHeight = radius / 2.5f; + const float scaledValue = (value-min)*rangeScale; + + float nameHeight = radius / 2.6f; paintText(name.toUpper(), color, nameHeight*0.7f, xRef-radius, yRef-radius, painter); + // Ensure some space + nameHeight *= 1.2f; + if (!solid) { circlePen.setStyle(Qt::DotLine); @@ -359,18 +530,17 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float //drawCircle(xRef, yRef+nameHeight, radius, 0.0f, 170.0f, 1.0f, color, painter); QString label; - label.sprintf("%05.1f", value); + label.sprintf("% 06.1f", value); // Text // height - const float textHeight = radius/2.0f; - const float textX = xRef-radius/6.0f; + const float textHeight = radius/2.1f; + const float textX = xRef-radius/3.0f; const float textY = yRef+radius/2.0f; // Draw background rectangle - // FIXME add background color for use in instrument panel - QBrush brush(QColor(0, 0, 0), Qt::SolidPattern); + QBrush brush(QGC::colorBackground, Qt::SolidPattern); painter->setBrush(brush); painter->setPen(Qt::NoPen); painter->drawRect(refToScreenX(xRef-radius/2.5f), refToScreenY(yRef+nameHeight+radius/4.0f), refToScreenX(radius+radius/2.0f), refToScreenY((radius - radius/4.0f)*1.2f)); @@ -396,7 +566,7 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float // Draw the value //painter->setPen(textColor); - paintText(label, color, textHeight, textX, textY+nameHeight, painter); + paintText(label, QGC::colorCyan, textHeight, textX, textY+nameHeight, painter); //paintText(label, color, ((radius - radius/3.0f)*1.1f), xRef-radius/2.5f, yRef+radius/3.0f, painter); // Draw the needle @@ -414,7 +584,7 @@ void HDDisplay::drawGauge(float xRef, float yRef, float radius, float min, float p.replace(5, QPointF(xRef-maxWidth/2.0f, yRef+nameHeight+radius * 0.05f)); - rotatePolygonClockWiseRad(p, value*rangeScale+zeroRotation, QPointF(xRef, yRef+nameHeight)); + rotatePolygonClockWiseRad(p, scaledValue+zeroRotation, QPointF(xRef, yRef+nameHeight)); QBrush indexBrush; indexBrush.setColor(color); @@ -580,20 +750,17 @@ float HDDisplay::refLineWidthToPen(float line) return line * 2.50f; } -void HDDisplay::updateValue(UASInterface* uas, QString name, double value, quint64 msec) +void HDDisplay::updateValue(int uasId, QString name, double value, quint64 msec) { - Q_UNUSED(uas); - //if (this->uas == uas) - //{ - // Update mean - const float oldMean = valuesMean.value(name, 0.0f); - const int meanCount = valuesCount.value(name, 0); - valuesMean.insert(name, (oldMean * meanCount + value) / (meanCount + 1)); - valuesCount.insert(name, meanCount + 1); - valuesDot.insert(name, (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f)); - values.insert(name, value); - lastUpdate.insert(name, msec); - //} + Q_UNUSED(uasId); + // Update mean + const float oldMean = valuesMean.value(name, 0.0f); + const int meanCount = valuesCount.value(name, 0); + valuesMean.insert(name, (oldMean * meanCount + value) / (meanCount + 1)); + valuesCount.insert(name, meanCount + 1); + valuesDot.insert(name, (value - values.value(name, 0.0f)) / ((msec - lastUpdate.value(name, 0))/1000.0f)); + values.insert(name, value); + lastUpdate.insert(name, msec); } /** @@ -664,20 +831,17 @@ void HDDisplay::showEvent(QShowEvent* event) { // React only to internal (pre-display) // events - Q_UNUSED(event) - { - refreshTimer->start(updateInterval); - } + Q_UNUSED(event); + refreshTimer->start(updateInterval); } void HDDisplay::hideEvent(QHideEvent* event) { // React only to internal (pre-display) // events - Q_UNUSED(event) - { - refreshTimer->stop(); - } + Q_UNUSED(event); + refreshTimer->stop(); + saveState(); } diff --git a/src/ui/HDDisplay.h b/src/ui/HDDisplay.h index 35121b560365c233ad47d6693e04f7cc6ffd05ff..0ddb8a354779982b788ca99ddc79857875f6f451 100644 --- a/src/ui/HDDisplay.h +++ b/src/ui/HDDisplay.h @@ -37,6 +37,7 @@ This file is part of the QGROUNDCONTROL project #include #include #include +#include #include #include @@ -58,14 +59,31 @@ class HDDisplay : public QGraphicsView { Q_OBJECT public: - HDDisplay(QStringList* plotList, QWidget *parent = 0); + HDDisplay(QStringList* plotList, QString title="", QWidget *parent = 0); ~HDDisplay(); public slots: /** @brief Update a HDD value */ - void updateValue(UASInterface* uas, QString name, double value, quint64 msec); + void updateValue(int uasId, QString name, double value, quint64 msec); void setActiveUAS(UASInterface* uas); + /** @brief Removes a plot item by the action data */ + void removeItemByAction(); + /** @brief Bring up the menu to add a gauge */ + void addGauge(); + /** @brief Add a gauge using this spec string */ + void addGauge(const QString& gauge); + /** @brief Set the title of this widget and any existing parent dock widget */ + void setTitle(); + /** @brief Set the number of colums via popup */ + void setColumns(); + /** @brief Set the number of colums */ + void setColumns(int cols); + /** @brief Save the current layout and state to disk */ + void saveState(); + /** @brief Restore the last layout and state from disk */ + void restoreState(); + protected slots: void enableGLRendering(bool enable); //void render(QPainter* painter, const QRectF& target = QRectF(), const QRect& source = QRect(), Qt::AspectRatioMode aspectRatioMode = Qt::KeepAspectRatio); @@ -73,10 +91,13 @@ protected slots: void triggerUpdate(); protected: - void changeEvent(QEvent *e); - void paintEvent(QPaintEvent * event); + void changeEvent(QEvent* e); + void paintEvent(QPaintEvent* event); void showEvent(QShowEvent* event); void hideEvent(QHideEvent* event); + void contextMenuEvent(QContextMenuEvent* event); + QList getItemRemoveActions(); + void createActions(); float refLineWidthToPen(float line); float refToScreenX(float x); float refToScreenY(float y); @@ -155,6 +176,11 @@ protected: QStringList* acceptList; ///< Variable names to plot quint64 lastPaintTime; ///< Last time this widget was refreshed + int columns; ///< Number of instrument columns + + QAction* addGaugeAction; ///< Action adding a gauge + QAction* setTitleAction; ///< Action setting the title + QAction* setColumnsAction; ///< Action setting the number of columns private: Ui::HDDisplay *m_ui; diff --git a/src/ui/HSIDisplay.cc b/src/ui/HSIDisplay.cc index 6b547aced32ae751a8193d127903251051848479..f8c73ce12d7cb08df7af9d411722465d94dce290 100644 --- a/src/ui/HSIDisplay.cc +++ b/src/ui/HSIDisplay.cc @@ -46,7 +46,7 @@ This file is part of the QGROUNDCONTROL project #include HSIDisplay::HSIDisplay(QWidget *parent) : - HDDisplay(NULL, parent), + HDDisplay(NULL, "", parent), gpsSatellites(), satellitesUsed(0), attXSet(0), diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index 8109230d67afc200cb3ff4db737945a6ac5f37ba..336842ff4a23c135342fe6a1138a9b167a03ac31 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -207,17 +207,19 @@ void MainWindow::buildPxWidgets() { //FIXME: memory of acceptList will never be freed again QStringList* acceptList = new QStringList(); - acceptList->append("roll IMU"); - acceptList->append("pitch IMU"); - acceptList->append("yaw IMU"); - acceptList->append("rollspeed IMU"); - acceptList->append("pitchspeed IMU"); - acceptList->append("yawspeed IMU"); + acceptList->append("-180,roll (deg),+180"); + acceptList->append("-180,pitch (deg),+180"); + acceptList->append("-180,yaw (deg),+180"); + + acceptList->append("-500,roll V (deg/s),+500"); + acceptList->append("-500,pitch V (deg/s),+500"); + acceptList->append("-500,yaw V (deg/s),+500"); //FIXME: memory of acceptList2 will never be freed again QStringList* acceptList2 = new QStringList(); - acceptList2->append("Battery"); - acceptList2->append("Pressure"); + acceptList2->append("0,Abs pressure,65500"); + acceptList2->append("-2000,Accel. X, 2000"); + acceptList2->append("-2000,Accel. Y, 2000"); if (!linechartWidget) { @@ -296,15 +298,15 @@ void MainWindow::buildPxWidgets() if (!headDown1DockWidget) { - headDown1DockWidget = new QDockWidget(tr("System Stats"), this); - headDown1DockWidget->setWidget( new HDDisplay(acceptList, this) ); + headDown1DockWidget = new QDockWidget(tr("Flight Display"), this); + headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) ); addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget()), MENU_HDD_1, Qt::RightDockWidgetArea); } if (!headDown2DockWidget) { headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); - headDown2DockWidget->setWidget( new HDDisplay(acceptList2, this) ); + headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) ); addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget()), MENU_HDD_2, Qt::RightDockWidgetArea); } diff --git a/src/ui/SerialConfigurationWindow.cc b/src/ui/SerialConfigurationWindow.cc index d85802706f1dc578cfeaf33546a37098a55d7490..11bd3dd915c661b4d4b2095f1eda438290193ace 100644 --- a/src/ui/SerialConfigurationWindow.cc +++ b/src/ui/SerialConfigurationWindow.cc @@ -234,6 +234,7 @@ userConfigured(false) if(serialLink != 0) { + serialLink->loadSettings(); this->link = serialLink; // Setup the user interface according to link type diff --git a/src/ui/SerialSettings.ui b/src/ui/SerialSettings.ui index 402d649c549beb840dd5ac436cc528c6da7dad66..b116400b26115baed98620bef520ee674324b5ce 100644 --- a/src/ui/SerialSettings.ui +++ b/src/ui/SerialSettings.ui @@ -159,11 +159,26 @@ 128000 + + + 230400 + + 256000 + + + 460800 + + + + + 921600 + + diff --git a/src/ui/linechart/LinechartWidget.h b/src/ui/linechart/LinechartWidget.h index d14269c6dd472779d1dc287904f6a566fb756f76..dbffd698ffc4c460fb85464170d3a223e93a7519 100644 --- a/src/ui/linechart/LinechartWidget.h +++ b/src/ui/linechart/LinechartWidget.h @@ -72,7 +72,7 @@ public: public slots: void addCurve(QString curve); void removeCurve(QString curve); - void appendData(int sysId, QString curve, double data, quint64 usec); + void appendData(int uasId, QString curve, double data, quint64 usec); void takeButtonClick(bool checked); void setPlotWindowPosition(int scrollBarValue); void setPlotWindowPosition(quint64 position); diff --git a/src/ui/map3D/QGCGoogleEarthView.cc b/src/ui/map3D/QGCGoogleEarthView.cc index 7354c9d1d209b8120563cc4182bf582d89c1050e..1ec7fe25453215a5db76d2f4ffce618fbe737734 100644 --- a/src/ui/map3D/QGCGoogleEarthView.cc +++ b/src/ui/map3D/QGCGoogleEarthView.cc @@ -231,7 +231,7 @@ void QGCGoogleEarthView::showEvent(QShowEvent* event) // Reloading the webpage, this resets Google Earth gEarthInitialized = false; - QTimer::singleShot(3000, this, SLOT(initializeGoogleEarth())); + QTimer::singleShot(10000, this, SLOT(initializeGoogleEarth())); } else { @@ -300,7 +300,7 @@ void QGCGoogleEarthView::initializeGoogleEarth() qDebug() << "COULD NOT GET DOCUMENT OBJECT! Aborting"; } #endif - QTimer::singleShot(3500, this, SLOT(initializeGoogleEarth())); + QTimer::singleShot(2500, this, SLOT(initializeGoogleEarth())); return; }