diff --git a/src/lib/qextserialport/win_qextserialport.cpp b/src/lib/qextserialport/win_qextserialport.cpp index 39768baf45a54702020ba8f15df4ab5dd0cdb4bb..7fca3621ba5f1e48c8890c975afd676a5cfdfeb9 100644 --- a/src/lib/qextserialport/win_qextserialport.cpp +++ b/src/lib/qextserialport/win_qextserialport.cpp @@ -1,1070 +1,1072 @@ -//#include -//#include -//#include -//#include -#include -#include "win_qextserialport.h" - - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort() -Default constructor. Note that the name of the device used by a Win_QextSerialPort constructed -with this constructor will be determined by #defined constants, or lack thereof - the default -behavior is the same as _TTY_LINUX_. Possible naming conventions and their associated constants -are: - -\verbatim - -Constant Used By Naming Convention ----------- ------------- ------------------------ -_TTY_WIN_ Windows COM1, COM2 -_TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 -_TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 -_TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb -_TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 -_TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 -_TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 - Linux /dev/ttyS0, /dev/ttyS1 -\endverbatim - -This constructor associates the object with the first port on the system, e.g. COM1 for Windows -platforms. See the other constructor if you need a port other than the first. -*/ -Win_QextSerialPort::Win_QextSerialPort(QextSerialBase::QueryMode mode): - QextSerialBase() -{ - Win_Handle=INVALID_HANDLE_VALUE; - setQueryMode(mode); - init(); -} - -/*!Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort&) -Copy constructor. -*/ -Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort& s): - QextSerialBase(s.port) -{ - Win_Handle=INVALID_HANDLE_VALUE; - _queryMode = s._queryMode; - _bytesToWrite = s._bytesToWrite; - bytesToWriteLock = new QReadWriteLock; - overlapThread = new Win_QextSerialThread(this); - memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); - memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); - setOpenMode(s.openMode()); - lastErr=s.lastErr; - port = s.port; - Settings.FlowControl=s.Settings.FlowControl; - Settings.Parity=s.Settings.Parity; - Settings.DataBits=s.Settings.DataBits; - Settings.StopBits=s.Settings.StopBits; - Settings.BaudRate=s.Settings.BaudRate; - Win_Handle=s.Win_Handle; - memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); - memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); - if (s.overlapThread->isRunning()) - overlapThread->start(); -} - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name) -Constructs a serial port attached to the port specified by devName. -devName is the name of the device, which is windowsystem-specific, -e.g."COM2" or "/dev/ttyS0". -*/ -Win_QextSerialPort::Win_QextSerialPort(const QString & name, QextSerialBase::QueryMode mode): - QextSerialBase(name) -{ - Win_Handle=INVALID_HANDLE_VALUE; - setQueryMode(mode); - init(); -} - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings) -Constructs a port with default name and specified settings. -*/ -Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings, QextSerialBase::QueryMode mode): - QextSerialBase() -{ - Win_Handle=INVALID_HANDLE_VALUE; - setBaudRate(settings.BaudRate); - setDataBits(settings.DataBits); - setStopBits(settings.StopBits); - setParity(settings.Parity); - setFlowControl(settings.FlowControl); - setTimeout(settings.Timeout_Millisec); - setQueryMode(mode); - init(); -} - -/*! -\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings) -Constructs a port with specified name and settings. -*/ -Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings, QextSerialBase::QueryMode mode): - QextSerialBase(name) -{ - Win_Handle=INVALID_HANDLE_VALUE; - setPortName(name); - setBaudRate(settings.BaudRate); - setDataBits(settings.DataBits); - setStopBits(settings.StopBits); - setParity(settings.Parity); - setFlowControl(settings.FlowControl); - setTimeout(settings.Timeout_Millisec); - setQueryMode(mode); - 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; -} - -/*! -\fn Win_QextSerialPort::~Win_QextSerialPort() -Standard destructor. -*/ -Win_QextSerialPort::~Win_QextSerialPort() { - if (isOpen()) { - close(); - } - CloseHandle(overlap.hEvent); - delete overlapThread; - delete bytesToWriteLock; -} - -/*! -\fn Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) -overrides the = operator -*/ -Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) { - setOpenMode(s.openMode()); - _queryMode = s._queryMode; - _bytesToWrite = s._bytesToWrite; - bytesToWriteLock = new QReadWriteLock; - overlapThread = new Win_QextSerialThread(this); - memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); - memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); - lastErr=s.lastErr; - port = s.port; - Settings.FlowControl=s.Settings.FlowControl; - Settings.Parity=s.Settings.Parity; - Settings.DataBits=s.Settings.DataBits; - Settings.StopBits=s.Settings.StopBits; - Settings.BaudRate=s.Settings.BaudRate; - Win_Handle=s.Win_Handle; - memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); - memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); - if (s.overlapThread->isRunning()) - overlapThread->start(); - return *this; -} - - -/*! -\fn bool Win_QextSerialPort::open(OpenMode mode) -Opens a serial port. Note that this function does not specify which device to open. If you need -to open a device by name, see Win_QextSerialPort::open(const char*). This function has no effect -if the port associated with the class is already open. The port is also configured to the current -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; - - LOCK_MUTEX(); - if (mode == QIODevice::NotOpen) - return isOpen(); - 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); - if (Win_Handle!=INVALID_HANDLE_VALUE) { - /*configure port settings*/ - GetCommConfig(Win_Handle, &Win_CommConfig, &confSize); - GetCommState(Win_Handle, &(Win_CommConfig.dcb)); - - /*set up parameters*/ - Win_CommConfig.dcb.fBinary=TRUE; - Win_CommConfig.dcb.fInX=FALSE; - Win_CommConfig.dcb.fOutX=FALSE; - Win_CommConfig.dcb.fAbortOnError=FALSE; - Win_CommConfig.dcb.fNull=FALSE; - setBaudRate(Settings.BaudRate); - setDataBits(Settings.DataBits); - setStopBits(Settings.StopBits); - setParity(Settings.Parity); - setFlowControl(Settings.FlowControl); - setTimeout(Settings.Timeout_Millisec); - 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 (!SetCommMask( Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { - qWarning("failed to set Comm Mask. Error code: %ld", GetLastError()); - UNLOCK_MUTEX(); - return false; - } - overlapThread->start(); - } - QIODevice::open(mode); - } - } else { - UNLOCK_MUTEX(); - return false; - } - UNLOCK_MUTEX(); - return isOpen(); -} - -/*! -\fn void Win_QextSerialPort::close() -Closes a serial port. This function has no effect if the serial port associated with the class -is not currently open. -*/ -void Win_QextSerialPort::close() -{ - LOCK_MUTEX(); - - if (isOpen()) { - 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(); - } - - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::flush() -Flushes all pending I/O to the serial port. This function has no effect if the serial port -associated with the class is not currently open. -*/ -void Win_QextSerialPort::flush() { - LOCK_MUTEX(); - if (isOpen()) { - FlushFileBuffers(Win_Handle); - } - UNLOCK_MUTEX(); -} - -/*! -\fn qint64 Win_QextSerialPort::size() const -This function will return the number of bytes waiting in the receive queue of the serial port. -It is included primarily to provide a complete QIODevice interface, and will not record errors -in the lastErr member (because it is const). This function is also not thread-safe - in -multithreading situations, use Win_QextSerialPort::bytesAvailable() instead. -*/ -qint64 Win_QextSerialPort::size() const { - int availBytes; - COMSTAT Win_ComStat; - DWORD Win_ErrorMask=0; - ClearCommError(Win_Handle, &Win_ErrorMask, &Win_ComStat); - availBytes = Win_ComStat.cbInQue; - return (qint64)availBytes; -} - -/*! -\fn qint64 Win_QextSerialPort::bytesAvailable() -Returns the number of bytes waiting in the port's receive queue. This function will return 0 if -the port is not currently open, or -1 on error. -*/ -qint64 Win_QextSerialPort::bytesAvailable() const { - LOCK_MUTEX(); - if (isOpen()) { - DWORD Errors; - COMSTAT Status; - if (ClearCommError(Win_Handle, &Errors, &Status)) { - UNLOCK_MUTEX(); - return Status.cbInQue + QIODevice::bytesAvailable(); - } - UNLOCK_MUTEX(); - return (qint64)-1; - } - UNLOCK_MUTEX(); - return 0; -} - -/*! -\fn void Win_QextSerialPort::translateError(ulong error) -Translates a system-specific error code to a QextSerialPort error code. Used internally. -*/ -void Win_QextSerialPort::translateError(ulong error) { - if (error&CE_BREAK) { - lastErr=E_BREAK_CONDITION; - } - else if (error&CE_FRAME) { - lastErr=E_FRAMING_ERROR; - } - else if (error&CE_IOE) { - lastErr=E_IO_ERROR; - } - else if (error&CE_MODE) { - lastErr=E_INVALID_FD; - } - else if (error&CE_OVERRUN) { - lastErr=E_BUFFER_OVERRUN; - } - else if (error&CE_RXPARITY) { - lastErr=E_RECEIVE_PARITY_ERROR; - } - else if (error&CE_RXOVER) { - lastErr=E_RECEIVE_OVERFLOW; - } - else if (error&CE_TXFULL) { - lastErr=E_TRANSMIT_OVERFLOW; - } -} - -/*! -\fn qint64 Win_QextSerialPort::readData(char *data, qint64 maxSize) -Reads a block of data from the serial port. This function will read at most maxlen bytes from -the serial port and place them in the buffer pointed to by data. Return value is the number of -bytes actually read, or -1 on error. - -\warning before calling this function ensure that serial port associated with this class -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)) { - lastErr = E_READ_FAILED; - retVal = (DWORD)-1; - } - - UNLOCK_MUTEX(); - - return (qint64)retVal; -} - -/*! -\fn qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) -Writes a block of data to the serial port. This function will write len bytes -from the buffer pointed to by data to the serial port. Return value is the number -of bytes actually written, or -1 on error. - -\warning before calling this function ensure that serial port associated with this class -is currently open (use isOpen() function to check if port is open). -*/ -qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) -{ - DWORD retVal; - - LOCK_MUTEX(); - - retVal = 0; - if (queryMode() == QextSerialBase::EventDriven) { - 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; - } else if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { - lastErr = E_WRITE_FAILED; - retVal = (DWORD)-1; - } - - UNLOCK_MUTEX(); - - return (qint64)retVal; -} - -/*! -\fn void Win_QextSerialPort::ungetChar(char c) -This function is included to implement the full QIODevice interface, and currently has no -purpose within this class. This function is meaningless on an unbuffered device and currently -only prints a warning message to that effect. -*/ -void Win_QextSerialPort::ungetChar(char c) { - - /*meaningless on unbuffered sequential device - return error and print a warning*/ - TTY_WARNING("Win_QextSerialPort: ungetChar() called on an unbuffered sequential device - operation is meaningless"); -} - -/*! -\fn void Win_QextSerialPort::setFlowControl(FlowType flow) -Sets the flow control used by the port. Possible values of flow are: -\verbatim - FLOW_OFF No flow control - FLOW_HARDWARE Hardware (RTS/CTS) flow control - FLOW_XONXOFF Software (XON/XOFF) flow control -\endverbatim -*/ -void Win_QextSerialPort::setFlowControl(FlowType flow) { - LOCK_MUTEX(); - if (Settings.FlowControl!=flow) { - Settings.FlowControl=flow; - } - if (isOpen()) { - 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; - - /*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_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(); -} - -/*! -\fn void Win_QextSerialPort::setParity(ParityType parity) -Sets the parity associated with the serial port. The possible values of parity are: -\verbatim - PAR_SPACE Space Parity - PAR_MARK Mark Parity - PAR_NONE No Parity - PAR_EVEN Even Parity - PAR_ODD Odd Parity -\endverbatim -*/ -void Win_QextSerialPort::setParity(ParityType parity) { - LOCK_MUTEX(); - if (Settings.Parity!=parity) { - Settings.Parity=parity; - } - if (isOpen()) { - Win_CommConfig.dcb.Parity=(unsigned char)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; - - /*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; - - /*no parity*/ - case PAR_NONE: - Win_CommConfig.dcb.fParity=FALSE; - break; - - /*even parity*/ - case PAR_EVEN: - Win_CommConfig.dcb.fParity=TRUE; - break; - - /*odd parity*/ - case PAR_ODD: - Win_CommConfig.dcb.fParity=TRUE; - break; - } - SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setDataBits(DataBitsType dataBits) -Sets the number of data bits used by the serial port. Possible values of dataBits are: -\verbatim - DATA_5 5 data bits - DATA_6 6 data bits - DATA_7 7 data bits - DATA_8 8 data bits -\endverbatim - -\note -This function is subject to the following restrictions: -\par - 5 data bits cannot be used with 2 stop bits. -\par - 1.5 stop bits can only be used with 5 data bits. -\par - 8 data bits cannot be used with space parity on POSIX systems. - -*/ -void Win_QextSerialPort::setDataBits(DataBitsType dataBits) { - LOCK_MUTEX(); - if (Settings.DataBits!=dataBits) { - if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || - (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5)) { - } - else { - Settings.DataBits=dataBits; - } - } - if (isOpen()) { - 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; - - /*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; - - /*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; - - /*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; - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setStopBits(StopBitsType stopBits) -Sets the number of stop bits used by the serial port. Possible values of stopBits are: -\verbatim - STOP_1 1 stop bit - STOP_1_5 1.5 stop bits - STOP_2 2 stop bits -\endverbatim - -\note -This function is subject to the following restrictions: -\par - 2 stop bits cannot be used with 5 data bits. -\par - 1.5 stop bits cannot be used with 6 or more data bits. -\par - POSIX does not support 1.5 stop bits. -*/ -void Win_QextSerialPort::setStopBits(StopBitsType stopBits) { - LOCK_MUTEX(); - if (Settings.StopBits!=stopBits) { - if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || - (stopBits==STOP_1_5 && Settings.DataBits!=DATA_5)) { - } - else { - Settings.StopBits=stopBits; - } - } - if (isOpen()) { - switch (stopBits) { - - /*one stop bit*/ - 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; - - /*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; - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setBaudRate(BaudRateType baudRate) -Sets the baud rate of the serial port. Note that not all rates are applicable on -all platforms. The following table shows translations of the various baud rate -constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * -are speeds that are usable on both Windows and POSIX. -\verbatim - - RATE Windows Speed POSIX Speed - ----------- ------------- ----------- - BAUD50 110 50 - BAUD75 110 75 - *BAUD110 110 110 - BAUD134 110 134.5 - BAUD150 110 150 - BAUD200 110 200 - *BAUD300 300 300 - *BAUD600 600 600 - *BAUD1200 1200 1200 - BAUD1800 1200 1800 - *BAUD2400 2400 2400 - *BAUD4800 4800 4800 - *BAUD9600 9600 9600 - BAUD14400 14400 9600 - *BAUD19200 19200 19200 - *BAUD38400 38400 38400 - BAUD56000 56000 38400 - *BAUD57600 57600 57600 - BAUD76800 57600 76800 - *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; - } - } - 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; - - /*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; - - /*110 baud*/ - 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; - - /*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; - - /*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; - - /*300 baud*/ - case BAUD300: - Win_CommConfig.dcb.BaudRate=CBR_300; - break; - - /*600 baud*/ - case BAUD600: - Win_CommConfig.dcb.BaudRate=CBR_600; - break; - - /*1200 baud*/ - 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; - - /*2400 baud*/ - case BAUD2400: - Win_CommConfig.dcb.BaudRate=CBR_2400; - break; - - /*4800 baud*/ - case BAUD4800: - Win_CommConfig.dcb.BaudRate=CBR_4800; - break; - - /*9600 baud*/ - 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; - - /*19200 baud*/ - case BAUD19200: - Win_CommConfig.dcb.BaudRate=CBR_19200; - break; - - /*38400 baud*/ - 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; - - /*57600 baud*/ - 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; - - /*115200 baud*/ - 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; - - /*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; - /*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)); - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setDtr(bool set) -Sets DTR line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void Win_QextSerialPort::setDtr(bool set) { - LOCK_MUTEX(); - if (isOpen()) { - if (set) { - EscapeCommFunction(Win_Handle, SETDTR); - } - else { - EscapeCommFunction(Win_Handle, CLRDTR); - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn void Win_QextSerialPort::setRts(bool set) -Sets RTS line to the requested state (high by default). This function will have no effect if -the port associated with the class is not currently open. -*/ -void Win_QextSerialPort::setRts(bool set) { - LOCK_MUTEX(); - if (isOpen()) { - if (set) { - EscapeCommFunction(Win_Handle, SETRTS); - } - else { - EscapeCommFunction(Win_Handle, CLRRTS); - } - } - UNLOCK_MUTEX(); -} - -/*! -\fn ulong Win_QextSerialPort::lineStatus(void) -returns the line status as stored by the port function. This function will retrieve the states -of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines -can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned -long with specific bits indicating which lines are high. The following constants should be used -to examine the states of individual lines: - -\verbatim -Mask Line ------- ---- -LS_CTS CTS -LS_DSR DSR -LS_DCD DCD -LS_RI RI -\endverbatim - -This function will return 0 if the port associated with the class is not currently open. -*/ -ulong Win_QextSerialPort::lineStatus(void) { - unsigned long Status=0, Temp=0; - LOCK_MUTEX(); - if (isOpen()) { - GetCommModemStatus(Win_Handle, &Temp); - if (Temp&MS_CTS_ON) { - Status|=LS_CTS; - } - if (Temp&MS_DSR_ON) { - Status|=LS_DSR; - } - if (Temp&MS_RING_ON) { - Status|=LS_RI; - } - if (Temp&MS_RLSD_ON) { - Status|=LS_DCD; - } - } - UNLOCK_MUTEX(); - return Status; -} - -bool Win_QextSerialPort::waitForReadyRead(int msecs) -{ - //@todo implement - return false; -} - -qint64 Win_QextSerialPort::bytesToWrite() const -{ - 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); - } -} - -void Win_QextSerialPort::terminateCommWait() -{ - SetCommMask(Win_Handle, 0); -} - - -/*! -\fn void Win_QextSerialPort::setTimeout(ulong millisec); -Sets the read and write timeouts for the port to millisec milliseconds. -Setting 0 indicates that timeouts are not used for read nor write operations; -however read() and write() functions will still block. Set -1 to provide -non-blocking behaviour (read() and write() will return immediately). - -\note this function does nothing in event driven mode. -*/ -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) - SetCommTimeouts(Win_Handle, &Win_CommTimeouts); - - UNLOCK_MUTEX(); -} - - -Win_QextSerialThread::Win_QextSerialThread(Win_QextSerialPort * qesp): - QThread() -{ - this->qesp = qesp; - terminate = false; -} - -void Win_QextSerialThread::stop() -{ - terminate = true; - qesp->terminateCommWait(); -} - -void Win_QextSerialThread::run() -{ - while (!terminate) - qesp->monitorCommEvent(); - terminate = false; -} +//#include +//#include +//#include +//#include +#include +#include "win_qextserialport.h" + + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort() +Default constructor. Note that the name of the device used by a Win_QextSerialPort constructed +with this constructor will be determined by #defined constants, or lack thereof - the default +behavior is the same as _TTY_LINUX_. Possible naming conventions and their associated constants +are: + +\verbatim + +Constant Used By Naming Convention +---------- ------------- ------------------------ +_TTY_WIN_ Windows COM1, COM2 +_TTY_IRIX_ SGI/IRIX /dev/ttyf1, /dev/ttyf2 +_TTY_HPUX_ HP-UX /dev/tty1p0, /dev/tty2p0 +_TTY_SUN_ SunOS/Solaris /dev/ttya, /dev/ttyb +_TTY_DIGITAL_ Digital UNIX /dev/tty01, /dev/tty02 +_TTY_FREEBSD_ FreeBSD /dev/ttyd0, /dev/ttyd1 +_TTY_LINUX_ Linux /dev/ttyS0, /dev/ttyS1 + Linux /dev/ttyS0, /dev/ttyS1 +\endverbatim + +This constructor associates the object with the first port on the system, e.g. COM1 for Windows +platforms. See the other constructor if you need a port other than the first. +*/ +Win_QextSerialPort::Win_QextSerialPort(QextSerialBase::QueryMode mode): + QextSerialBase() +{ + Win_Handle=INVALID_HANDLE_VALUE; + setQueryMode(mode); + init(); +} + +/*!Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort&) +Copy constructor. +*/ +Win_QextSerialPort::Win_QextSerialPort(const Win_QextSerialPort& s): + QextSerialBase(s.port) +{ + Win_Handle=INVALID_HANDLE_VALUE; + _queryMode = s._queryMode; + _bytesToWrite = s._bytesToWrite; + bytesToWriteLock = new QReadWriteLock; + overlapThread = new Win_QextSerialThread(this); + memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); + memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); + setOpenMode(s.openMode()); + lastErr=s.lastErr; + port = s.port; + Settings.FlowControl=s.Settings.FlowControl; + Settings.Parity=s.Settings.Parity; + Settings.DataBits=s.Settings.DataBits; + Settings.StopBits=s.Settings.StopBits; + Settings.BaudRate=s.Settings.BaudRate; + Win_Handle=s.Win_Handle; + memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); + memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); + if (s.overlapThread->isRunning()) + overlapThread->start(); +} + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name) +Constructs a serial port attached to the port specified by devName. +devName is the name of the device, which is windowsystem-specific, +e.g."COM2" or "/dev/ttyS0". +*/ +Win_QextSerialPort::Win_QextSerialPort(const QString & name, QextSerialBase::QueryMode mode): + QextSerialBase(name) +{ + Win_Handle=INVALID_HANDLE_VALUE; + setQueryMode(mode); + init(); +} + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings) +Constructs a port with default name and specified settings. +*/ +Win_QextSerialPort::Win_QextSerialPort(const PortSettings& settings, QextSerialBase::QueryMode mode): + QextSerialBase() +{ + Win_Handle=INVALID_HANDLE_VALUE; + setBaudRate(settings.BaudRate); + setDataBits(settings.DataBits); + setStopBits(settings.StopBits); + setParity(settings.Parity); + setFlowControl(settings.FlowControl); + setTimeout(settings.Timeout_Millisec); + setQueryMode(mode); + init(); +} + +/*! +\fn Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings) +Constructs a port with specified name and settings. +*/ +Win_QextSerialPort::Win_QextSerialPort(const QString & name, const PortSettings& settings, QextSerialBase::QueryMode mode): + QextSerialBase(name) +{ + Win_Handle=INVALID_HANDLE_VALUE; + setPortName(name); + setBaudRate(settings.BaudRate); + setDataBits(settings.DataBits); + setStopBits(settings.StopBits); + setParity(settings.Parity); + setFlowControl(settings.FlowControl); + setTimeout(settings.Timeout_Millisec); + setQueryMode(mode); + 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; +} + +/*! +\fn Win_QextSerialPort::~Win_QextSerialPort() +Standard destructor. +*/ +Win_QextSerialPort::~Win_QextSerialPort() { + if (isOpen()) { + close(); + } + CloseHandle(overlap.hEvent); + delete overlapThread; + delete bytesToWriteLock; +} + +/*! +\fn Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) +overrides the = operator +*/ +Win_QextSerialPort& Win_QextSerialPort::operator=(const Win_QextSerialPort& s) { + setOpenMode(s.openMode()); + _queryMode = s._queryMode; + _bytesToWrite = s._bytesToWrite; + bytesToWriteLock = new QReadWriteLock; + overlapThread = new Win_QextSerialThread(this); + memcpy(& overlap, & s.overlap, sizeof(OVERLAPPED)); + memcpy(& overlapWrite, & s.overlapWrite, sizeof(OVERLAPPED)); + lastErr=s.lastErr; + port = s.port; + Settings.FlowControl=s.Settings.FlowControl; + Settings.Parity=s.Settings.Parity; + Settings.DataBits=s.Settings.DataBits; + Settings.StopBits=s.Settings.StopBits; + Settings.BaudRate=s.Settings.BaudRate; + Win_Handle=s.Win_Handle; + memcpy(&Win_CommConfig, &s.Win_CommConfig, sizeof(COMMCONFIG)); + memcpy(&Win_CommTimeouts, &s.Win_CommTimeouts, sizeof(COMMTIMEOUTS)); + if (s.overlapThread->isRunning()) + overlapThread->start(); + return *this; +} + + +/*! +\fn bool Win_QextSerialPort::open(OpenMode mode) +Opens a serial port. Note that this function does not specify which device to open. If you need +to open a device by name, see Win_QextSerialPort::open(const char*). This function has no effect +if the port associated with the class is already open. The port is also configured to the current +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; + + LOCK_MUTEX(); + if (mode == QIODevice::NotOpen) + return isOpen(); + 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); + if (Win_Handle!=INVALID_HANDLE_VALUE) { + /*configure port settings*/ + GetCommConfig(Win_Handle, &Win_CommConfig, &confSize); + GetCommState(Win_Handle, &(Win_CommConfig.dcb)); + + /*set up parameters*/ + Win_CommConfig.dcb.fBinary=TRUE; + Win_CommConfig.dcb.fInX=FALSE; + Win_CommConfig.dcb.fOutX=FALSE; + Win_CommConfig.dcb.fAbortOnError=FALSE; + Win_CommConfig.dcb.fNull=FALSE; + setBaudRate(Settings.BaudRate); + setDataBits(Settings.DataBits); + setStopBits(Settings.StopBits); + setParity(Settings.Parity); + setFlowControl(Settings.FlowControl); + setTimeout(Settings.Timeout_Millisec); + 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 (!SetCommMask( Win_Handle, EV_TXEMPTY | EV_RXCHAR | EV_DSR)) { + qWarning("failed to set Comm Mask. Error code: %ld", GetLastError()); + UNLOCK_MUTEX(); + return false; + } + overlapThread->start(); + } + QIODevice::open(mode); + } + } else { + UNLOCK_MUTEX(); + return false; + } + UNLOCK_MUTEX(); + return isOpen(); +} + +/*! +\fn void Win_QextSerialPort::close() +Closes a serial port. This function has no effect if the serial port associated with the class +is not currently open. +*/ +void Win_QextSerialPort::close() +{ + LOCK_MUTEX(); + + if (isOpen()) { + 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(); + } + + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::flush() +Flushes all pending I/O to the serial port. This function has no effect if the serial port +associated with the class is not currently open. +*/ +void Win_QextSerialPort::flush() { + LOCK_MUTEX(); + if (isOpen()) { + FlushFileBuffers(Win_Handle); + } + UNLOCK_MUTEX(); +} + +/*! +\fn qint64 Win_QextSerialPort::size() const +This function will return the number of bytes waiting in the receive queue of the serial port. +It is included primarily to provide a complete QIODevice interface, and will not record errors +in the lastErr member (because it is const). This function is also not thread-safe - in +multithreading situations, use Win_QextSerialPort::bytesAvailable() instead. +*/ +qint64 Win_QextSerialPort::size() const { + int availBytes; + COMSTAT Win_ComStat; + DWORD Win_ErrorMask=0; + ClearCommError(Win_Handle, &Win_ErrorMask, &Win_ComStat); + availBytes = Win_ComStat.cbInQue; + return (qint64)availBytes; +} + +/*! +\fn qint64 Win_QextSerialPort::bytesAvailable() +Returns the number of bytes waiting in the port's receive queue. This function will return 0 if +the port is not currently open, or -1 on error. +*/ +qint64 Win_QextSerialPort::bytesAvailable() const { + LOCK_MUTEX(); + if (isOpen()) { + DWORD Errors; + COMSTAT Status; + if (ClearCommError(Win_Handle, &Errors, &Status)) { + UNLOCK_MUTEX(); + return Status.cbInQue + QIODevice::bytesAvailable(); + } + UNLOCK_MUTEX(); + return (qint64)-1; + } + UNLOCK_MUTEX(); + return 0; +} + +/*! +\fn void Win_QextSerialPort::translateError(ulong error) +Translates a system-specific error code to a QextSerialPort error code. Used internally. +*/ +void Win_QextSerialPort::translateError(ulong error) { + if (error&CE_BREAK) { + lastErr=E_BREAK_CONDITION; + } + else if (error&CE_FRAME) { + lastErr=E_FRAMING_ERROR; + } + else if (error&CE_IOE) { + lastErr=E_IO_ERROR; + } + else if (error&CE_MODE) { + lastErr=E_INVALID_FD; + } + else if (error&CE_OVERRUN) { + lastErr=E_BUFFER_OVERRUN; + } + else if (error&CE_RXPARITY) { + lastErr=E_RECEIVE_PARITY_ERROR; + } + else if (error&CE_RXOVER) { + lastErr=E_RECEIVE_OVERFLOW; + } + else if (error&CE_TXFULL) { + lastErr=E_TRANSMIT_OVERFLOW; + } +} + +/*! +\fn qint64 Win_QextSerialPort::readData(char *data, qint64 maxSize) +Reads a block of data from the serial port. This function will read at most maxlen bytes from +the serial port and place them in the buffer pointed to by data. Return value is the number of +bytes actually read, or -1 on error. + +\warning before calling this function ensure that serial port associated with this class +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)) { + lastErr = E_READ_FAILED; + retVal = (DWORD)-1; + } + + UNLOCK_MUTEX(); + + return (qint64)retVal; +} + +/*! +\fn qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) +Writes a block of data to the serial port. This function will write len bytes +from the buffer pointed to by data to the serial port. Return value is the number +of bytes actually written, or -1 on error. + +\warning before calling this function ensure that serial port associated with this class +is currently open (use isOpen() function to check if port is open). +*/ +qint64 Win_QextSerialPort::writeData(const char *data, qint64 maxSize) +{ + DWORD retVal; + + LOCK_MUTEX(); + + retVal = 0; + if (queryMode() == QextSerialBase::EventDriven) { + 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; + } else if (!WriteFile(Win_Handle, (void*)data, (DWORD)maxSize, & retVal, NULL)) { + lastErr = E_WRITE_FAILED; + retVal = (DWORD)-1; + } + + UNLOCK_MUTEX(); + + return (qint64)retVal; +} + +/*! +\fn void Win_QextSerialPort::ungetChar(char c) +This function is included to implement the full QIODevice interface, and currently has no +purpose within this class. This function is meaningless on an unbuffered device and currently +only prints a warning message to that effect. +*/ +void Win_QextSerialPort::ungetChar(char c) { + + /*meaningless on unbuffered sequential device - return error and print a warning*/ + TTY_WARNING("Win_QextSerialPort: ungetChar() called on an unbuffered sequential device - operation is meaningless"); +} + +/*! +\fn void Win_QextSerialPort::setFlowControl(FlowType flow) +Sets the flow control used by the port. Possible values of flow are: +\verbatim + FLOW_OFF No flow control + FLOW_HARDWARE Hardware (RTS/CTS) flow control + FLOW_XONXOFF Software (XON/XOFF) flow control +\endverbatim +*/ +void Win_QextSerialPort::setFlowControl(FlowType flow) { + LOCK_MUTEX(); + if (Settings.FlowControl!=flow) { + Settings.FlowControl=flow; + } + if (isOpen()) { + 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; + + /*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_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(); +} + +/*! +\fn void Win_QextSerialPort::setParity(ParityType parity) +Sets the parity associated with the serial port. The possible values of parity are: +\verbatim + PAR_SPACE Space Parity + PAR_MARK Mark Parity + PAR_NONE No Parity + PAR_EVEN Even Parity + PAR_ODD Odd Parity +\endverbatim +*/ +void Win_QextSerialPort::setParity(ParityType parity) { + LOCK_MUTEX(); + if (Settings.Parity!=parity) { + Settings.Parity=parity; + } + if (isOpen()) { + Win_CommConfig.dcb.Parity=(unsigned char)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; + + /*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; + + /*no parity*/ + case PAR_NONE: + Win_CommConfig.dcb.fParity=FALSE; + break; + + /*even parity*/ + case PAR_EVEN: + Win_CommConfig.dcb.fParity=TRUE; + break; + + /*odd parity*/ + case PAR_ODD: + Win_CommConfig.dcb.fParity=TRUE; + break; + } + SetCommConfig(Win_Handle, &Win_CommConfig, sizeof(COMMCONFIG)); + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setDataBits(DataBitsType dataBits) +Sets the number of data bits used by the serial port. Possible values of dataBits are: +\verbatim + DATA_5 5 data bits + DATA_6 6 data bits + DATA_7 7 data bits + DATA_8 8 data bits +\endverbatim + +\note +This function is subject to the following restrictions: +\par + 5 data bits cannot be used with 2 stop bits. +\par + 1.5 stop bits can only be used with 5 data bits. +\par + 8 data bits cannot be used with space parity on POSIX systems. + +*/ +void Win_QextSerialPort::setDataBits(DataBitsType dataBits) { + LOCK_MUTEX(); + if (Settings.DataBits!=dataBits) { + if ((Settings.StopBits==STOP_2 && dataBits==DATA_5) || + (Settings.StopBits==STOP_1_5 && dataBits!=DATA_5)) { + } + else { + Settings.DataBits=dataBits; + } + } + if (isOpen()) { + 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; + + /*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; + + /*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; + + /*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; + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setStopBits(StopBitsType stopBits) +Sets the number of stop bits used by the serial port. Possible values of stopBits are: +\verbatim + STOP_1 1 stop bit + STOP_1_5 1.5 stop bits + STOP_2 2 stop bits +\endverbatim + +\note +This function is subject to the following restrictions: +\par + 2 stop bits cannot be used with 5 data bits. +\par + 1.5 stop bits cannot be used with 6 or more data bits. +\par + POSIX does not support 1.5 stop bits. +*/ +void Win_QextSerialPort::setStopBits(StopBitsType stopBits) { + LOCK_MUTEX(); + if (Settings.StopBits!=stopBits) { + if ((Settings.DataBits==DATA_5 && stopBits==STOP_2) || + (stopBits==STOP_1_5 && Settings.DataBits!=DATA_5)) { + } + else { + Settings.StopBits=stopBits; + } + } + if (isOpen()) { + switch (stopBits) { + + /*one stop bit*/ + 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; + + /*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; + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setBaudRate(BaudRateType baudRate) +Sets the baud rate of the serial port. Note that not all rates are applicable on +all platforms. The following table shows translations of the various baud rate +constants on Windows(including NT/2000) and POSIX platforms. Speeds marked with an * +are speeds that are usable on both Windows and POSIX. +\verbatim + + RATE Windows Speed POSIX Speed + ----------- ------------- ----------- + BAUD50 110 50 + BAUD75 110 75 + *BAUD110 110 110 + BAUD134 110 134.5 + BAUD150 110 150 + BAUD200 110 200 + *BAUD300 300 300 + *BAUD600 600 600 + *BAUD1200 1200 1200 + BAUD1800 1200 1800 + *BAUD2400 2400 2400 + *BAUD4800 4800 4800 + *BAUD9600 9600 9600 + BAUD14400 14400 9600 + *BAUD19200 19200 19200 + *BAUD38400 38400 38400 + BAUD56000 56000 38400 + *BAUD57600 57600 57600 + BAUD76800 57600 76800 + *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; + } + } + 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; + + /*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; + + /*110 baud*/ + 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; + + /*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; + + /*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; + + /*300 baud*/ + case BAUD300: + Win_CommConfig.dcb.BaudRate=CBR_300; + break; + + /*600 baud*/ + case BAUD600: + Win_CommConfig.dcb.BaudRate=CBR_600; + break; + + /*1200 baud*/ + 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; + + /*2400 baud*/ + case BAUD2400: + Win_CommConfig.dcb.BaudRate=CBR_2400; + break; + + /*4800 baud*/ + case BAUD4800: + Win_CommConfig.dcb.BaudRate=CBR_4800; + break; + + /*9600 baud*/ + 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; + + /*19200 baud*/ + case BAUD19200: + Win_CommConfig.dcb.BaudRate=CBR_19200; + break; + + /*38400 baud*/ + 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; + + /*57600 baud*/ + 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; + + /*115200 baud*/ + 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; + + /*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; + /*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)); + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setDtr(bool set) +Sets DTR line to the requested state (high by default). This function will have no effect if +the port associated with the class is not currently open. +*/ +void Win_QextSerialPort::setDtr(bool set) { + LOCK_MUTEX(); + if (isOpen()) { + if (set) { + EscapeCommFunction(Win_Handle, SETDTR); + } + else { + EscapeCommFunction(Win_Handle, CLRDTR); + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn void Win_QextSerialPort::setRts(bool set) +Sets RTS line to the requested state (high by default). This function will have no effect if +the port associated with the class is not currently open. +*/ +void Win_QextSerialPort::setRts(bool set) { + LOCK_MUTEX(); + if (isOpen()) { + if (set) { + EscapeCommFunction(Win_Handle, SETRTS); + } + else { + EscapeCommFunction(Win_Handle, CLRRTS); + } + } + UNLOCK_MUTEX(); +} + +/*! +\fn ulong Win_QextSerialPort::lineStatus(void) +returns the line status as stored by the port function. This function will retrieve the states +of the following lines: DCD, CTS, DSR, and RI. On POSIX systems, the following additional lines +can be monitored: DTR, RTS, Secondary TXD, and Secondary RXD. The value returned is an unsigned +long with specific bits indicating which lines are high. The following constants should be used +to examine the states of individual lines: + +\verbatim +Mask Line +------ ---- +LS_CTS CTS +LS_DSR DSR +LS_DCD DCD +LS_RI RI +\endverbatim + +This function will return 0 if the port associated with the class is not currently open. +*/ +ulong Win_QextSerialPort::lineStatus(void) { + unsigned long Status=0, Temp=0; + LOCK_MUTEX(); + if (isOpen()) { + GetCommModemStatus(Win_Handle, &Temp); + if (Temp&MS_CTS_ON) { + Status|=LS_CTS; + } + if (Temp&MS_DSR_ON) { + Status|=LS_DSR; + } + if (Temp&MS_RING_ON) { + Status|=LS_RI; + } + if (Temp&MS_RLSD_ON) { + Status|=LS_DCD; + } + } + UNLOCK_MUTEX(); + return Status; +} + +bool Win_QextSerialPort::waitForReadyRead(int msecs) +{ + //@todo implement + return false; +} + +qint64 Win_QextSerialPort::bytesToWrite() const +{ + 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); + } + } +} + +void Win_QextSerialPort::terminateCommWait() +{ + SetCommMask(Win_Handle, 0); +} + + +/*! +\fn void Win_QextSerialPort::setTimeout(ulong millisec); +Sets the read and write timeouts for the port to millisec milliseconds. +Setting 0 indicates that timeouts are not used for read nor write operations; +however read() and write() functions will still block. Set -1 to provide +non-blocking behaviour (read() and write() will return immediately). + +\note this function does nothing in event driven mode. +*/ +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) + SetCommTimeouts(Win_Handle, &Win_CommTimeouts); + + UNLOCK_MUTEX(); +} + + +Win_QextSerialThread::Win_QextSerialThread(Win_QextSerialPort * qesp): + QThread() +{ + this->qesp = qesp; + terminate = false; +} + +void Win_QextSerialThread::stop() +{ + terminate = true; + qesp->terminateCommWait(); +} + +void Win_QextSerialThread::run() +{ + while (!terminate) + qesp->monitorCommEvent(); + terminate = false; +} diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index dde06f5391bd68b447821e1a0971113839e9ff70..dc8cb9e047eca0828e258d4311c30d1009e043ac 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1,1823 +1,1829 @@ -/*=================================================================== -======================================================================*/ - -/** - * @file - * @brief Represents one unmanned aerial vehicle - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include "UAS.h" -#include "LinkInterface.h" -#include "UASManager.h" -//#include "MG.h" -#include "QGC.h" -#include "GAudioOutput.h" -#include "MAVLinkProtocol.h" -#include "QGCMAVLink.h" -#include "LinkManager.h" -#include "SerialLink.h" - -#ifndef M_PI -#define M_PI 3.14159265358979323846 /* pi */ -#endif - -#ifndef M_PI_2 -#define M_PI_2 1.57079632679489661923 /* pi/2 */ -#endif - -#ifndef M_PI_4 -#define M_PI_4 0.78539816339744830962 /* pi/4 */ -#endif - - - -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), -airframe(0) -{ - color = UASInterface::getNextColor(); - setBattery(LIPOLY, 3); - connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState())); - connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); - statusTimeout->start(500); - readSettings(); -} - -UAS::~UAS() -{ - writeSettings(); - delete links; - links=NULL; -} - -void UAS::writeSettings() -{ - QSettings settings; - settings.beginGroup(QString("MAV%1").arg(uasId)); - settings.setValue("NAME", this->name); - settings.setValue("AIRFRAME", this->airframe); - settings.setValue("AP_TYPE", this->autopilot); - settings.endGroup(); - settings.sync(); -} - -void UAS::readSettings() -{ - QSettings settings; - settings.beginGroup(QString("MAV%1").arg(uasId)); - this->name = settings.value("NAME", this->name).toString(); - this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); - this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt(); - settings.endGroup(); -} - -int UAS::getUASID() const -{ - return uasId; -} - -void UAS::updateState() -{ - // Check if heartbeat timed out - quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat; - if (heartbeatInterval > timeoutIntervalHeartbeat) - { - emit heartbeatTimeout(heartbeatInterval); - emit heartbeatTimeout(); - } - - // Position lock is set by the MAVLink message handler - // if no position lock is available, indicate an error - if (positionLock) - { - positionLock = false; - } - else - { - if (mode > (uint8_t)MAV_MODE_LOCKED && positionLock) - { - GAudioOutput::instance()->notifyNegative(); - } - } -} - -void UAS::setSelected() -{ - if (UASManager::instance()->getActiveUAS() != this) - { - UASManager::instance()->setActiveUAS(this); - emit systemSelected(true); - } -} - -bool UAS::getSelected() const -{ - return (UASManager::instance()->getActiveUAS() == this); -} - -void UAS::receiveMessageNamedValue(const mavlink_message_t& message) -{ - if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT) - { - mavlink_named_value_float_t val; - mavlink_msg_named_value_float_decode(&message, &val); - emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), val.value, getUnixTime(0)); - } - else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) - { - mavlink_named_value_int_t val; - mavlink_msg_named_value_int_decode(&message, &val); - emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), (float)val.value, getUnixTime(0)); - } -} - -void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) -{ - if (!link) return; - if (!links->contains(link)) - { - addLink(link); - // qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); - } - // 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; - - if (message.sysid == uasId) - { - QString uasState; - QString stateDescription; - QString patternPath; - - // Receive named value message - receiveMessageNamedValue(message); - - switch (message.msgid) - { - case MAVLINK_MSG_ID_HEARTBEAT: - lastHeartbeat = QGC::groundTimeUsecs(); - emit heartbeat(this); - // Set new type if it has changed - if (this->type != mavlink_msg_heartbeat_get_type(&message)) - { - this->type = mavlink_msg_heartbeat_get_type(&message); - if (airframe == 0) - { - switch (type) - { - case MAV_FIXED_WING: - setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); - break; - case MAV_QUADROTOR: - setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH); - break; - default: - // Do nothing - break; - } - } - this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message); - emit systemTypeSet(this, type); - } - - break; - case MAVLINK_MSG_ID_BOOT: - getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription); - emit statusChanged(this, uasState, stateDescription); - onboardTimeOffset = 0; // Reset offset measurement - break; - case MAVLINK_MSG_ID_SYS_STATUS: - { - mavlink_sys_status_t state; - mavlink_msg_sys_status_decode(&message, &state); - - // FIXME - //qDebug() << "SYSTEM NAV MODE:" << state.nav_mode; - - QString audiostring = "System " + QString::number(this->getUASID()); - QString stateAudio = ""; - QString modeAudio = ""; - bool statechanged = false; - bool modechanged = false; - - if (state.status != this->status) - { - statechanged = true; - this->status = (int)state.status; - getStatusForCode((int)state.status, uasState, stateDescription); - emit statusChanged(this, uasState, stateDescription); - emit statusChanged(this->status); - emit loadChanged(this,state.load/10.0f); - emit valueChanged(uasId, "Load", "%", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); - stateAudio = " changed status to " + uasState; - } - - if (this->mode != static_cast(state.mode)) - { - modechanged = true; - this->mode = static_cast(state.mode); - QString mode; - - switch (state.mode) - { - case (uint8_t)MAV_MODE_LOCKED: - mode = "LOCKED MODE"; - break; - case (uint8_t)MAV_MODE_MANUAL: - mode = "MANUAL MODE"; - break; - case (uint8_t)MAV_MODE_AUTO: - mode = "AUTO MODE"; - break; - case (uint8_t)MAV_MODE_GUIDED: - mode = "GUIDED MODE"; - break; - case (uint8_t)MAV_MODE_READY: - mode = "READY"; - break; - case (uint8_t)MAV_MODE_TEST1: - mode = "TEST1 MODE"; - break; - case (uint8_t)MAV_MODE_TEST2: - mode = "TEST2 MODE"; - break; - case (uint8_t)MAV_MODE_TEST3: - mode = "TEST3 MODE"; - break; - case (uint8_t)MAV_MODE_RC_TRAINING: - mode = "RC TRAINING MODE"; - break; - default: - mode = "UNINIT MODE"; - break; - } - - emit modeChanged(this->getUASID(), mode, ""); - modeAudio = " is now in " + mode; - } - currentVoltage = state.vbat/1000.0f; - lpVoltage = filterVoltage(currentVoltage); - if (startVoltage == 0) startVoltage = currentVoltage; - timeRemaining = calculateTimeRemaining(); - //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; - emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); - emit voltageChanged(message.sysid, state.vbat/1000.0f); - - // LOW BATTERY ALARM - float chargeLevel = getChargeLevel(); - if (chargeLevel <= 20.0f) - { - startLowBattAlarm(); - } - else - { - stopLowBattAlarm(); - } - - // COMMUNICATIONS DROP RATE - emit dropRateChanged(this->getUASID(), state.packet_drop/1000.0f); - //qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop; - - // AUDIO - if (modechanged && statechanged) - { - // Output both messages - audiostring += modeAudio + " and " + stateAudio; - } - else - { - // Output the one message - audiostring += modeAudio + stateAudio; - } - if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY) - { - GAudioOutput::instance()->startEmergency(); - } - else if (modechanged || statechanged) - { - GAudioOutput::instance()->stopEmergency(); - GAudioOutput::instance()->say(audiostring); - } - - if (state.status == MAV_STATE_POWEROFF) - { - emit systemRemoved(this); - emit systemRemoved(); - } - } - break; - case MAVLINK_MSG_ID_RAW_IMU: - { - mavlink_raw_imu_t raw; - mavlink_msg_raw_imu_decode(&message, &raw); - quint64 time = getUnixTime(raw.usec); - - emit valueChanged(uasId, "accel x", "raw", static_cast(raw.xacc), time); - emit valueChanged(uasId, "accel y", "raw", static_cast(raw.yacc), time); - emit valueChanged(uasId, "accel z", "raw", static_cast(raw.zacc), time); - emit valueChanged(uasId, "gyro roll", "raw", static_cast(raw.xgyro), time); - emit valueChanged(uasId, "gyro pitch", "raw", static_cast(raw.ygyro), time); - emit valueChanged(uasId, "gyro yaw", "raw", static_cast(raw.zgyro), time); - emit valueChanged(uasId, "mag x", "raw", static_cast(raw.xmag), time); - emit valueChanged(uasId, "mag y", "raw", static_cast(raw.ymag), time); - emit valueChanged(uasId, "mag z", "raw", static_cast(raw.zmag), time); - } - break; - case MAVLINK_MSG_ID_ATTITUDE: - //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; - { - mavlink_attitude_t attitude; - mavlink_msg_attitude_decode(&message, &attitude); - quint64 time = getUnixTime(attitude.usec); - 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", "rad", mavlink_msg_attitude_get_roll(&message), time); - emit valueChanged(uasId, "pitch", "rad", mavlink_msg_attitude_get_pitch(&message), time); - emit valueChanged(uasId, "yaw", "rad", mavlink_msg_attitude_get_yaw(&message), time); - emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time); - emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time); - emit valueChanged(uasId, "yawspeed", "rad/s", 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, "rollspeed", "deg/s", (attitude.rollspeed/M_PI)*180.0, time); - emit valueChanged(uasId, "pitchspeed", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time); - - // Force yaw to 180 deg range - double yaw = ((attitude.yaw/M_PI)*180.0); - double sign = 1.0; - if (yaw < 0) - { - sign = -1.0; - yaw = -yaw; - } - while (yaw > 180.0) - { - yaw -= 180.0; - } - - yaw *= sign; - - emit valueChanged(uasId, "yaw", "deg", yaw, time); - emit valueChanged(uasId, "yawspeed", "deg/s", (attitude.yawspeed/M_PI)*180.0, time); - - emit attitudeChanged(this, attitude.roll, attitude.pitch, attitude.yaw, time); - } - break; - case MAVLINK_MSG_ID_LOCAL_POSITION: - //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; - { - mavlink_local_position_t pos; - mavlink_msg_local_position_decode(&message, &pos); - quint64 time = getUnixTime(pos.usec); - localX = pos.x; - localY = pos.y; - localZ = pos.z; - emit valueChanged(uasId, "x", "m", pos.x, time); - emit valueChanged(uasId, "y", "m", pos.y, time); - emit valueChanged(uasId, "z", "m", pos.z, time); - emit valueChanged(uasId, "x speed", "m/s", pos.vx, time); - emit valueChanged(uasId, "y speed", "m/s", pos.vy, time); - emit valueChanged(uasId, "z speed", "m/s", pos.vz, time); - emit localPositionChanged(this, pos.x, pos.y, pos.z, time); - emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); - - // qDebug()<<"Local Position = "<notifyPositive(); - } - positionLock = true; - } - break; - case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: - //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; - { - mavlink_global_position_int_t pos; - mavlink_msg_global_position_int_decode(&message, &pos); - quint64 time = QGC::groundTimeUsecs()/1000; - latitude = pos.lat/(double)1E7; - longitude = pos.lon/(double)1E7; - altitude = pos.alt/1000.0; - speedX = pos.vx/100.0; - speedY = pos.vy/100.0; - speedZ = pos.vz/100.0; - emit valueChanged(uasId, "latitude", "deg", latitude, time); - emit valueChanged(uasId, "longitude", "deg", longitude, time); - emit valueChanged(uasId, "altitude", "m", altitude, time); - emit valueChanged(uasId, "gps x speed", "m/s", speedX, time); - emit valueChanged(uasId, "gps y speed", "m/s", speedY, time); - emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time); - emit globalPositionChanged(this, longitude, latitude, altitude, time); - emit speedChanged(this, speedX, speedY, speedZ, time); - // Set internal state - if (!positionLock) - { - // If position was not locked before, notify positive - GAudioOutput::instance()->notifyPositive(); - } - positionLock = true; - //TODO fix this hack for forwarding of global position for patch antenna tracking - forwardMessage(message); - } - break; - case MAVLINK_MSG_ID_GPS_RAW: - //std::cerr << std::endl; - //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; - { - mavlink_gps_raw_t pos; - mavlink_msg_gps_raw_decode(&message, &pos); - - // SANITY CHECK - // only accept values in a realistic range - // quint64 time = getUnixTime(pos.usec); - quint64 time = MG::TIME::getGroundTimeNow(); - - emit valueChanged(uasId, "latitude", "deg", pos.lat, time); - emit valueChanged(uasId, "longitude", "deg", pos.lon, time); - - // FIXME REMOVE - longitude = pos.lon; - latitude = pos.lat; - altitude = pos.alt; - emit globalPositionChanged(this, longitude, latitude, altitude, time); - - if (pos.fix_type > 0) - { - emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); - - // Check for NaN - int alt = pos.alt; - if (alt != alt) - { - alt = 0; - emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE"); - } - emit valueChanged(uasId, "altitude", "m", pos.alt, time); - // Smaller than threshold and not NaN - if (pos.v < 1000000 && pos.v == pos.v) - { - emit valueChanged(uasId, "speed", "m/s", pos.v, time); - //qDebug() << "GOT GPS RAW"; - // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); - } - else - { - emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v)); - } - } - } - break; - case MAVLINK_MSG_ID_GPS_STATUS: - { - mavlink_gps_status_t pos; - mavlink_msg_gps_status_decode(&message, &pos); - for(int i = 0; i < (int)pos.satellites_visible; i++) - { - emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast(pos.satellite_used[i])); - } - } - break; - case MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET: - { - mavlink_gps_local_origin_set_t pos; - mavlink_msg_gps_local_origin_set_decode(&message, &pos); - // FIXME Emit to other components - } - break; - case MAVLINK_MSG_ID_RAW_PRESSURE: - { - mavlink_raw_pressure_t pressure; - mavlink_msg_raw_pressure_decode(&message, &pressure); - quint64 time = this->getUnixTime(0); - emit valueChanged(uasId, "abs pressure", "hP", pressure.press_abs, time); - emit valueChanged(uasId, "diff pressure 1", "hP", pressure.press_diff1, time); - emit valueChanged(uasId, "diff pressure 2", "hP", pressure.press_diff2, time); - } - break; - case MAVLINK_MSG_ID_RC_CHANNELS_RAW: - { - mavlink_rc_channels_raw_t channels; - mavlink_msg_rc_channels_raw_decode(&message, &channels); - emit remoteControlRSSIChanged(channels.rssi/255.0f); - emit remoteControlChannelRawChanged(0, channels.chan1_raw); - emit remoteControlChannelRawChanged(1, channels.chan2_raw); - emit remoteControlChannelRawChanged(2, channels.chan3_raw); - emit remoteControlChannelRawChanged(3, channels.chan4_raw); - emit remoteControlChannelRawChanged(4, channels.chan5_raw); - emit remoteControlChannelRawChanged(5, channels.chan6_raw); - emit remoteControlChannelRawChanged(6, channels.chan7_raw); - emit remoteControlChannelRawChanged(7, channels.chan8_raw); - } - break; - case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: - { - mavlink_rc_channels_scaled_t channels; - mavlink_msg_rc_channels_scaled_decode(&message, &channels); - emit remoteControlRSSIChanged(channels.rssi/255.0f); - emit remoteControlChannelScaledChanged(0, channels.chan1_scaled/10000.0f); - emit remoteControlChannelScaledChanged(1, channels.chan2_scaled/10000.0f); - emit remoteControlChannelScaledChanged(2, channels.chan3_scaled/10000.0f); - emit remoteControlChannelScaledChanged(3, channels.chan4_scaled/10000.0f); - emit remoteControlChannelScaledChanged(4, channels.chan5_scaled/10000.0f); - emit remoteControlChannelScaledChanged(5, channels.chan6_scaled/10000.0f); - emit remoteControlChannelScaledChanged(6, channels.chan7_scaled/10000.0f); - emit remoteControlChannelScaledChanged(7, channels.chan8_scaled/10000.0f); - } - break; - case MAVLINK_MSG_ID_PARAM_VALUE: - { - mavlink_param_value_t value; - mavlink_msg_param_value_decode(&message, &value); - - QString parameterName = QString((char*)value.param_id); - int component = message.compid; - float val = value.param_value; - - // Insert component if necessary - if (!parameters.contains(component)) - { - parameters.insert(component, new QMap()); - } - - // Insert parameter into registry - if (parameters.value(component)->contains(parameterName)) parameters.value(component)->remove(parameterName); - parameters.value(component)->insert(parameterName, val); - - // Emit change - emit parameterChanged(uasId, message.compid, parameterName, val); - } - break; - case MAVLINK_MSG_ID_DEBUG: - emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); - break; - case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT: - { - mavlink_attitude_controller_output_t out; - mavlink_msg_attitude_controller_output_decode(&message, &out); - quint64 time = MG::TIME::getGroundTimeNowUsecs(); - emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time); - emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f); - emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f); - emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f); - } - break; - case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT: - { - mavlink_position_controller_output_t out; - mavlink_msg_position_controller_output_decode(&message, &out); - quint64 time = MG::TIME::getGroundTimeNow(); - //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time); - emit valueChanged(uasId, "pos control x", "raw", out.x, time); - emit valueChanged(uasId, "pos control y", "raw", out.y, time); - emit valueChanged(uasId, "pos control z", "raw", out.z, time); - } - break; - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - mavlink_waypoint_count_t wpc; - mavlink_msg_waypoint_count_decode(&message, &wpc); - if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId()) - { - waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count); - } - } - break; - - case MAVLINK_MSG_ID_WAYPOINT: - { - mavlink_waypoint_t wp; - mavlink_msg_waypoint_decode(&message, &wp); - //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z; - if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId()) - { - waypointManager.handleWaypoint(message.sysid, message.compid, &wp); - } - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - mavlink_waypoint_ack_t wpa; - mavlink_msg_waypoint_ack_decode(&message, &wpa); - if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) - { - waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa); - } - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - mavlink_waypoint_request_t wpr; - mavlink_msg_waypoint_request_decode(&message, &wpr); - if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId()) - { - waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); - } - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_REACHED: - { - mavlink_waypoint_reached_t wpr; - mavlink_msg_waypoint_reached_decode(&message, &wpr); - waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr); - } - break; - - case MAVLINK_MSG_ID_WAYPOINT_CURRENT: - { - mavlink_waypoint_current_t wpc; - mavlink_msg_waypoint_current_decode(&message, &wpc); - waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc); - } - break; - - case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: - { - mavlink_local_position_setpoint_t p; - mavlink_msg_local_position_setpoint_decode(&message, &p); - emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); - } - break; - - case MAVLINK_MSG_ID_STATUSTEXT: - { - QByteArray b; - b.resize(256); - mavlink_msg_statustext_get_text(&message, (int8_t*)b.data()); - //b.append('\0'); - QString text = QString(b); - int severity = mavlink_msg_statustext_get_severity(&message); - //qDebug() << "RECEIVED STATUS:" << text;false - //emit statusTextReceived(severity, text); - emit textMessageReceived(uasId, message.compid, severity, text); - } - break; - case MAVLINK_MSG_ID_DEBUG_VECT: - { - mavlink_debug_vect_t vect; - mavlink_msg_debug_vect_decode(&message, &vect); - QString str((const char*)vect.name); - quint64 time = getUnixTime(vect.usec); - emit valueChanged(uasId, str+".x", "raw", vect.x, time); - emit valueChanged(uasId, str+".y", "raw", vect.y, time); - emit valueChanged(uasId, str+".z", "raw", 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_UALBERTA - case MAVLINK_MSG_ID_NAV_FILTER_BIAS: - { - mavlink_nav_filter_bias_t bias; - mavlink_msg_nav_filter_bias_decode(&message, &bias); - quint64 time = MG::TIME::getGroundTimeNow(); - emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time); - emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time); - emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time); - emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time); - emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time); - emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time); - } - break; - case MAVLINK_MSG_ID_RADIO_CALIBRATION: - { - mavlink_radio_calibration_t radioMsg; - mavlink_msg_radio_calibration_decode(&message, &radioMsg); - QVector aileron; - QVector elevator; - QVector rudder; - QVector gyro; - QVector pitch; - QVector throttle; - - for (int i=0; i radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle); - emit radioCalibrationReceived(radioData); - delete radioData; - } - break; - -#endif - default: - { - if (!unknownPackets.contains(message.msgid)) - { - unknownPackets.append(message.msgid); - //GAudioOutput::instance()->say("UNABLE TO DECODE MESSAGE WITH ID " + QString::number(message.msgid) + " FROM SYSTEM " + QString::number(message.sysid)); - std::cout << "Unable to decode message from system " << std::dec << static_cast(message.sysid) << " with message id:" << static_cast(message.msgid) << std::endl; - //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast(message.acid) << " with message id:" << static_cast(message.msgid) << std::endl; - } - } - break; - } - } -} - -void UAS::setLocalOriginAtCurrentGPSPosition() -{ - - bool result = false; - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Warning); - msgBox.setText("Setting new World Coordinate Frame Origin"); - msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); - - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - - if (ret == QMessageBox::Yes) - { - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getSystemId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN); - // Send message twice to increase chance that it reaches its goal - sendMessage(msg); - // Wait 5 ms - MG::SLEEP::usleep(5000); - // Send again - sendMessage(msg); - result = true; - } -} - -void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) -{ -#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 - Q_UNUSED(x); - Q_UNUSED(y); - Q_UNUSED(z); - Q_UNUSED(yaw); -#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); -#else - 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); -} - -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); -} - -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); -} - -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); -} - -void UAS::startMagnetometerCalibration() -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG); - sendMessage(msg); -} - -void UAS::startGyroscopeCalibration() -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO); - sendMessage(msg); -} - -void UAS::startPressureCalibration() -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE); - sendMessage(msg); -} - -quint64 UAS::getUnixTime(quint64 time) -{ - if (time == 0) - { - return MG::TIME::getGroundTimeNow(); - } - // Check if time is smaller than 40 years, - // assuming no system without Unix timestamp - // runs longer than 40 years continuously without - // reboot. In worst case this will add/subtract the - // communication delay between GCS and MAV, - // it will never alter the timestamp in a safety - // critical way. - // - // Calculation: - // 40 years - // 365 days - // 24 hours - // 60 minutes - // 60 seconds - // 1000 milliseconds - // 1000 microseconds -#ifndef _MSC_VER - else if (time < 1261440000000000LLU) -#else - else if (time < 1261440000000000) -#endif - { - if (onboardTimeOffset == 0) - { - onboardTimeOffset = MG::TIME::getGroundTimeNow() - time/1000; - } - return time/1000 + onboardTimeOffset; - } - else - { - // Time is not zero and larger than 40 years -> has to be - // a Unix epoch timestamp. Do nothing. - return time/1000; - } -} - -QList UAS::getParameterNames(int component) -{ - if (parameters.contains(component)) - { - return parameters.value(component)->keys(); - } - else - { - return QList(); - } -} - -QList UAS::getComponentIds() -{ - return parameters.keys(); -} - -void UAS::setMode(int mode) -{ - if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) - { - this->mode = mode; - mavlink_message_t msg; - mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode); - sendMessage(msg); - qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; - } -} - -void UAS::sendMessage(mavlink_message_t message) -{ - // Emit message on all links that are currently connected - QList::iterator i; - for (i = links->begin(); i != links->end(); ++i) - { - sendMessage(*i, message); - } -} - -void UAS::forwardMessage(mavlink_message_t message) -{ - // Emit message on all links that are currently connected - QListlink_list = LinkManager::instance()->getLinksForProtocol(mavlink); - - foreach(LinkInterface* link, link_list) - { - if (link) - { - SerialLink* serial = dynamic_cast(link); - if(serial != 0) - { - - for(int i=0;isize();i++) - { - if(serial != links->at(i)) - { - qDebug()<<"Forwarding Over link: "<getName()<<" "<getSystemId(), mavlink->getComponentId(), link->getId(), message.len); - // If link is connected - if (link->isConnected()) - { - // Send the portion of the buffer now occupied by the message - link->writeBytes((const char*)buffer, len); - } -} - -/** - * @param value battery voltage - */ -float UAS::filterVoltage(float value) const -{ - return lpVoltage * 0.7f + value * 0.3f; -} - -void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription) -{ - switch (statusCode) - { - case MAV_STATE_UNINIT: - uasState = tr("UNINIT"); - stateDescription = tr("Waiting.."); - break; - case MAV_STATE_BOOT: - uasState = tr("BOOT"); - stateDescription = tr("Booting.."); - break; - case MAV_STATE_CALIBRATING: - uasState = tr("CALIBRATING"); - stateDescription = tr("Calibrating.."); - break; - case MAV_STATE_ACTIVE: - uasState = tr("ACTIVE"); - stateDescription = tr("Normal"); - break; - case MAV_STATE_STANDBY: - uasState = tr("STANDBY"); - stateDescription = tr("Standby, OK"); - break; - case MAV_STATE_CRITICAL: - uasState = tr("CRITICAL"); - stateDescription = tr("FAILURE: Continue"); - break; - case MAV_STATE_EMERGENCY: - uasState = tr("EMERGENCY"); - stateDescription = tr("EMERGENCY: Land!"); - break; - case MAV_STATE_POWEROFF: - uasState = tr("SHUTDOWN"); - stateDescription = tr("Powering off"); - break; - default: - uasState = tr("UNKNOWN"); - stateDescription = tr("Unknown state"); - break; - } -} - - - -/* MANAGEMENT */ - -/* - * - * @return The uptime in milliseconds - * - **/ -quint64 UAS::getUptime() const -{ - if(startTime == 0) { - return 0; - } else { - return MG::TIME::getGroundTimeNow() - startTime; - } -} - -int UAS::getCommunicationStatus() const -{ - return commStatus; -} - -void UAS::requestParameters() -{ - mavlink_message_t msg; - mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 25); - // Send message twice to increase chance of reception - sendMessage(msg); -} - -void UAS::writeParametersToStorage() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); - //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); - sendMessage(msg); -} - -void UAS::readParametersFromStorage() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ); - sendMessage(msg); -} - -void UAS::enableAllDataTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - // 0 is a magic ID and will enable/disable the standard message set except for heartbeat - stream.req_stream_id = MAV_DATA_STREAM_ALL; - // Select the update rate in Hz the message should be send - // All messages will be send with their default rate - // TODO: use 0 to turn off and get rid of enable/disable? will require - // a different magic flag for turning on defaults, possibly something really high like 1111 ? - stream.req_message_rate = 0; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enableRawSensorDataTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enableExtendedSystemStatusTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_EXTENDED_STATUS; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enableRCChannelDataTransmission(int rate) -{ -#if defined(MAVLINK_ENABLED_UALBERTA_MESSAGES) - mavlink_message_t msg; - mavlink_msg_request_rc_channels_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, enabled); - sendMessage(msg); -#else - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_RC_CHANNELS; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -#endif -} - -void UAS::enableRawControllerDataTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_RAW_CONTROLLER; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enableRawSensorFusionTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enablePositionTransmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_POSITION; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enableExtra1Transmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_EXTRA1; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enableExtra2Transmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_EXTRA2; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::enableExtra3Transmission(int rate) -{ - // Buffers to write data to - mavlink_message_t msg; - mavlink_request_data_stream_t stream; - // Select the message to request from now on - stream.req_stream_id = MAV_DATA_STREAM_EXTRA3; - // Select the update rate in Hz the message should be send - stream.req_message_rate = rate; - // Start / stop the message - stream.start_stop = (rate) ? 1 : 0; - // The system which should take this command - stream.target_system = uasId; - // The component / subsystem which should take this command - stream.target_component = 0; - // Encode and send the message - mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -/** - * Set a parameter value onboard - * - * @param component The component to set the parameter - * @param id Name of the parameter - * @param value Parameter value - */ -void UAS::setParameter(const int component, const QString& id, const float value) -{ - if (!id.isNull()) - { - mavlink_message_t msg; - mavlink_param_set_t p; - p.param_value = value; - p.target_system = (uint8_t)uasId; - p.target_component = (uint8_t)component; - - // Copy string into buffer, ensuring not to exceed the buffer size - for (unsigned int i = 0; i < sizeof(p.param_id); i++) - { - // String characters - if ((int)i < id.length() && i < (sizeof(p.param_id) - 1)) - { - p.param_id[i] = id.toAscii()[i]; - } - // Null termination at end of string or end of buffer - else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1)) - { - p.param_id[i] = '\0'; - } - // Zero fill - else - { - p.param_id[i] = 0; - } - } - mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p); - sendMessage(msg); - } -} - -void UAS::setUASName(const QString& name) -{ - this->name = name; - writeSettings(); - emit nameChanged(name); - emit systemSpecsChanged(uasId); -} - -/** - * Sets an action - * - **/ -void UAS::setAction(MAV_ACTION action) -{ - mavlink_message_t msg; - mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action); - // Send message twice to increase chance that it reaches its goal - sendMessage(msg); - sendMessage(msg); -} - -/** - * Launches the system - * - **/ -void UAS::launch() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_LAUNCH); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -/** - * Depending on the UAS, this might make the rotors of a helicopter spinning - * - **/ -void UAS::enable_motors() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -/** - * @warning Depending on the UAS, this might completely stop all motors. - * - **/ -void UAS::disable_motors() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust) -{ - // Scale values - double rollPitchScaling = 0.2f; - double yawScaling = 0.5f; - double thrustScaling = 1.0f; - - manualRollAngle = roll * rollPitchScaling; - manualPitchAngle = pitch * rollPitchScaling; - manualYawAngle = yaw * yawScaling; - manualThrust = thrust * thrustScaling; - - if(mode == (int)MAV_MODE_MANUAL) - { - mavlink_message_t message; - mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &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()); - } - else - { - qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first"; - } -} - -int UAS::getSystemType() -{ - return this->type; -} - -void UAS::receiveButton(int buttonIndex) -{ - switch (buttonIndex) - { - case 0: - - break; - case 1: - - break; - default: - - break; - } - // qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!"; - -} - - -/*void UAS::requestWaypoints() -{ -// mavlink_message_t msg; -// mavlink_msg_waypoint_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 25); -// // Send message twice to increase chance of reception -// sendMessage(msg); - waypointManager.requestWaypoints(); - qDebug() << "UAS Request WPs"; -} - -void UAS::setWaypoint(Waypoint* wp) -{ -// mavlink_message_t msg; -// mavlink_waypoint_set_t set; -// set.id = wp->id; -// //QString name = wp->name; -// // FIXME Check if this works properly -// //name.truncate(MAVLINK_MSG_WAYPOINT_SET_FIELD_NAME_LEN); -// //strcpy((char*)set.name, name.toStdString().c_str()); -// set.autocontinue = wp->autocontinue; -// set.target_component = 25; // FIXME -// set.target_system = uasId; -// set.active = wp->current; -// set.x = wp->x; -// set.y = wp->y; -// set.z = wp->z; -// set.yaw = wp->yaw; -// mavlink_msg_waypoint_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &set); -// // Send message twice to increase chance of reception -// sendMessage(msg); -} - -void UAS::setWaypointActive(int id) -{ -// mavlink_message_t msg; -// mavlink_waypoint_set_active_t active; -// active.id = id; -// active.target_system = uasId; -// active.target_component = 25; // FIXME -// mavlink_msg_waypoint_set_active_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &active); -// // Send message twice to increase chance of reception -// sendMessage(msg); -// sendMessage(msg); -// // TODO This should be not directly emitted, but rather being fed back from the UAS -// emit waypointSelected(getUASID(), id); -} - -void UAS::clearWaypointList() -{ -// mavlink_message_t msg; -// // FIXME -// mavlink_waypoint_clear_list_t clist; -// clist.target_system = uasId; -// clist.target_component = 25; // FIXME -// mavlink_msg_waypoint_clear_list_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &clist); -// sendMessage(msg); -// qDebug() << "UAS clears Waypoints!"; -}*/ - - -void UAS::halt() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -void UAS::go() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_CONTINUE); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -/** Order the robot to return home / to land on the runway **/ -void UAS::home() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_RETURN); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -/** - * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation - * and might differ between systems. - */ -void UAS::emergencySTOP() -{ - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -/** - * All systems are immediately shut down (e.g. the main power line is cut). - * @warning This might lead to a crash - * - * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards - */ -bool UAS::emergencyKILL() -{ - bool result = false; - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); - msgBox.setInformativeText("Do you want to cut power on all systems?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); - - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - - if (ret == QMessageBox::Yes) - { - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); - result = true; - } - return result; -} - -void UAS::shutdown() -{ - bool result = false; - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("Shutting down the UAS"); - msgBox.setInformativeText("Do you want to shut down the onboard computer?"); - msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); - msgBox.setDefaultButton(QMessageBox::Cancel); - int ret = msgBox.exec(); - - // Close the message box shortly after the click to prevent accidental clicks - QTimer::singleShot(5000, &msgBox, SLOT(reject())); - - - if (ret == QMessageBox::Yes) - { - // If the active UAS is set, execute command - mavlink_message_t msg; - // TODO Replace MG System ID with static function call and allow to change ID in GUI - mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN); - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); - result = true; - } -} - -void UAS::setTargetPosition(float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - mavlink_msg_position_target_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, x, y, z, yaw); - - // Send message twice to increase chance of reception - sendMessage(msg); - sendMessage(msg); -} - -/** - * @return The name of this system as string in human-readable form - */ -QString UAS::getUASName(void) const -{ - QString result; - if (name == "") - { - result = tr("MAV ") + result.sprintf("%03d", getUASID()); - } - else - { - result = name; - } - return result; -} - -void UAS::addLink(LinkInterface* link) -{ - if (!links->contains(link)) - { - links->append(link); - connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); - } - //links->append(link); - //qDebug() << link<<" ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK"; -} - -void UAS::removeLink(QObject* object) -{ - LinkInterface* link = dynamic_cast(object); - if (link) - { - links->removeAt(links->indexOf(link)); - } -} - -/** - * @brief Get the links associated with this robot - * - **/ -QList* UAS::getLinks() -{ - return links; -} - - - -void UAS::setBattery(BatteryType type, int cells) -{ - this->batteryType = type; - this->cells = cells; - switch (batteryType) - { - case NICD: - break; - case NIMH: - break; - case LIION: - break; - case LIPOLY: - fullVoltage = this->cells * UAS::lipoFull; - emptyVoltage = this->cells * UAS::lipoEmpty; - break; - case LIFE: - break; - case AGZN: - break; - } -} - -int UAS::calculateTimeRemaining() -{ - quint64 dt = MG::TIME::getGroundTimeNow() - startTime; - double seconds = dt / 1000.0f; - double voltDifference = startVoltage - currentVoltage; - if (voltDifference <= 0) voltDifference = 0.00000000001f; - double dischargePerSecond = voltDifference / seconds; - int remaining = static_cast((currentVoltage - emptyVoltage) / dischargePerSecond); - // Can never be below 0 - if (remaining < 0) remaining = 0; - return remaining; -} - -/** - * @return charge level in percent - 0 - 100 - */ -double UAS::getChargeLevel() -{ - float chargeLevel; - if (lpVoltage < emptyVoltage) - { - chargeLevel = 0.0f; - } - else if (lpVoltage > fullVoltage) - { - chargeLevel = 100.0f; - } - else - { - chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); - } - return chargeLevel; -} - -void UAS::startLowBattAlarm() -{ - if (!lowBattAlarm) - { - GAudioOutput::instance()->alert("LOW BATTERY"); - QTimer::singleShot(2000, GAudioOutput::instance(), SLOT(startEmergency())); - lowBattAlarm = true; - } -} - -void UAS::stopLowBattAlarm() -{ - if (lowBattAlarm) - { - GAudioOutput::instance()->stopEmergency(); - lowBattAlarm = false; - } -} +/*=================================================================== +======================================================================*/ + +/** + * @file + * @brief Represents one unmanned aerial vehicle + * + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include "UAS.h" +#include "LinkInterface.h" +#include "UASManager.h" +//#include "MG.h" +#include "QGC.h" +#include "GAudioOutput.h" +#include "MAVLinkProtocol.h" +#include "QGCMAVLink.h" +#include "LinkManager.h" +#include "SerialLink.h" + +#ifndef M_PI +#define M_PI 3.14159265358979323846 /* pi */ +#endif + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 /* pi/2 */ +#endif + +#ifndef M_PI_4 +#define M_PI_4 0.78539816339744830962 /* pi/4 */ +#endif + + + +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), +airframe(0) +{ + color = UASInterface::getNextColor(); + setBattery(LIPOLY, 3); + connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState())); + connect(this, SIGNAL(systemSpecsChanged(int)), this, SLOT(writeSettings())); + statusTimeout->start(500); + readSettings(); +} + +UAS::~UAS() +{ + writeSettings(); + delete links; + links=NULL; +} + +void UAS::writeSettings() +{ + QSettings settings; + settings.beginGroup(QString("MAV%1").arg(uasId)); + settings.setValue("NAME", this->name); + settings.setValue("AIRFRAME", this->airframe); + settings.setValue("AP_TYPE", this->autopilot); + settings.endGroup(); + settings.sync(); +} + +void UAS::readSettings() +{ + QSettings settings; + settings.beginGroup(QString("MAV%1").arg(uasId)); + this->name = settings.value("NAME", this->name).toString(); + this->airframe = settings.value("AIRFRAME", this->airframe).toInt(); + this->autopilot = settings.value("AP_TYPE", this->autopilot).toInt(); + settings.endGroup(); +} + +int UAS::getUASID() const +{ + return uasId; +} + +void UAS::updateState() +{ + // Check if heartbeat timed out + quint64 heartbeatInterval = QGC::groundTimeUsecs() - lastHeartbeat; + if (heartbeatInterval > timeoutIntervalHeartbeat) + { + emit heartbeatTimeout(heartbeatInterval); + emit heartbeatTimeout(); + } + + // Position lock is set by the MAVLink message handler + // if no position lock is available, indicate an error + if (positionLock) + { + positionLock = false; + } + else + { + if (mode > (uint8_t)MAV_MODE_LOCKED && positionLock) + { + GAudioOutput::instance()->notifyNegative(); + } + } +} + +void UAS::setSelected() +{ + if (UASManager::instance()->getActiveUAS() != this) + { + UASManager::instance()->setActiveUAS(this); + emit systemSelected(true); + } +} + +bool UAS::getSelected() const +{ + return (UASManager::instance()->getActiveUAS() == this); +} + +void UAS::receiveMessageNamedValue(const mavlink_message_t& message) +{ + if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_FLOAT) + { + mavlink_named_value_float_t val; + mavlink_msg_named_value_float_decode(&message, &val); + emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), val.value, getUnixTime(0)); + } + else if (message.msgid == MAVLINK_MSG_ID_NAMED_VALUE_INT) + { + mavlink_named_value_int_t val; + mavlink_msg_named_value_int_decode(&message, &val); + emit valueChanged(this->getUASID(), QString(val.name), tr("raw"), (float)val.value, getUnixTime(0)); + } +} + +void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) +{ + if (!link) return; + if (!links->contains(link)) + { + addLink(link); + // qDebug() << __FILE__ << __LINE__ << "ADDED LINK!" << link->getName(); + } + // 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; + + if (message.sysid == uasId) + { + QString uasState; + QString stateDescription; + QString patternPath; + + // Receive named value message + receiveMessageNamedValue(message); + + switch (message.msgid) + { + case MAVLINK_MSG_ID_HEARTBEAT: + lastHeartbeat = QGC::groundTimeUsecs(); + emit heartbeat(this); + // Set new type if it has changed + if (this->type != mavlink_msg_heartbeat_get_type(&message)) + { + this->type = mavlink_msg_heartbeat_get_type(&message); + if (airframe == 0) + { + switch (type) + { + case MAV_FIXED_WING: + setAirframe(UASInterface::QGC_AIRFRAME_EASYSTAR); + break; + case MAV_QUADROTOR: + setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH); + break; + default: + // Do nothing + break; + } + } + this->autopilot = mavlink_msg_heartbeat_get_autopilot(&message); + emit systemTypeSet(this, type); + } + + break; + case MAVLINK_MSG_ID_BOOT: + getStatusForCode((int)MAV_STATE_BOOT, uasState, stateDescription); + emit statusChanged(this, uasState, stateDescription); + onboardTimeOffset = 0; // Reset offset measurement + break; + case MAVLINK_MSG_ID_SYS_STATUS: + { + mavlink_sys_status_t state; + mavlink_msg_sys_status_decode(&message, &state); + + // FIXME + //qDebug() << "SYSTEM NAV MODE:" << state.nav_mode; + + QString audiostring = "System " + QString::number(this->getUASID()); + QString stateAudio = ""; + QString modeAudio = ""; + bool statechanged = false; + bool modechanged = false; + + if (state.status != this->status) + { + statechanged = true; + this->status = (int)state.status; + getStatusForCode((int)state.status, uasState, stateDescription); + emit statusChanged(this, uasState, stateDescription); + emit statusChanged(this->status); + emit loadChanged(this,state.load/10.0f); + emit valueChanged(uasId, "Load", "%", ((float)state.load)/1000.0f, MG::TIME::getGroundTimeNow()); + stateAudio = " changed status to " + uasState; + } + + if (this->mode != static_cast(state.mode)) + { + modechanged = true; + this->mode = static_cast(state.mode); + QString mode; + + switch (state.mode) + { + case (uint8_t)MAV_MODE_LOCKED: + mode = "LOCKED MODE"; + break; + case (uint8_t)MAV_MODE_MANUAL: + mode = "MANUAL MODE"; + break; + case (uint8_t)MAV_MODE_AUTO: + mode = "AUTO MODE"; + break; + case (uint8_t)MAV_MODE_GUIDED: + mode = "GUIDED MODE"; + break; + case (uint8_t)MAV_MODE_READY: + mode = "READY"; + break; + case (uint8_t)MAV_MODE_TEST1: + mode = "TEST1 MODE"; + break; + case (uint8_t)MAV_MODE_TEST2: + mode = "TEST2 MODE"; + break; + case (uint8_t)MAV_MODE_TEST3: + mode = "TEST3 MODE"; + break; + case (uint8_t)MAV_MODE_RC_TRAINING: + mode = "RC TRAINING MODE"; + break; + default: + mode = "UNINIT MODE"; + break; + } + + emit modeChanged(this->getUASID(), mode, ""); + modeAudio = " is now in " + mode; + } + currentVoltage = state.vbat/1000.0f; + lpVoltage = filterVoltage(currentVoltage); + if (startVoltage == 0) startVoltage = currentVoltage; + timeRemaining = calculateTimeRemaining(); + //qDebug() << "Voltage: " << currentVoltage << " Chargelevel: " << getChargeLevel() << " Time remaining " << timeRemaining; + emit batteryChanged(this, lpVoltage, getChargeLevel(), timeRemaining); + emit voltageChanged(message.sysid, state.vbat/1000.0f); + + // LOW BATTERY ALARM + float chargeLevel = getChargeLevel(); + if (chargeLevel <= 20.0f) + { + startLowBattAlarm(); + } + else + { + stopLowBattAlarm(); + } + + // COMMUNICATIONS DROP RATE + emit dropRateChanged(this->getUASID(), state.packet_drop/1000.0f); + //qDebug() << __FILE__ << __LINE__ << "RCV LOSS: " << state.packet_drop; + + // AUDIO + if (modechanged && statechanged) + { + // Output both messages + audiostring += modeAudio + " and " + stateAudio; + } + else + { + // Output the one message + audiostring += modeAudio + stateAudio; + } + if ((int)state.status == (int)MAV_STATE_CRITICAL || state.status == (int)MAV_STATE_EMERGENCY) + { + GAudioOutput::instance()->startEmergency(); + } + else if (modechanged || statechanged) + { + GAudioOutput::instance()->stopEmergency(); + GAudioOutput::instance()->say(audiostring); + } + + if (state.status == MAV_STATE_POWEROFF) + { + emit systemRemoved(this); + emit systemRemoved(); + } + } + break; + case MAVLINK_MSG_ID_RAW_IMU: + { + mavlink_raw_imu_t raw; + mavlink_msg_raw_imu_decode(&message, &raw); + quint64 time = getUnixTime(raw.usec); + + emit valueChanged(uasId, "accel x", "raw", static_cast(raw.xacc), time); + emit valueChanged(uasId, "accel y", "raw", static_cast(raw.yacc), time); + emit valueChanged(uasId, "accel z", "raw", static_cast(raw.zacc), time); + emit valueChanged(uasId, "gyro roll", "raw", static_cast(raw.xgyro), time); + emit valueChanged(uasId, "gyro pitch", "raw", static_cast(raw.ygyro), time); + emit valueChanged(uasId, "gyro yaw", "raw", static_cast(raw.zgyro), time); + emit valueChanged(uasId, "mag x", "raw", static_cast(raw.xmag), time); + emit valueChanged(uasId, "mag y", "raw", static_cast(raw.ymag), time); + emit valueChanged(uasId, "mag z", "raw", static_cast(raw.zmag), time); + } + break; + case MAVLINK_MSG_ID_ATTITUDE: + //std::cerr << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; + { + mavlink_attitude_t attitude; + mavlink_msg_attitude_decode(&message, &attitude); + quint64 time = getUnixTime(attitude.usec); + 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 batteryChanged(this, 25, 15, time*1000); + //emit thrustChanged(this, 30); + //emit modeChanged(1, "miModo", "Descripcion"); + + + emit valueChanged(uasId, "roll", "rad", mavlink_msg_attitude_get_roll(&message), time); + emit valueChanged(uasId, "pitch", "rad", mavlink_msg_attitude_get_pitch(&message), time); + emit valueChanged(uasId, "yaw", "rad", mavlink_msg_attitude_get_yaw(&message), time); + emit valueChanged(uasId, "rollspeed", "rad/s", attitude.rollspeed, time); + emit valueChanged(uasId, "pitchspeed", "rad/s", attitude.pitchspeed, time); + emit valueChanged(uasId, "yawspeed", "rad/s", 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, "rollspeed", "deg/s", (attitude.rollspeed/M_PI)*180.0, time); + emit valueChanged(uasId, "pitchspeed", "deg/s", (attitude.pitchspeed/M_PI)*180.0, time); + + // Force yaw to 180 deg range + double yaw = ((attitude.yaw/M_PI)*180.0); + double sign = 1.0; + if (yaw < 0) + { + sign = -1.0; + yaw = -yaw; + } + while (yaw > 180.0) + { + yaw -= 180.0; + } + + yaw *= sign; + + emit valueChanged(uasId, "yaw", "deg", yaw, time); + emit valueChanged(uasId, "yawspeed", "deg/s", (attitude.yawspeed/M_PI)*180.0, time); + + emit attitudeChanged(this, attitude.roll, attitude.pitch, attitude.yaw, time); + } + break; + case MAVLINK_MSG_ID_LOCAL_POSITION: + //std::cerr << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(&message, &pos); + quint64 time = getUnixTime(pos.usec); + localX = pos.x; + localY = pos.y; + localZ = pos.z; + emit valueChanged(uasId, "x", "m", pos.x, time); + emit valueChanged(uasId, "y", "m", pos.y, time); + emit valueChanged(uasId, "z", "m", pos.z, time); + emit valueChanged(uasId, "x speed", "m/s", pos.vx, time); + emit valueChanged(uasId, "y speed", "m/s", pos.vy, time); + emit valueChanged(uasId, "z speed", "m/s", pos.vz, time); + emit localPositionChanged(this, pos.x, pos.y, pos.z, time); + emit speedChanged(this, pos.vx, pos.vy, pos.vz, time); + + // qDebug()<<"Local Position = "<notifyPositive(); + } + positionLock = true; + } + break; + case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: + //std::cerr << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; + { + mavlink_global_position_int_t pos; + mavlink_msg_global_position_int_decode(&message, &pos); + quint64 time = QGC::groundTimeUsecs()/1000; + latitude = pos.lat/(double)1E7; + longitude = pos.lon/(double)1E7; + altitude = pos.alt/1000.0; + speedX = pos.vx/100.0; + speedY = pos.vy/100.0; + speedZ = pos.vz/100.0; + emit valueChanged(uasId, "latitude", "deg", latitude, time); + emit valueChanged(uasId, "longitude", "deg", longitude, time); + emit valueChanged(uasId, "altitude", "m", altitude, time); + emit valueChanged(uasId, "gps x speed", "m/s", speedX, time); + emit valueChanged(uasId, "gps y speed", "m/s", speedY, time); + emit valueChanged(uasId, "gps z speed", "m/s", speedZ, time); + emit globalPositionChanged(this, longitude, latitude, altitude, time); + emit speedChanged(this, speedX, speedY, speedZ, time); + // Set internal state + if (!positionLock) + { + // If position was not locked before, notify positive + GAudioOutput::instance()->notifyPositive(); + } + positionLock = true; + //TODO fix this hack for forwarding of global position for patch antenna tracking + forwardMessage(message); + } + break; + case MAVLINK_MSG_ID_GPS_RAW: + //std::cerr << std::endl; + //std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl; + { + mavlink_gps_raw_t pos; + mavlink_msg_gps_raw_decode(&message, &pos); + + // SANITY CHECK + // only accept values in a realistic range + // quint64 time = getUnixTime(pos.usec); + quint64 time = MG::TIME::getGroundTimeNow(); + + emit valueChanged(uasId, "latitude", "deg", pos.lat, time); + emit valueChanged(uasId, "longitude", "deg", pos.lon, time); + + // FIXME REMOVE + longitude = pos.lon; + latitude = pos.lat; + altitude = pos.alt; + emit globalPositionChanged(this, longitude, latitude, altitude, time); + + if (pos.fix_type > 0) + { + emit globalPositionChanged(this, pos.lon, pos.lat, pos.alt, time); + + // Check for NaN + int alt = pos.alt; + if (alt != alt) + { + alt = 0; + emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE"); + } + emit valueChanged(uasId, "altitude", "m", pos.alt, time); + // Smaller than threshold and not NaN + if (pos.v < 1000000 && pos.v == pos.v) + { + emit valueChanged(uasId, "speed", "m/s", pos.v, time); + //qDebug() << "GOT GPS RAW"; + // emit speedChanged(this, (double)pos.v, 0.0, 0.0, time); + } + else + { + emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(pos.v)); + } + } + } + break; + case MAVLINK_MSG_ID_GPS_STATUS: + { + mavlink_gps_status_t pos; + mavlink_msg_gps_status_decode(&message, &pos); + for(int i = 0; i < (int)pos.satellites_visible; i++) + { + emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast(pos.satellite_used[i])); + } + } + break; + case MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET: + { + mavlink_gps_local_origin_set_t pos; + mavlink_msg_gps_local_origin_set_decode(&message, &pos); + // FIXME Emit to other components + } + break; + case MAVLINK_MSG_ID_RAW_PRESSURE: + { + mavlink_raw_pressure_t pressure; + mavlink_msg_raw_pressure_decode(&message, &pressure); + quint64 time = this->getUnixTime(0); + emit valueChanged(uasId, "abs pressure", "hP", pressure.press_abs, time); + emit valueChanged(uasId, "diff pressure 1", "hP", pressure.press_diff1, time); + emit valueChanged(uasId, "diff pressure 2", "hP", pressure.press_diff2, time); + } + break; + case MAVLINK_MSG_ID_RC_CHANNELS_RAW: + { + mavlink_rc_channels_raw_t channels; + mavlink_msg_rc_channels_raw_decode(&message, &channels); + emit remoteControlRSSIChanged(channels.rssi/255.0f); + emit remoteControlChannelRawChanged(0, channels.chan1_raw); + emit remoteControlChannelRawChanged(1, channels.chan2_raw); + emit remoteControlChannelRawChanged(2, channels.chan3_raw); + emit remoteControlChannelRawChanged(3, channels.chan4_raw); + emit remoteControlChannelRawChanged(4, channels.chan5_raw); + emit remoteControlChannelRawChanged(5, channels.chan6_raw); + emit remoteControlChannelRawChanged(6, channels.chan7_raw); + emit remoteControlChannelRawChanged(7, channels.chan8_raw); + } + break; + case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: + { + mavlink_rc_channels_scaled_t channels; + mavlink_msg_rc_channels_scaled_decode(&message, &channels); + emit remoteControlRSSIChanged(channels.rssi/255.0f); + emit remoteControlChannelScaledChanged(0, channels.chan1_scaled/10000.0f); + emit remoteControlChannelScaledChanged(1, channels.chan2_scaled/10000.0f); + emit remoteControlChannelScaledChanged(2, channels.chan3_scaled/10000.0f); + emit remoteControlChannelScaledChanged(3, channels.chan4_scaled/10000.0f); + emit remoteControlChannelScaledChanged(4, channels.chan5_scaled/10000.0f); + emit remoteControlChannelScaledChanged(5, channels.chan6_scaled/10000.0f); + emit remoteControlChannelScaledChanged(6, channels.chan7_scaled/10000.0f); + emit remoteControlChannelScaledChanged(7, channels.chan8_scaled/10000.0f); + } + break; + case MAVLINK_MSG_ID_PARAM_VALUE: + { + mavlink_param_value_t value; + mavlink_msg_param_value_decode(&message, &value); + + QString parameterName = QString((char*)value.param_id); + int component = message.compid; + float val = value.param_value; + + // Insert component if necessary + if (!parameters.contains(component)) + { + parameters.insert(component, new QMap()); + } + + // Insert parameter into registry + if (parameters.value(component)->contains(parameterName)) parameters.value(component)->remove(parameterName); + parameters.value(component)->insert(parameterName, val); + + // Emit change + emit parameterChanged(uasId, message.compid, parameterName, val); + } + break; + case MAVLINK_MSG_ID_DEBUG: + emit valueChanged(uasId, QString("debug ") + QString::number(mavlink_msg_debug_get_ind(&message)), "raw", mavlink_msg_debug_get_value(&message), MG::TIME::getGroundTimeNow()); + break; + case MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT: + { + mavlink_attitude_controller_output_t out; + mavlink_msg_attitude_controller_output_decode(&message, &out); + quint64 time = MG::TIME::getGroundTimeNowUsecs(); + emit attitudeThrustSetPointChanged(this, out.roll/127.0f, out.pitch/127.0f, out.yaw/127.0f, (uint8_t)out.thrust, time); + emit valueChanged(uasId, "att control roll", "raw", out.roll, time/1000.0f); + emit valueChanged(uasId, "att control pitch", "raw", out.pitch, time/1000.0f); + emit valueChanged(uasId, "att control yaw", "raw", out.yaw, time/1000.0f); + } + break; + case MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT: + { + mavlink_position_controller_output_t out; + mavlink_msg_position_controller_output_decode(&message, &out); + quint64 time = MG::TIME::getGroundTimeNow(); + //emit positionSetPointsChanged(uasId, out.x/127.0f, out.y/127.0f, out.z/127.0f, out.yaw, time); + emit valueChanged(uasId, "pos control x", "raw", out.x, time); + emit valueChanged(uasId, "pos control y", "raw", out.y, time); + emit valueChanged(uasId, "pos control z", "raw", out.z, time); + } + break; + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(&message, &wpc); + if (wpc.target_system == mavlink->getSystemId() && wpc.target_component == mavlink->getComponentId()) + { + waypointManager.handleWaypointCount(message.sysid, message.compid, wpc.count); + } + } + break; + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(&message, &wp); + //qDebug() << "got waypoint (" << wp.seq << ") from ID " << message.sysid << " x=" << wp.x << " y=" << wp.y << " z=" << wp.z; + if(wp.target_system == mavlink->getSystemId() && wp.target_component == mavlink->getComponentId()) + { + waypointManager.handleWaypoint(message.sysid, message.compid, &wp); + } + } + break; + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(&message, &wpa); + if(wpa.target_system == mavlink->getSystemId() && wpa.target_component == mavlink->getComponentId()) + { + waypointManager.handleWaypointAck(message.sysid, message.compid, &wpa); + } + } + break; + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(&message, &wpr); + if(wpr.target_system == mavlink->getSystemId() && wpr.target_component == mavlink->getComponentId()) + { + waypointManager.handleWaypointRequest(message.sysid, message.compid, &wpr); + } + } + break; + + case MAVLINK_MSG_ID_WAYPOINT_REACHED: + { + mavlink_waypoint_reached_t wpr; + mavlink_msg_waypoint_reached_decode(&message, &wpr); + waypointManager.handleWaypointReached(message.sysid, message.compid, &wpr); + } + break; + + case MAVLINK_MSG_ID_WAYPOINT_CURRENT: + { + mavlink_waypoint_current_t wpc; + mavlink_msg_waypoint_current_decode(&message, &wpc); + waypointManager.handleWaypointCurrent(message.sysid, message.compid, &wpc); + } + break; + + case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT: + { + mavlink_local_position_setpoint_t p; + mavlink_msg_local_position_setpoint_decode(&message, &p); + emit positionSetPointsChanged(uasId, p.x, p.y, p.z, p.yaw, QGC::groundTimeUsecs()); + } + break; + + case MAVLINK_MSG_ID_STATUSTEXT: + { + QByteArray b; + b.resize(256); + mavlink_msg_statustext_get_text(&message, (int8_t*)b.data()); + //b.append('\0'); + QString text = QString(b); + int severity = mavlink_msg_statustext_get_severity(&message); + //qDebug() << "RECEIVED STATUS:" << text;false + //emit statusTextReceived(severity, text); + emit textMessageReceived(uasId, message.compid, severity, text); + } + break; + case MAVLINK_MSG_ID_DEBUG_VECT: + { + mavlink_debug_vect_t vect; + mavlink_msg_debug_vect_decode(&message, &vect); + QString str((const char*)vect.name); + quint64 time = getUnixTime(vect.usec); + emit valueChanged(uasId, str+".x", "raw", vect.x, time); + emit valueChanged(uasId, str+".y", "raw", vect.y, time); + emit valueChanged(uasId, str+".z", "raw", 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_UALBERTA + case MAVLINK_MSG_ID_NAV_FILTER_BIAS: + { + mavlink_nav_filter_bias_t bias; + mavlink_msg_nav_filter_bias_decode(&message, &bias); + quint64 time = MG::TIME::getGroundTimeNow(); + emit valueChanged(uasId, "b_f[0]", "raw", bias.accel_0, time); + emit valueChanged(uasId, "b_f[1]", "raw", bias.accel_1, time); + emit valueChanged(uasId, "b_f[2]", "raw", bias.accel_2, time); + emit valueChanged(uasId, "b_w[0]", "raw", bias.gyro_0, time); + emit valueChanged(uasId, "b_w[1]", "raw", bias.gyro_1, time); + emit valueChanged(uasId, "b_w[2]", "raw", bias.gyro_2, time); + } + break; + case MAVLINK_MSG_ID_RADIO_CALIBRATION: + { + mavlink_radio_calibration_t radioMsg; + mavlink_msg_radio_calibration_decode(&message, &radioMsg); + QVector aileron; + QVector elevator; + QVector rudder; + QVector gyro; + QVector pitch; + QVector throttle; + + for (int i=0; i radioData = new RadioCalibrationData(aileron, elevator, rudder, gyro, pitch, throttle); + emit radioCalibrationReceived(radioData); + delete radioData; + } + break; + +#endif + default: + { + if (!unknownPackets.contains(message.msgid)) + { + unknownPackets.append(message.msgid); + //GAudioOutput::instance()->say("UNABLE TO DECODE MESSAGE WITH ID " + QString::number(message.msgid) + " FROM SYSTEM " + QString::number(message.sysid)); + std::cout << "Unable to decode message from system " << std::dec << static_cast(message.sysid) << " with message id:" << static_cast(message.msgid) << std::endl; + //qDebug() << std::cerr << "Unable to decode message from system " << std::dec << static_cast(message.acid) << " with message id:" << static_cast(message.msgid) << std::endl; + } + } + break; + } + } +} + +void UAS::setLocalOriginAtCurrentGPSPosition() +{ + + bool result = false; + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Warning); + msgBox.setText("Setting new World Coordinate Frame Origin"); + msgBox.setInformativeText("Do you want to set a new origin? Waypoints defined in the local frame will be shifted in their physical location"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Cancel); + int ret = msgBox.exec(); + + // Close the message box shortly after the click to prevent accidental clicks + QTimer::singleShot(5000, &msgBox, SLOT(reject())); + + + if (ret == QMessageBox::Yes) + { + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getSystemId(), &msg, this->getUASID(), 0, MAV_ACTION_SET_ORIGIN); + // Send message twice to increase chance that it reaches its goal + sendMessage(msg); + // Wait 5 ms + MG::SLEEP::usleep(5000); + // Send again + sendMessage(msg); + result = true; + } +} + +void UAS::setLocalPositionSetpoint(float x, float y, float z, float yaw) +{ +#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 + Q_UNUSED(x); + Q_UNUSED(y); + Q_UNUSED(z); + Q_UNUSED(yaw); +#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); +#else + 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); +} + +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); +} + +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); +} + +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); +} + +void UAS::startMagnetometerCalibration() +{ + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_MAG); + sendMessage(msg); +} + +void UAS::startGyroscopeCalibration() +{ + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_GYRO); + sendMessage(msg); +} + +void UAS::startPressureCalibration() +{ + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_ACTION_CALIBRATE_PRESSURE); + sendMessage(msg); +} + +quint64 UAS::getUnixTime(quint64 time) +{ + if (time == 0) + { + return MG::TIME::getGroundTimeNow(); + } + // Check if time is smaller than 40 years, + // assuming no system without Unix timestamp + // runs longer than 40 years continuously without + // reboot. In worst case this will add/subtract the + // communication delay between GCS and MAV, + // it will never alter the timestamp in a safety + // critical way. + // + // Calculation: + // 40 years + // 365 days + // 24 hours + // 60 minutes + // 60 seconds + // 1000 milliseconds + // 1000 microseconds +#ifndef _MSC_VER + else if (time < 1261440000000000LLU) +#else + else if (time < 1261440000000000) +#endif + { + if (onboardTimeOffset == 0) + { + onboardTimeOffset = MG::TIME::getGroundTimeNow() - time/1000; + } + return time/1000 + onboardTimeOffset; + } + else + { + // Time is not zero and larger than 40 years -> has to be + // a Unix epoch timestamp. Do nothing. + return time/1000; + } +} + +QList UAS::getParameterNames(int component) +{ + if (parameters.contains(component)) + { + return parameters.value(component)->keys(); + } + else + { + return QList(); + } +} + +QList UAS::getComponentIds() +{ + return parameters.keys(); +} + +void UAS::setMode(int mode) +{ + if ((uint8_t)mode >= MAV_MODE_LOCKED && (uint8_t)mode <= MAV_MODE_RC_TRAINING) + { + this->mode = mode; + mavlink_message_t msg; + mavlink_msg_set_mode_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, (uint8_t)uasId, (uint8_t)mode); + sendMessage(msg); + qDebug() << "SENDING REQUEST TO SET MODE TO SYSTEM" << uasId << ", REQUEST TO SET MODE " << (uint8_t)mode; + } +} + +void UAS::sendMessage(mavlink_message_t message) +{ + // Emit message on all links that are currently connected + QList::iterator i; + for (i = links->begin(); i != links->end(); ++i) + { + sendMessage(*i, message); + } +} + +void UAS::forwardMessage(mavlink_message_t message) +{ + // Emit message on all links that are currently connected + QListlink_list = LinkManager::instance()->getLinksForProtocol(mavlink); + + foreach(LinkInterface* link, link_list) + { + if (link) + { + SerialLink* serial = dynamic_cast(link); + if(serial != 0) + { + + for(int i=0;isize();i++) + { + if(serial != links->at(i)) + { + qDebug()<<"Forwarding Over link: "<getName()<<" "<getSystemId(), mavlink->getComponentId(), link->getId(), message.len); + // If link is connected + if (link->isConnected()) + { + // Send the portion of the buffer now occupied by the message + link->writeBytes((const char*)buffer, len); + } +} + +/** + * @param value battery voltage + */ +float UAS::filterVoltage(float value) const +{ + return lpVoltage * 0.7f + value * 0.3f; +} + +void UAS::getStatusForCode(int statusCode, QString& uasState, QString& stateDescription) +{ + switch (statusCode) + { + case MAV_STATE_UNINIT: + uasState = tr("UNINIT"); + stateDescription = tr("Waiting.."); + break; + case MAV_STATE_BOOT: + uasState = tr("BOOT"); + stateDescription = tr("Booting.."); + break; + case MAV_STATE_CALIBRATING: + uasState = tr("CALIBRATING"); + stateDescription = tr("Calibrating.."); + break; + case MAV_STATE_ACTIVE: + uasState = tr("ACTIVE"); + stateDescription = tr("Normal"); + break; + case MAV_STATE_STANDBY: + uasState = tr("STANDBY"); + stateDescription = tr("Standby, OK"); + break; + case MAV_STATE_CRITICAL: + uasState = tr("CRITICAL"); + stateDescription = tr("FAILURE: Continue"); + break; + case MAV_STATE_EMERGENCY: + uasState = tr("EMERGENCY"); + stateDescription = tr("EMERGENCY: Land!"); + break; + case MAV_STATE_POWEROFF: + uasState = tr("SHUTDOWN"); + stateDescription = tr("Powering off"); + break; + default: + uasState = tr("UNKNOWN"); + stateDescription = tr("Unknown state"); + break; + } +} + + + +/* MANAGEMENT */ + +/* + * + * @return The uptime in milliseconds + * + **/ +quint64 UAS::getUptime() const +{ + if(startTime == 0) { + return 0; + } else { + return MG::TIME::getGroundTimeNow() - startTime; + } +} + +int UAS::getCommunicationStatus() const +{ + return commStatus; +} + +void UAS::requestParameters() +{ + mavlink_message_t msg; + mavlink_msg_param_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 25); + // Send message twice to increase chance of reception + sendMessage(msg); +} + +void UAS::writeParametersToStorage() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_STORAGE_WRITE); + //mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(),(uint8_t)MAV_ACTION_STORAGE_WRITE); + sendMessage(msg); +} + +void UAS::readParametersFromStorage() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(uint8_t)MAV_ACTION_STORAGE_READ); + sendMessage(msg); +} + +void UAS::enableAllDataTransmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + // 0 is a magic ID and will enable/disable the standard message set except for heartbeat + stream.req_stream_id = MAV_DATA_STREAM_ALL; + // Select the update rate in Hz the message should be send + // All messages will be send with their default rate + // TODO: use 0 to turn off and get rid of enable/disable? will require + // a different magic flag for turning on defaults, possibly something really high like 1111 ? + stream.req_message_rate = 0; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enableRawSensorDataTransmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSORS; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enableExtendedSystemStatusTransmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_EXTENDED_STATUS; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enableRCChannelDataTransmission(int rate) +{ +#if defined(MAVLINK_ENABLED_UALBERTA_MESSAGES) + mavlink_message_t msg; + mavlink_msg_request_rc_channels_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, enabled); + sendMessage(msg); +#else + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_RC_CHANNELS; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +#endif +} + +void UAS::enableRawControllerDataTransmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_RAW_CONTROLLER; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enableRawSensorFusionTransmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_RAW_SENSOR_FUSION; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enablePositionTransmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_POSITION; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enableExtra1Transmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_EXTRA1; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enableExtra2Transmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_EXTRA2; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::enableExtra3Transmission(int rate) +{ + // Buffers to write data to + mavlink_message_t msg; + mavlink_request_data_stream_t stream; + // Select the message to request from now on + stream.req_stream_id = MAV_DATA_STREAM_EXTRA3; + // Select the update rate in Hz the message should be send + stream.req_message_rate = rate; + // Start / stop the message + stream.start_stop = (rate) ? 1 : 0; + // The system which should take this command + stream.target_system = uasId; + // The component / subsystem which should take this command + stream.target_component = 0; + // Encode and send the message + mavlink_msg_request_data_stream_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &stream); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +/** + * Set a parameter value onboard + * + * @param component The component to set the parameter + * @param id Name of the parameter + * @param value Parameter value + */ +void UAS::setParameter(const int component, const QString& id, const float value) +{ + if (!id.isNull()) + { + mavlink_message_t msg; + mavlink_param_set_t p; + p.param_value = value; + p.target_system = (uint8_t)uasId; + p.target_component = (uint8_t)component; + + // Copy string into buffer, ensuring not to exceed the buffer size + for (unsigned int i = 0; i < sizeof(p.param_id); i++) + { + // String characters + if ((int)i < id.length() && i < (sizeof(p.param_id) - 1)) + { + p.param_id[i] = id.toAscii()[i]; + } + // Null termination at end of string or end of buffer + else if ((int)i == id.length() || i == (sizeof(p.param_id) - 1)) + { + p.param_id[i] = '\0'; + } + // Zero fill + else + { + p.param_id[i] = 0; + } + } + mavlink_msg_param_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &p); + sendMessage(msg); + } +} + +void UAS::setUASName(const QString& name) +{ + this->name = name; + writeSettings(); + emit nameChanged(name); + emit systemSpecsChanged(uasId); +} + +/** + * Sets an action + * + **/ +void UAS::setAction(MAV_ACTION action) +{ + mavlink_message_t msg; + mavlink_msg_action_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, this->getUASID(), 0, action); + // Send message twice to increase chance that it reaches its goal + sendMessage(msg); + sendMessage(msg); +} + +/** + * Launches the system + * + **/ +void UAS::launch() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_LAUNCH); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +/** + * Depending on the UAS, this might make the rotors of a helicopter spinning + * + **/ +void UAS::enable_motors() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_START); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +/** + * @warning Depending on the UAS, this might completely stop all motors. + * + **/ +void UAS::disable_motors() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (uint8_t)MAV_ACTION_MOTORS_STOP); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::setManualControlCommands(double roll, double pitch, double yaw, double thrust) +{ + // Scale values + double rollPitchScaling = 0.2f; + double yawScaling = 0.5f; + double thrustScaling = 1.0f; + + manualRollAngle = roll * rollPitchScaling; + manualPitchAngle = pitch * rollPitchScaling; + manualYawAngle = yaw * yawScaling; + manualThrust = thrust * thrustScaling; + + if(mode == (int)MAV_MODE_MANUAL) + { + mavlink_message_t message; + mavlink_msg_manual_control_pack(mavlink->getSystemId(), mavlink->getComponentId(), &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()); + } + else + { + qDebug() << "JOYSTICK/MANUAL CONTROL: IGNORING COMMANDS: Set mode to MANUAL to send joystick commands first"; + } +} + +int UAS::getSystemType() +{ + return this->type; +} + +void UAS::receiveButton(int buttonIndex) +{ + switch (buttonIndex) + { + case 0: + + break; + case 1: + + break; + default: + + break; + } + // qDebug() << __FILE__ << __LINE__ << ": Received button clicked signal (button # is: " << buttonIndex << "), UNIMPLEMENTED IN MAVLINK!"; + +} + + +/*void UAS::requestWaypoints() +{ +// mavlink_message_t msg; +// mavlink_msg_waypoint_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 25); +// // Send message twice to increase chance of reception +// sendMessage(msg); + waypointManager.requestWaypoints(); + qDebug() << "UAS Request WPs"; +} + +void UAS::setWaypoint(Waypoint* wp) +{ +// mavlink_message_t msg; +// mavlink_waypoint_set_t set; +// set.id = wp->id; +// //QString name = wp->name; +// // FIXME Check if this works properly +// //name.truncate(MAVLINK_MSG_WAYPOINT_SET_FIELD_NAME_LEN); +// //strcpy((char*)set.name, name.toStdString().c_str()); +// set.autocontinue = wp->autocontinue; +// set.target_component = 25; // FIXME +// set.target_system = uasId; +// set.active = wp->current; +// set.x = wp->x; +// set.y = wp->y; +// set.z = wp->z; +// set.yaw = wp->yaw; +// mavlink_msg_waypoint_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &set); +// // Send message twice to increase chance of reception +// sendMessage(msg); +} + +void UAS::setWaypointActive(int id) +{ +// mavlink_message_t msg; +// mavlink_waypoint_set_active_t active; +// active.id = id; +// active.target_system = uasId; +// active.target_component = 25; // FIXME +// mavlink_msg_waypoint_set_active_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &active); +// // Send message twice to increase chance of reception +// sendMessage(msg); +// sendMessage(msg); +// // TODO This should be not directly emitted, but rather being fed back from the UAS +// emit waypointSelected(getUASID(), id); +} + +void UAS::clearWaypointList() +{ +// mavlink_message_t msg; +// // FIXME +// mavlink_waypoint_clear_list_t clist; +// clist.target_system = uasId; +// clist.target_component = 25; // FIXME +// mavlink_msg_waypoint_clear_list_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &clist); +// sendMessage(msg); +// qDebug() << "UAS clears Waypoints!"; +}*/ + + +void UAS::halt() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_HALT); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +void UAS::go() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_CONTINUE); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +/** Order the robot to return home / to land on the runway **/ +void UAS::home() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_RETURN); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +/** + * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation + * and might differ between systems. + */ +void UAS::emergencySTOP() +{ + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_LAND); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +/** + * All systems are immediately shut down (e.g. the main power line is cut). + * @warning This might lead to a crash + * + * The command will not be executed until emergencyKILLConfirm is issues immediately afterwards + */ +bool UAS::emergencyKILL() +{ + bool result = false; + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("EMERGENCY: KILL ALL MOTORS ON UAS"); + msgBox.setInformativeText("Do you want to cut power on all systems?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Cancel); + int ret = msgBox.exec(); + + // Close the message box shortly after the click to prevent accidental clicks + QTimer::singleShot(5000, &msgBox, SLOT(reject())); + + + if (ret == QMessageBox::Yes) + { + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU, (int)MAV_ACTION_EMCY_KILL); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); + result = true; + } + return result; +} + +void UAS::shutdown() +{ + bool result = false; + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("Shutting down the UAS"); + msgBox.setInformativeText("Do you want to shut down the onboard computer?"); + msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::Cancel); + msgBox.setDefaultButton(QMessageBox::Cancel); + int ret = msgBox.exec(); + + // Close the message box shortly after the click to prevent accidental clicks + QTimer::singleShot(5000, &msgBox, SLOT(reject())); + + + if (ret == QMessageBox::Yes) + { + // If the active UAS is set, execute command + mavlink_message_t msg; + // TODO Replace MG System ID with static function call and allow to change ID in GUI + mavlink_msg_action_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, this->getUASID(), MAV_COMP_ID_IMU,(int)MAV_ACTION_SHUTDOWN); + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); + result = true; + } +} + +void UAS::setTargetPosition(float x, float y, float z, float yaw) +{ + mavlink_message_t msg; + mavlink_msg_position_target_pack(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, x, y, z, yaw); + + // Send message twice to increase chance of reception + sendMessage(msg); + sendMessage(msg); +} + +/** + * @return The name of this system as string in human-readable form + */ +QString UAS::getUASName(void) const +{ + QString result; + if (name == "") + { + result = tr("MAV ") + result.sprintf("%03d", getUASID()); + } + else + { + result = name; + } + return result; +} + +void UAS::addLink(LinkInterface* link) +{ + if (!links->contains(link)) + { + links->append(link); + connect(link, SIGNAL(destroyed(QObject*)), this, SLOT(removeLink(QObject*))); + } + //links->append(link); + //qDebug() << link<<" ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK ADDED LINK"; +} + +void UAS::removeLink(QObject* object) +{ + LinkInterface* link = dynamic_cast(object); + if (link) + { + links->removeAt(links->indexOf(link)); + } +} + +/** + * @brief Get the links associated with this robot + * + **/ +QList* UAS::getLinks() +{ + return links; +} + + + +void UAS::setBattery(BatteryType type, int cells) +{ + this->batteryType = type; + this->cells = cells; + switch (batteryType) + { + case NICD: + break; + case NIMH: + break; + case LIION: + break; + case LIPOLY: + fullVoltage = this->cells * UAS::lipoFull; + emptyVoltage = this->cells * UAS::lipoEmpty; + break; + case LIFE: + break; + case AGZN: + break; + } +} + +int UAS::calculateTimeRemaining() +{ + quint64 dt = MG::TIME::getGroundTimeNow() - startTime; + double seconds = dt / 1000.0f; + double voltDifference = startVoltage - currentVoltage; + if (voltDifference <= 0) voltDifference = 0.00000000001f; + double dischargePerSecond = voltDifference / seconds; + int remaining = static_cast((currentVoltage - emptyVoltage) / dischargePerSecond); + // Can never be below 0 + if (remaining < 0) remaining = 0; + return remaining; +} + +/** + * @return charge level in percent - 0 - 100 + */ +double UAS::getChargeLevel() +{ + float chargeLevel; + if (lpVoltage < emptyVoltage) + { + chargeLevel = 0.0f; + } + else if (lpVoltage > fullVoltage) + { + chargeLevel = 100.0f; + } + else + { + chargeLevel = 100.0f * ((lpVoltage - emptyVoltage)/(fullVoltage - emptyVoltage)); + } + return chargeLevel; +} + +void UAS::startLowBattAlarm() +{ + if (!lowBattAlarm) + { + GAudioOutput::instance()->alert("LOW BATTERY"); + QTimer::singleShot(2000, GAudioOutput::instance(), SLOT(startEmergency())); + lowBattAlarm = true; + } +} + +void UAS::stopLowBattAlarm() +{ + if (lowBattAlarm) + { + GAudioOutput::instance()->stopEmergency(); + lowBattAlarm = false; + } +} diff --git a/src/uas/UASManager.cc b/src/uas/UASManager.cc index c59f50ada8e203b390bccbb0b597efca4d2f726b..7f026a5925c48d8f86014788aaf416404d86eb96 100644 --- a/src/uas/UASManager.cc +++ b/src/uas/UASManager.cc @@ -1,252 +1,252 @@ -/*===================================================================== - -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 - * @brief Implementation of class UASManager - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include "UAS.h" -#include "UASInterface.h" -#include "UASManager.h" -#include "QGC.h" - -UASManager* UASManager::instance() -{ - static UASManager* _instance = 0; - if(_instance == 0) { - _instance = new UASManager(); - - // Set the application as parent to ensure that this object - // will be destroyed when the main application exits - _instance->setParent(qApp); - } - return _instance; -} - -/** - * @brief Private singleton constructor - * - * This class implements the singleton design pattern and has therefore only a private constructor. - **/ -UASManager::UASManager() : - activeUAS(NULL) -{ - systems = QList(); - start(QThread::LowPriority); -} - -UASManager::~UASManager() -{ -} - - -void UASManager::run() -{ - forever - { - QGC::SLEEP::msleep(5000); - } -} - -void UASManager::addUAS(UASInterface* uas) -{ - // WARNING: The active uas is set here - // and then announced below. This is necessary - // to make sure the getActiveUAS() function - // returns the UAS once the UASCreated() signal - // is emitted. The code is thus NOT redundant. - bool firstUAS = false; - if (activeUAS == NULL) - { - firstUAS = true; - activeUAS = uas; - } - - // Only execute if there is no UAS at this index - if (!systems.contains(uas)) - { - systems.append(uas); - connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(removeUAS(QObject*))); - emit UASCreated(uas); - } - - // If there is no active UAS yet, set the first one as the active UAS - if (firstUAS) - { - setActiveUAS(uas); - } -} - -void UASManager::removeUAS(QObject* uas) -{ - UASInterface* mav = qobject_cast(uas); - - if (mav) - { - int listindex = systems.indexOf(mav); - - if (mav == activeUAS) - { - if (systems.count() > 1) - { - // We only set a new UAS if more than one is present - if (listindex != 0) - { - // The system to be removed is not at position 1 - // set position one as new active system - setActiveUAS(systems.first()); - } - else - { - // The system to be removed is at position 1, - // select the next system - setActiveUAS(systems.at(1)); - } - } - else - { - // TODO send a null pointer if no UAS is present any more - // This has to be proberly tested however, since it might - // crash code parts not handling null pointers correctly. - } - } - systems.removeAt(listindex); - } -} - -QList UASManager::getUASList() -{ - return systems; -} - -UASInterface* UASManager::getActiveUAS() -{ - return activeUAS; ///< Return zero pointer if no UAS has been loaded -} - -UASInterface* UASManager::silentGetActiveUAS() -{ - return activeUAS; ///< Return zero pointer if no UAS has been loaded -} - -bool UASManager::launchActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->launch(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::haltActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->halt(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::continueActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->go(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::returnActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->home(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::stopActiveUAS() -{ - // If the active UAS is set, execute command - if (getActiveUAS()) activeUAS->emergencySTOP(); - return (activeUAS); ///< Returns true if the UAS exists, false else -} - -bool UASManager::killActiveUAS() -{ - if (getActiveUAS()) activeUAS->emergencyKILL(); - return (activeUAS); -} - -bool UASManager::shutdownActiveUAS() -{ - if (getActiveUAS()) activeUAS->shutdown(); - return (activeUAS); -} - -void UASManager::configureActiveUAS() -{ - UASInterface* actUAS = getActiveUAS(); - if(actUAS) - { - // Do something - } -} - -UASInterface* UASManager::getUASForId(int id) -{ - UASInterface* system = NULL; - - foreach(UASInterface* sys, systems) - { - if (sys->getUASID() == id) - { - system = sys; - } - } - - // Return NULL if not found - return system; -} - -void UASManager::setActiveUAS(UASInterface* uas) -{ - if (uas != NULL) - { - activeUASMutex.lock(); - if (activeUAS != NULL) - { - emit activeUASStatusChanged(activeUAS, false); - emit activeUASStatusChanged(activeUAS->getUASID(), false); - } - activeUAS = uas; - activeUASMutex.unlock(); - - activeUAS->setSelected(); - emit activeUASSet(uas); - emit activeUASSet(uas->getUASID()); - emit activeUASSetListIndex(systems.indexOf(uas)); - emit activeUASStatusChanged(uas, true); - emit activeUASStatusChanged(uas->getUASID(), true); - } -} - +/*===================================================================== + +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 + * @brief Implementation of class UASManager + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include "UAS.h" +#include "UASInterface.h" +#include "UASManager.h" +#include "QGC.h" + +UASManager* UASManager::instance() +{ + static UASManager* _instance = 0; + if(_instance == 0) { + _instance = new UASManager(); + + // Set the application as parent to ensure that this object + // will be destroyed when the main application exits + _instance->setParent(qApp); + } + return _instance; +} + +/** + * @brief Private singleton constructor + * + * This class implements the singleton design pattern and has therefore only a private constructor. + **/ +UASManager::UASManager() : + activeUAS(NULL) +{ + systems = QList(); + start(QThread::LowPriority); +} + +UASManager::~UASManager() +{ +} + + +void UASManager::run() +{ + forever + { + QGC::SLEEP::msleep(5000); + } +} + +void UASManager::addUAS(UASInterface* uas) +{ + // WARNING: The active uas is set here + // and then announced below. This is necessary + // to make sure the getActiveUAS() function + // returns the UAS once the UASCreated() signal + // is emitted. The code is thus NOT redundant. + bool firstUAS = false; + if (activeUAS == NULL) + { + firstUAS = true; + activeUAS = uas; + } + + // Only execute if there is no UAS at this index + if (!systems.contains(uas)) + { + systems.append(uas); + connect(uas, SIGNAL(destroyed(QObject*)), this, SLOT(removeUAS(QObject*))); + emit UASCreated(uas); + } + + // If there is no active UAS yet, set the first one as the active UAS + if (firstUAS) + { + setActiveUAS(uas); + } +} + +void UASManager::removeUAS(QObject* uas) +{ + UASInterface* mav = qobject_cast(uas); + + if (mav) + { + int listindex = systems.indexOf(mav); + + if (mav == activeUAS) + { + if (systems.count() > 1) + { + // We only set a new UAS if more than one is present + if (listindex != 0) + { + // The system to be removed is not at position 1 + // set position one as new active system + setActiveUAS(systems.first()); + } + else + { + // The system to be removed is at position 1, + // select the next system + setActiveUAS(systems.at(1)); + } + } + else + { + // TODO send a null pointer if no UAS is present any more + // This has to be proberly tested however, since it might + // crash code parts not handling null pointers correctly. + } + } + systems.removeAt(listindex); + } +} + +QList UASManager::getUASList() +{ + return systems; +} + +UASInterface* UASManager::getActiveUAS() +{ + return activeUAS; ///< Return zero pointer if no UAS has been loaded +} + +UASInterface* UASManager::silentGetActiveUAS() +{ + return activeUAS; ///< Return zero pointer if no UAS has been loaded +} + +bool UASManager::launchActiveUAS() +{ + // If the active UAS is set, execute command + if (getActiveUAS()) activeUAS->launch(); + return (activeUAS); ///< Returns true if the UAS exists, false else +} + +bool UASManager::haltActiveUAS() +{ + // If the active UAS is set, execute command + if (getActiveUAS()) activeUAS->halt(); + return (activeUAS); ///< Returns true if the UAS exists, false else +} + +bool UASManager::continueActiveUAS() +{ + // If the active UAS is set, execute command + if (getActiveUAS()) activeUAS->go(); + return (activeUAS); ///< Returns true if the UAS exists, false else +} + +bool UASManager::returnActiveUAS() +{ + // If the active UAS is set, execute command + if (getActiveUAS()) activeUAS->home(); + return (activeUAS); ///< Returns true if the UAS exists, false else +} + +bool UASManager::stopActiveUAS() +{ + // If the active UAS is set, execute command + if (getActiveUAS()) activeUAS->emergencySTOP(); + return (activeUAS); ///< Returns true if the UAS exists, false else +} + +bool UASManager::killActiveUAS() +{ + if (getActiveUAS()) activeUAS->emergencyKILL(); + return (activeUAS); +} + +bool UASManager::shutdownActiveUAS() +{ + if (getActiveUAS()) activeUAS->shutdown(); + return (activeUAS); +} + +void UASManager::configureActiveUAS() +{ + UASInterface* actUAS = getActiveUAS(); + if(actUAS) + { + // Do something + } +} + +UASInterface* UASManager::getUASForId(int id) +{ + UASInterface* system = NULL; + + foreach(UASInterface* sys, systems) + { + if (sys->getUASID() == id) + { + system = sys; + } + } + + // Return NULL if not found + return system; +} + +void UASManager::setActiveUAS(UASInterface* uas) +{ + if (uas != NULL) + { + activeUASMutex.lock(); + if (activeUAS != NULL) + { + emit activeUASStatusChanged(activeUAS, false); + emit activeUASStatusChanged(activeUAS->getUASID(), false); + } + activeUAS = uas; + activeUASMutex.unlock(); + + activeUAS->setSelected(); + emit activeUASSet(uas); + emit activeUASSet(uas->getUASID()); + emit activeUASSetListIndex(systems.indexOf(uas)); + emit activeUASStatusChanged(uas, true); + emit activeUASStatusChanged(uas->getUASID(), true); + } +} + diff --git a/src/ui/MainWindow.cc b/src/ui/MainWindow.cc index ea551a5e85349624481ce3d3f11dc8b7336cf353..acffd27736b5fc86f5c716c28679b96130088b64 100644 --- a/src/ui/MainWindow.cc +++ b/src/ui/MainWindow.cc @@ -1,1909 +1,1910 @@ -/*=================================================================== -======================================================================*/ - -/** - * @file - * @brief Implementation of class MainWindow - * @author Lorenz Meier - */ - -#include -#include -#include -#include -#include -#include -#include - -#include "MG.h" -#include "QGC.h" -#include "MAVLinkSimulationLink.h" -#include "SerialLink.h" -#include "UDPLink.h" -#include "MAVLinkProtocol.h" -#include "CommConfigurationWindow.h" -#include "QGCWaypointListMulti.h" -#include "MainWindow.h" -#include "JoystickWidget.h" -#include "GAudioOutput.h" -#include "QGCToolWidget.h" -#include "QGCMAVLinkLogPlayer.h" - -#ifdef QGC_OSG_ENABLED -#include "Q3DWidgetFactory.h" -#endif - -// FIXME Move -#include "PxQuadMAV.h" -#include "SlugsMAV.h" - - -#include "LogCompressor.h" - -MainWindow* MainWindow::instance() -{ - static MainWindow* _instance = 0; - if(_instance == 0) - { - _instance = new MainWindow(); - - /* Set the application as parent to ensure that this object - * will be destroyed when the main application exits */ - //_instance->setParent(qApp); - } - return _instance; -} - -/** -* Create new mainwindow. The constructor instantiates all parts of the user -* interface. It does NOT show the mainwindow. To display it, call the show() -* method. -* -* @see QMainWindow::show() -**/ -MainWindow::MainWindow(QWidget *parent): - QMainWindow(parent), - toolsMenuActions(), - currentView(VIEW_UNCONNECTED), - aboutToCloseFlag(false), - changingViewsFlag(false) -{ - if (!settings.contains("CURRENT_VIEW")) - { - // Set this view as default view - settings.setValue("CURRENT_VIEW", currentView); - } - else - { - // LOAD THE LAST VIEW - VIEW_SECTIONS currentViewCandidate = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt(); - if (currentViewCandidate != VIEW_ENGINEER && - currentViewCandidate != VIEW_OPERATOR && - currentViewCandidate != VIEW_PILOT) - { - currentView = currentViewCandidate; - } - } - - setDefaultSettingsForAp(); - - settings.sync(); - - // Setup user interface - ui.setupUi(this); - - setVisible(false); - - buildCommonWidgets(); - - connectCommonWidgets(); - - arrangeCommonCenterStack(); - - configureWindowName(); - - // Set the application style (not the same as a style sheet) - // Set the style to Plastique - qApp->setStyle("plastique"); - - // Set style sheet as last step - QFile* styleSheet = new QFile(":/images/style-mission.css"); - if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) - { - QString style = QString(styleSheet->readAll()); - style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); - qApp->setStyleSheet(style); - } - - // Create actions - connectCommonActions(); - - // Set dock options - setDockOptions(AnimatedDocks | AllowTabbedDocks | AllowNestedDocks); - - // Load mavlink view as default widget set - //loadMAVLinkView(); - - statusBar()->setSizeGripEnabled(true); - - // Restore the window position and size - if (settings.contains(getWindowGeometryKey())) - { - // Restore the window geometry - restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray()); - } - else - { - // Adjust the size - adjustSize(); - } - - // Populate link menu - QList links = LinkManager::instance()->getLinks(); - foreach(LinkInterface* link, links) - { - this->addLink(link); - } - - connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*))); - - // Connect user interface devices - if (!joystick) - { - joystick = new JoystickInput(); - } - - // Enable and update view - presentView(); -} - -MainWindow::~MainWindow() -{ - -} - -/** - * Set default settings for this AP type. - */ -void MainWindow::setDefaultSettingsForAp() -{ - // Check if the settings exist, instantiate defaults if necessary - - // UNCONNECTED VIEW DEFAULT - QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_UNCONNECTED); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - - // ENABLE UAS LIST - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST, VIEW_UNCONNECTED), true); - // ENABLE COMMUNICATION CONSOLE - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE, VIEW_UNCONNECTED), true); - } - - // OPERATOR VIEW DEFAULT - centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - - // ENABLE UAS LIST - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,VIEW_OPERATOR), true); - // ENABLE HUD TOOL WIDGET - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HUD,VIEW_OPERATOR), true); - } - - // ENGINEER VIEW DEFAULT - centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - // Enable Parameter widget - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_PARAMETERS,VIEW_ENGINEER), true); - } - - // MAVLINK VIEW DEFAULT - centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - } - - // PILOT VIEW DEFAULT - centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT); - if (!settings.contains(centralKey)) - { - settings.setValue(centralKey,true); - // Enable Flight display - settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HDD_1,VIEW_PILOT), true); - } -} - -void MainWindow::resizeEvent(QResizeEvent * event) -{ - Q_UNUSED(event); - if (height() < 800) - { - ui.statusBar->setVisible(false); - } - else - { - ui.statusBar->setVisible(true); - } -} - -QString MainWindow::getWindowStateKey() -{ - return QString::number(currentView)+"/windowstate"; -} - -QString MainWindow::getWindowGeometryKey() -{ - return QString::number(currentView)+"/geometry"; -} - -void MainWindow::buildCustomWidget() -{ - // Show custom widgets only if UAS is connected - if (UASManager::instance()->getActiveUAS() != NULL) - { - // Enable custom widgets - ui.actionNewCustomWidget->setEnabled(true); - - // Create custom widgets - QList widgets = QGCToolWidget::createWidgetsFromSettings(this); - - if (widgets.size() > 0) - { - ui.menuTools->addSeparator(); - } - - for(int i = 0; i < widgets.size(); ++i) - { - qDebug() << "ADDING WIDGET #" << i << widgets.at(i); - // Check if this widget already has a parent, do not create it in this case - QDockWidget* dock = dynamic_cast(widgets.at(i)->parentWidget()); - if (!dock) - { - QDockWidget* dock = new QDockWidget(widgets.at(i)->windowTitle(), this); - dock->setWidget(widgets.at(i)); - connect(widgets.at(i), SIGNAL(destroyed()), dock, SLOT(deleteLater())); - QAction* showAction = new QAction(widgets.at(i)->windowTitle(), this); - connect(showAction, SIGNAL(triggered(bool)), dock, SLOT(setVisible(bool))); - connect(dock, SIGNAL(visibilityChanged(bool)), showAction, SLOT(setChecked(bool))); - widgets.at(i)->setMainMenuAction(showAction); - ui.menuTools->addAction(showAction); - addDockWidget(Qt::BottomDockWidgetArea, dock); - } - } - } -} - -void MainWindow::buildCommonWidgets() -{ - //TODO: move protocol outside UI - mavlink = new MAVLinkProtocol(); - connect(mavlink, SIGNAL(protocolStatusMessage(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection); - - // Dock widgets - if (!controlDockWidget) - { - controlDockWidget = new QDockWidget(tr("Control"), this); - controlDockWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_DOCKWIDGET"); - controlDockWidget->setWidget( new UASControlWidget(this) ); - addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); - } - - if (!listDockWidget) - { - listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); - listDockWidget->setWidget( new UASListWidget(this) ); - listDockWidget->setObjectName("UNMANNED_SYSTEMS_LIST_DOCKWIDGET"); - addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget(bool)), MENU_UAS_LIST, Qt::RightDockWidgetArea); - } - - if (!waypointsDockWidget) - { - waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); - waypointsDockWidget->setWidget( new QGCWaypointListMulti(this) ); - waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET"); - addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); - } - - if (!infoDockWidget) - { - infoDockWidget = new QDockWidget(tr("Status Details"), this); - infoDockWidget->setWidget( new UASInfoWidget(this) ); - infoDockWidget->setObjectName("UAS_STATUS_DETAILS_DOCKWIDGET"); - addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget(bool)), MENU_STATUS, Qt::RightDockWidgetArea); - } - - if (!debugConsoleDockWidget) - { - debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); - debugConsoleDockWidget->setWidget( new DebugConsole(this) ); - debugConsoleDockWidget->setObjectName("COMMUNICATION_DEBUG_CONSOLE_DOCKWIDGET"); - addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget(bool)), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); - } - - if (!logPlayerDockWidget) - { - logPlayerDockWidget = new QDockWidget(tr("MAVLink Log Player"), this); - logPlayerDockWidget->setWidget( new QGCMAVLinkLogPlayer(mavlink, this) ); - logPlayerDockWidget->setObjectName("MAVLINK_LOG_PLAYER_DOCKWIDGET"); - addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget(bool)), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea); - } - - // Center widgets - if (!mapWidget) - { - mapWidget = new MapWidget(this); - addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); - } - - if (!protocolWidget) - { - protocolWidget = new XMLCommProtocolWidget(this); - addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); - } - - if (!dataplotWidget) - { - dataplotWidget = new QGCDataPlot2D(this); - addToCentralWidgetsMenu (dataplotWidget, "Data Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT); - } -} - - -void MainWindow::buildPxWidgets() -{ - //FIXME: memory of acceptList will never be freed again - QStringList* acceptList = new QStringList(); - acceptList->append("-105,roll,deg,+105,s"); - acceptList->append("-105,pitch,deg,+105,s"); - acceptList->append("-105,yaw,deg,+105,s"); - - acceptList->append("-260,rollspeed,deg/s,+260,s"); - acceptList->append("-260,pitchspeed,deg/s,+260,s"); - acceptList->append("-260,yawspeed,deg/s,+260,s"); - - //FIXME: memory of acceptList2 will never be freed again - QStringList* acceptList2 = new QStringList(); - acceptList2->append("0,abs pressure,hPa,65500"); - acceptList2->append("-999,accel. X,raw,999,s"); - acceptList2->append("-999,accel. Y,raw,999,s"); - - if (!linechartWidget) - { - // Center widgets - linechartWidget = new Linecharts(this); - addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); - } - - - if (!hudWidget) - { - hudWidget = new HUD(320, 240, this); - addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); - } - - if (!dataplotWidget) - { - dataplotWidget = new QGCDataPlot2D(this); - addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); - } - -#ifdef QGC_OSG_ENABLED - if (!_3DWidget) - { - _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); - addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); - } -#endif - -#ifdef QGC_OSGEARTH_ENABLED - if (!_3DMapWidget) - { - _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); - addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); - } -#endif - -#if (defined _MSC_VER) | (defined Q_OS_MAC) - if (!gEarthWidget) - { - gEarthWidget = new QGCGoogleEarthView(this); - addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); - } - -#endif - - // Dock widgets - - if (!detectionDockWidget) - { - detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); - detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); - detectionDockWidget->setObjectName("OBJECT_DETECTION_DOCK_WIDGET"); - addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget(bool)), MENU_DETECTION, Qt::RightDockWidgetArea); - } - - if (!parametersDockWidget) - { - parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this); - parametersDockWidget->setWidget( new ParameterInterface(this) ); - parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET"); - addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea); - } - - if (!watchdogControlDockWidget) - { - watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); - watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); - watchdogControlDockWidget->setObjectName("WATCHDOG_CONTROL_DOCKWIDGET"); - addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget(bool)), MENU_WATCHDOG, Qt::BottomDockWidgetArea); - } - - if (!hsiDockWidget) - { - hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); - hsiDockWidget->setWidget( new HSIDisplay(this) ); - hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET"); - addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea); - } - - if (!headDown1DockWidget) - { - headDown1DockWidget = new QDockWidget(tr("Flight Display"), this); - headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) ); - headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET"); - addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget(bool)), MENU_HDD_1, Qt::RightDockWidgetArea); - } - - if (!headDown2DockWidget) - { - headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); - headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) ); - headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET"); - addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea); - } - - if (!rcViewDockWidget) - { - rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); - rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); - rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); - addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea); - } - - if (!headUpDockWidget) - { - headUpDockWidget = new QDockWidget(tr("HUD"), this); - headUpDockWidget->setWidget( new HUD(320, 240, this)); - headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); - addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea); - } - - if (!video1DockWidget) - { - video1DockWidget = new QDockWidget(tr("Video Stream 1"), this); - HUD* video1 = new HUD(160, 120, this); - video1->enableHUDInstruments(false); - video1->enableVideo(true); - // FIXME select video stream as well - video1DockWidget->setWidget(video1); - video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET"); - addToToolsMenu (video1DockWidget, tr("Video Stream 1"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_1, Qt::LeftDockWidgetArea); - } - - if (!video2DockWidget) - { - video2DockWidget = new QDockWidget(tr("Video Stream 2"), this); - HUD* video2 = new HUD(160, 120, this); - video2->enableHUDInstruments(false); - video2->enableVideo(true); - // FIXME select video stream as well - video2DockWidget->setWidget(video2); - video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET"); - addToToolsMenu (video2DockWidget, tr("Video Stream 2"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_2, Qt::LeftDockWidgetArea); - } - - // Dialogue widgets - //FIXME: free memory in destructor -} - -void MainWindow::buildSlugsWidgets() -{ - if (!linechartWidget) - { - // Center widgets - linechartWidget = new Linecharts(this); - } - - if (!headUpDockWidget) - { - // Dock widgets - headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); - headUpDockWidget->setWidget( new HUD(320, 240, this)); - headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); - addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea); - } - - if (!rcViewDockWidget) - { - rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); - rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); - rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); - addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea); - } - -// if (!slugsDataWidget) -// { -// // Dialog widgets -// slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); -// slugsDataWidget->setWidget( new SlugsDataSensorView(this)); -// slugsDataWidget->setObjectName("SLUGS_DATA_DOCK_WIDGET"); -// addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget()), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); -// } - -// if (!slugsPIDControlWidget) -// { -// slugsPIDControlWidget = new QDockWidget(tr("Slugs PID Control"), this); -// slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); -// slugsPIDControlWidget->setObjectName("SLUGS_PID_CONTROL_DOCK_WIDGET"); -// addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget()), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); -// } - -// if (!slugsHilSimWidget) -// { -// slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); -// slugsHilSimWidget->setWidget( new SlugsHilSim(this)); -// slugsHilSimWidget->setObjectName("SLUGS_HIL_SIM_DOCK_WIDGET"); -// addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); -// } - -// if (!slugsCamControlWidget) -// { -// slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); -// slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); -// slugsCamControlWidget->setObjectName("SLUGS_CAM_CONTROL_DOCK_WIDGET"); -// addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); -// } -} - - -void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, - const QString title, - const char * slotName, - TOOLS_WIDGET_NAMES centralWidget) -{ - QAction* tempAction; - - -// Not needed any more - separate menu now available - -// // Add the separator that will separate tools from central Widgets -// if (!toolsMenuActions[CENTRAL_SEPARATOR]) -// { -// tempAction = ui.menuTools->addSeparator(); -// toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; -// tempAction->setData(CENTRAL_SEPARATOR); -// } - - tempAction = ui.menuMain->addAction(title); - - tempAction->setCheckable(true); - tempAction->setData(centralWidget); - - // populate the Hashes - toolsMenuActions[centralWidget] = tempAction; - dockWidgets[centralWidget] = widget; - - QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView); - - if (!settings.contains(chKey)) - { - settings.setValue(chKey,false); - tempAction->setChecked(false); - } - else - { - tempAction->setChecked(settings.value(chKey).toBool()); - } - - // connect the action - connect(tempAction,SIGNAL(triggered(bool)),this, slotName); -} - - -void MainWindow::showCentralWidget() -{ - QAction* senderAction = qobject_cast(sender()); - - // Block sender action while manipulating state - senderAction->blockSignals(true); - - int tool = senderAction->data().toInt(); - QString chKey; - - // check the current action - - if (senderAction && dockWidgets[tool]) - { - // uncheck all central widget actions - QHashIterator i(toolsMenuActions); - while (i.hasNext()) - { - i.next(); - //qDebug() << "shCW" << i.key() << "read"; - if (i.value() && i.value()->data().toInt() > 255) - { - // Block signals and uncheck action - // firing would be unneccesary - i.value()->blockSignals(true); - i.value()->setChecked(false); - i.value()->blockSignals(false); - - // update the settings - chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView); - settings.setValue(chKey,false); - } - } - - // check the current action - //qDebug() << senderAction->text(); - senderAction->setChecked(true); - - // update the central widget - centerStack->setCurrentWidget(dockWidgets[tool]); - - // store the selected central widget - chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); - settings.setValue(chKey,true); - - // Unblock sender action - senderAction->blockSignals(false); - - presentView(); - } -} - -/** - * Adds a widget to the tools menu and sets it visible if it was - * enabled last time. - */ -void MainWindow::addToToolsMenu ( QWidget* widget, - const QString title, - const char * slotName, - TOOLS_WIDGET_NAMES tool, - Qt::DockWidgetArea location) -{ - QAction* tempAction; - QString posKey, chKey; - - - if (toolsMenuActions[CENTRAL_SEPARATOR]) - { - tempAction = new QAction(title, this); - ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], - tempAction); - } - else - { - tempAction = ui.menuTools->addAction(title); - } - - tempAction->setCheckable(true); - tempAction->setData(tool); - - // populate the Hashes - toolsMenuActions[tool] = tempAction; - dockWidgets[tool] = widget; - //qDebug() << widget; - - posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); - - if (!settings.contains(posKey)) - { - settings.setValue(posKey,location); - dockWidgetLocations[tool] = location; - } - else - { - dockWidgetLocations[tool] = static_cast (settings.value(posKey, Qt::RightDockWidgetArea).toInt()); - } - - chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView); - - if (!settings.contains(chKey)) - { - settings.setValue(chKey,false); - tempAction->setChecked(false); - widget->setVisible(false); - } - else - { - tempAction->setChecked(settings.value(chKey, false).toBool()); - widget->setVisible(settings.value(chKey, false).toBool()); - } - - // connect the action - connect(tempAction,SIGNAL(toggled(bool)),this, slotName); - - connect(qobject_cast (dockWidgets[tool]), - SIGNAL(visibilityChanged(bool)), this, SLOT(showToolWidget(bool))); - - // connect(qobject_cast (dockWidgets[tool]), - // SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); - - connect(qobject_cast (dockWidgets[tool]), - SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); -} - -void MainWindow::showToolWidget(bool visible) -{ - if (!aboutToCloseFlag && !changingViewsFlag) - { - QAction* action = qobject_cast(sender()); - - // Prevent this to fire if undocked - if (action) - { - int tool = action->data().toInt(); - - QDockWidget* dockWidget = qobject_cast (dockWidgets[tool]); - - qDebug() << "DATA:" << tool << "FLOATING" << dockWidget->isFloating() << "checked" << action->isChecked() << "visible" << dockWidget->isVisible() << "action vis:" << action->isVisible(); - if (dockWidget && dockWidget->isVisible() != visible) - { - if (visible) - { - qDebug() << "DOCK WIDGET ADDED"; - addDockWidget(dockWidgetLocations[tool], dockWidget); - dockWidget->show(); - } - else - { - qDebug() << "DOCK WIDGET REMOVED"; - removeDockWidget(dockWidget); - //dockWidget->hide(); - } - - QHashIterator i(dockWidgets); - while (i.hasNext()) - { - i.next(); - if ((static_cast (dockWidgets[i.key()])) == dockWidget) - { - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); - settings.setValue(chKey,visible); - qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; - break; - } - } - } - } - - QDockWidget* dockWidget = qobject_cast(QObject::sender()); - - qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible; - - if (dockWidget) - { - // Get action - int tool = dockWidgets.key(dockWidget); - - qDebug() << "Updating widget setting" << tool << "to" << visible; - - QAction* action = toolsMenuActions[tool]; - action->blockSignals(true); - action->setChecked(visible); - action->blockSignals(false); - - QHashIterator i(dockWidgets); - while (i.hasNext()) - { - i.next(); - if ((static_cast (dockWidgets[i.key()])) == dockWidget) - { - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); - settings.setValue(chKey,visible); - qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; - break; - } - } - } - } -} - - -void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) -{ - bool tempVisible; - Qt::DockWidgetArea tempLocation; - QDockWidget* tempWidget = static_cast (dockWidgets[widget]); - - tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool(); - - qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible; - - if (tempWidget) - { - toolsMenuActions[widget]->setChecked(tempVisible); - } - - - //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; - - tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view), QVariant(Qt::RightDockWidgetArea)).toInt()); - - // if (widget == MainWindow::MENU_UAS_LIST) - // { - // if (!settings.contains(buildMenuKey (SUB_SECTION_LOCATION,widget, view))) - // { - // tempLocation = Qt::RightDockWidgetArea; - // } - // } - - if (tempWidget != NULL) - { - if (tempVisible) - { - addDockWidget(tempLocation, tempWidget); - tempWidget->show(); - } - } -} - -QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view) -{ - // Key is built as follows: autopilot_type/section_menu/view/tool/section - int apType; - -// apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? -// UASManager::instance()->getActiveUAS()->getAutopilotType(): -// -1; - - apType = 1; - - return (QString::number(apType) + "_" + - QString::number(SECTION_MENU) + "_" + - QString::number(view) + "_" + - QString::number(tool) + "_" + - QString::number(section) + "_" ); -} - -void MainWindow::closeEvent(QCloseEvent *event) -{ - settings.setValue(getWindowGeometryKey(), saveGeometry()); - //settings.setValue("windowState", saveState()); - aboutToCloseFlag = true; - // Save the last current view in any case - settings.setValue("CURRENT_VIEW", currentView); - // Save the current window state, but only if a system is connected (else no real number of widgets would be present) - if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); - // Save the current view only if a UAS is connected - if (UASManager::instance()->getUASList().length() > 0) settings.setValue("CURRENT_VIEW_WITH_UAS_CONNECTED", currentView); - settings.sync(); - QMainWindow::closeEvent(event); -} - -void MainWindow::showDockWidget (bool vis) -{ - if (!aboutToCloseFlag && !changingViewsFlag) - { - QDockWidget* temp = qobject_cast(sender()); - - if (temp) - { - QHashIterator i(dockWidgets); - while (i.hasNext()) - { - i.next(); - if ((static_cast (dockWidgets[i.key()])) == temp) - { - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); - settings.setValue(chKey,vis); - toolsMenuActions[i.key()]->setChecked(vis); - break; - } - } - } - } -} - -void MainWindow::updateVisibilitySettings (bool vis) -{ - if (!aboutToCloseFlag && !changingViewsFlag) - { - QDockWidget* temp = qobject_cast(sender()); - - if (temp) - { - QHashIterator i(dockWidgets); - while (i.hasNext()) - { - i.next(); - if ((static_cast (dockWidgets[i.key()])) == temp) - { - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); - settings.setValue(chKey,vis); - toolsMenuActions[i.key()]->setChecked(vis); - break; - } - } - } - } -} - -void MainWindow::updateLocationSettings (Qt::DockWidgetArea location) -{ - QDockWidget* temp = qobject_cast(sender()); - - QHashIterator i(dockWidgets); - while (i.hasNext()) - { - i.next(); - if ((static_cast (dockWidgets[i.key()])) == temp) - { - QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key()), currentView); - settings.setValue(posKey,location); - break; - } - } -} - - -/** - * Connect the signals and slots of the common window widgets - */ -void MainWindow::connectCommonWidgets() -{ - if (infoDockWidget && infoDockWidget->widget()) - { - connect(mavlink, SIGNAL(receiveLossChanged(int, float)), - infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); - } - - if (mapWidget && waypointsDockWidget->widget()) - { - - // - connect(waypointsDockWidget->widget(), SIGNAL(changePointList()), mapWidget, SLOT(clearWaypoints())); - - - - - } - - //TODO temporaly debug - if (slugsHilSimWidget && slugsHilSimWidget->widget()){ - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); - } -} - -void MainWindow::createCustomWidget() -{ - //qDebug() << "ADDING CUSTOM WIDGET"; - QGCToolWidget* tool = new QGCToolWidget("Unnamed Tool", this); - - if (QGCToolWidget::instances()->size() < 2) - { - // This is the first widget - ui.menuTools->addSeparator(); - } - - QDockWidget* dock = new QDockWidget("Unnamed Tool", this); - connect(tool, SIGNAL(destroyed()), dock, SLOT(deleteLater())); - dock->setWidget(tool); - QAction* showAction = new QAction("Show Unnamed Tool", this); - connect(dock, SIGNAL(visibilityChanged(bool)), showAction, SLOT(setChecked(bool))); - connect(showAction, SIGNAL(triggered(bool)), dock, SLOT(setVisible(bool))); - tool->setMainMenuAction(showAction); - ui.menuTools->addAction(showAction); - this->addDockWidget(Qt::BottomDockWidgetArea, dock); - dock->setVisible(true); -} - -void MainWindow::connectPxWidgets() -{ - // No special connections necessary at this point -} - -void MainWindow::connectSlugsWidgets() -{ - if (slugsHilSimWidget && slugsHilSimWidget->widget()){ - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); - } - - if (slugsDataWidget && slugsDataWidget->widget()){ - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), - slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); - } - - -} - -void MainWindow::arrangeCommonCenterStack() -{ - centerStack = new QStackedWidget(this); - centerStack->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); - - if (!centerStack) return; - - if (mapWidget && (centerStack->indexOf(mapWidget) == -1)) centerStack->addWidget(mapWidget); - if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget); - if (protocolWidget && (centerStack->indexOf(protocolWidget) == -1)) centerStack->addWidget(protocolWidget); - - setCentralWidget(centerStack); -} - -void MainWindow::arrangePxCenterStack() -{ - - if (!centerStack) { - qDebug() << "Center Stack not Created!"; - return; - } - - - if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); - -#ifdef QGC_OSG_ENABLED - if (_3DWidget && (centerStack->indexOf(_3DWidget) == -1)) centerStack->addWidget(_3DWidget); -#endif -#ifdef QGC_OSGEARTH_ENABLED - if (_3DMapWidget && (centerStack->indexOf(_3DMapWidget) == -1)) centerStack->addWidget(_3DMapWidget); -#endif -#if (defined _MSC_VER) | (defined Q_OS_MAC) - if (gEarthWidget && (centerStack->indexOf(gEarthWidget) == -1)) centerStack->addWidget(gEarthWidget); -#endif - if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); - if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget); -} - -void MainWindow::arrangeSlugsCenterStack() -{ - - if (!centerStack) { - qDebug() << "Center Stack not Created!"; - return; - } - - if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); - if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); - -} - -void MainWindow::configureWindowName() -{ - QList hostAddresses = QNetworkInterface::allAddresses(); - QString windowname = qApp->applicationName() + " " + qApp->applicationVersion(); - bool prevAddr = false; - - windowname.append(" (" + QHostInfo::localHostName() + ": "); - - for (int i = 0; i < hostAddresses.size(); i++) - { - // Exclude loopback IPv4 and all IPv6 addresses - if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":")) - { - if(prevAddr) windowname.append("/"); - windowname.append(hostAddresses.at(i).toString()); - prevAddr = true; - } - } - - windowname.append(")"); - - setWindowTitle(windowname); - -#ifndef Q_WS_MAC - //qApp->setWindowIcon(QIcon(":/core/images/qtcreator_logo_128.png")); -#endif -} - -void MainWindow::startVideoCapture() -{ - QString format = "bmp"; - QString initialPath = QDir::currentPath() + tr("/untitled.") + format; - - QString screenFileName = QFileDialog::getSaveFileName(this, tr("Save As"), - initialPath, - tr("%1 Files (*.%2);;All Files (*)") - .arg(format.toUpper()) - .arg(format)); - delete videoTimer; - videoTimer = new QTimer(this); - //videoTimer->setInterval(40); - //connect(videoTimer, SIGNAL(timeout()), this, SLOT(saveScreen())); - //videoTimer->stop(); -} - -void MainWindow::stopVideoCapture() -{ - videoTimer->stop(); - - // TODO Convert raw images to PNG -} - -void MainWindow::saveScreen() -{ - QPixmap window = QPixmap::grabWindow(this->winId()); - QString format = "bmp"; - - if (!screenFileName.isEmpty()) - { - window.save(screenFileName, format.toAscii()); - } -} - -/** - * Reload the style sheet from disk. The function tries to load "qgroundcontrol.css" from the application - * directory (which by default does not exist). If it fails, it will load the bundled default CSS - * from memory. - * To customize the application, just create a qgroundcontrol.css file in the application directory - */ -void MainWindow::reloadStylesheet() -{ - QString fileName = QCoreApplication::applicationDirPath() + "/style-indoor.css"; - - // Let user select style sheet - fileName = QFileDialog::getOpenFileName(this, tr("Specify stylesheet"), fileName, tr("CSS Stylesheet (*.css);;")); - - if (!fileName.endsWith(".css")) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Information); - msgBox.setText(tr("QGroundControl did lot load a new style")); - msgBox.setInformativeText(tr("No suitable .css file selected. Please select a valid .css file.")); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - return; - } - - // Load style sheet - QFile* styleSheet = new QFile(fileName); - if (!styleSheet->exists()) - { - styleSheet = new QFile(":/images/style-mission.css"); - } - if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) - { - QString style = QString(styleSheet->readAll()); - style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); - qApp->setStyleSheet(style); - } - else - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Information); - msgBox.setText(tr("QGroundControl did lot load a new style")); - msgBox.setInformativeText(tr("Stylesheet file %1 was not readable").arg(fileName)); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - } - delete styleSheet; -} - -/** - * The status message will be overwritten if a new message is posted to this function - * - * @param status message text - * @param timeout how long the status should be displayed - */ -void MainWindow::showStatusMessage(const QString& status, int timeout) -{ - statusBar()->showMessage(status, timeout); -} - -/** - * The status message will be overwritten if a new message is posted to this function. - * it will be automatically hidden after 5 seconds. - * - * @param status message text - */ -void MainWindow::showStatusMessage(const QString& status) -{ - statusBar()->showMessage(status, 20000); -} - -void MainWindow::showCriticalMessage(const QString& title, const QString& message) -{ - QMessageBox msgBox(this); - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText(title); - msgBox.setInformativeText(message); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); -} - -void MainWindow::showInfoMessage(const QString& title, const QString& message) -{ - QMessageBox msgBox(this); - msgBox.setIcon(QMessageBox::Information); - msgBox.setText(title); - msgBox.setInformativeText(message); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); -} - -/** -* @brief Create all actions associated to the main window -* -**/ -void MainWindow::connectCommonActions() -{ - ui.actionNewCustomWidget->setEnabled(false); - - // Bind together the perspective actions - QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives); - perspectives->addAction(ui.actionEngineersView); - perspectives->addAction(ui.actionMavlinkView); - perspectives->addAction(ui.actionPilotsView); - perspectives->addAction(ui.actionOperatorsView); - perspectives->addAction(ui.actionUnconnectedView); - perspectives->setExclusive(true); - - // Mark the right one as selected - if (currentView == VIEW_ENGINEER) ui.actionEngineersView->setChecked(true); - if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true); - if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true); - if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true); - if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true); - - // The pilot, engineer and operator view are not available on startup - // since they only make sense with a system connected. - ui.actionPilotsView->setEnabled(false); - ui.actionOperatorsView->setEnabled(false); - ui.actionEngineersView->setEnabled(false); - // The UAS actions are not enabled without connection to system - ui.actionLiftoff->setEnabled(false); - ui.actionLand->setEnabled(false); - ui.actionEmergency_Kill->setEnabled(false); - ui.actionEmergency_Land->setEnabled(false); - ui.actionShutdownMAV->setEnabled(false); - - // Connect actions from ui - connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); - - // Connect internal actions - connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(UASCreated(UASInterface*))); - connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); - - // Unmanned System controls - connect(ui.actionLiftoff, SIGNAL(triggered()), UASManager::instance(), SLOT(launchActiveUAS())); - connect(ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS())); - connect(ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS())); - connect(ui.actionEmergency_Kill, SIGNAL(triggered()), UASManager::instance(), SLOT(killActiveUAS())); - connect(ui.actionShutdownMAV, SIGNAL(triggered()), UASManager::instance(), SLOT(shutdownActiveUAS())); - connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); - - // Views actions - connect(ui.actionPilotsView, SIGNAL(triggered()), this, SLOT(loadPilotView())); - connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); - connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); - connect(ui.actionUnconnectedView, SIGNAL(triggered()), this, SLOT(loadUnconnectedView())); - - connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); - connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); - - // Help Actions - connect(ui.actionOnline_Documentation, SIGNAL(triggered()), this, SLOT(showHelp())); - connect(ui.actionDeveloper_Credits, SIGNAL(triggered()), this, SLOT(showCredits())); - connect(ui.actionProject_Roadmap_2, SIGNAL(triggered()), this, SLOT(showRoadMap())); - - // Custom widget actions - connect(ui.actionNewCustomWidget, SIGNAL(triggered()), this, SLOT(createCustomWidget())); - - // Audio output - ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted()); - connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool))); - - // User interaction - // NOTE: Joystick thread is not started and - // configuration widget is not instantiated - // unless it is actually used - // so no ressources spend on this. - ui.actionJoystickSettings->setVisible(true); - // Joystick configuration - connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); -} - -void MainWindow::connectPxActions() -{ - - -} - -void MainWindow::connectSlugsActions() -{ - -} - -void MainWindow::showHelp() -{ - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/users/"))) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("Could not open help in browser"); - msgBox.setInformativeText("To get to the online help, please open http://qgroundcontrol.org/user_guide in a browser."); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - } -} - -void MainWindow::showCredits() -{ - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits/"))) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("Could not open credits in browser"); - msgBox.setInformativeText("To get to the online help, please open http://qgroundcontrol.org/credits in a browser."); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - } -} - -void MainWindow::showRoadMap() -{ - if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/roadmap/"))) - { - QMessageBox msgBox; - msgBox.setIcon(QMessageBox::Critical); - msgBox.setText("Could not open roadmap in browser"); - msgBox.setInformativeText("To get to the online help, please open http://qgroundcontrol.org/roadmap in a browser."); - msgBox.setStandardButtons(QMessageBox::Ok); - msgBox.setDefaultButton(QMessageBox::Ok); - msgBox.exec(); - } -} - -void MainWindow::configure() -{ - if (!joystickWidget) - { - if (!joystick->isRunning()) - { - joystick->start(); - } - joystickWidget = new JoystickWidget(joystick); - } - joystickWidget->show(); -} - -void MainWindow::addLink() -{ - SerialLink* link = new SerialLink(); - // TODO This should be only done in the dialog itself - - LinkManager::instance()->add(link); - LinkManager::instance()->addProtocol(link, mavlink); - - // Go fishing for this link's configuration window - QList actions = ui.menuNetwork->actions(); - - foreach (QAction* act, actions) - { - if (act->data().toInt() == LinkManager::instance()->getLinks().indexOf(link)) - { - act->trigger(); - break; - } - } -} - -void MainWindow::addLink(LinkInterface *link) -{ - LinkManager::instance()->add(link); - LinkManager::instance()->addProtocol(link, mavlink); - - CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); - ui.menuNetwork->addAction(commWidget->getAction()); - - // Error handling - connect(link, SIGNAL(communicationError(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection); - - //qDebug() << "ADDING LINK:" << link->getName() << "ACTION IS: " << commWidget->getAction(); - - // Special case for simulationlink - MAVLinkSimulationLink* sim = dynamic_cast(link); - if (sim) - { - //connect(sim, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64))); - connect(ui.actionSimulate, SIGNAL(triggered(bool)), sim, SLOT(connectLink(bool))); - } -} - -void MainWindow::setActiveUAS(UASInterface* uas) -{ - // Enable and rename menu - ui.menuUnmanned_System->setTitle(uas->getUASName()); - if (!ui.menuUnmanned_System->isEnabled()) ui.menuUnmanned_System->setEnabled(true); -} - -void MainWindow::UASSpecsChanged(int uas) -{ - UASInterface* activeUAS = UASManager::instance()->getActiveUAS(); - if (activeUAS) - { - if (activeUAS->getUASID() == uas) - { - ui.menuUnmanned_System->setTitle(activeUAS->getUASName()); - } - } -} - -void MainWindow::UASCreated(UASInterface* uas) -{ - - // Connect the UAS to the full user interface - - if (uas != NULL) - { - // Set default settings - setDefaultSettingsForAp(); - - // The pilot, operator and engineer views were not available on startup, enable them now - ui.actionPilotsView->setEnabled(true); - ui.actionOperatorsView->setEnabled(true); - ui.actionEngineersView->setEnabled(true); - // The UAS actions are not enabled without connection to system - ui.actionLiftoff->setEnabled(true); - ui.actionLand->setEnabled(true); - ui.actionEmergency_Kill->setEnabled(true); - ui.actionEmergency_Land->setEnabled(true); - ui.actionShutdownMAV->setEnabled(true); - - QIcon icon; - // Set matching icon - switch (uas->getSystemType()) - { - case 0: - icon = QIcon(":/images/mavs/generic.svg"); - break; - case 1: - icon = QIcon(":/images/mavs/fixed-wing.svg"); - break; - case 2: - icon = QIcon(":/images/mavs/quadrotor.svg"); - break; - case 3: - icon = QIcon(":/images/mavs/coaxial.svg"); - break; - case 4: - icon = QIcon(":/images/mavs/helicopter.svg"); - break; - case 5: - icon = QIcon(":/images/mavs/groundstation.svg"); - break; - default: - icon = QIcon(":/images/mavs/unknown.svg"); - break; - } - - QAction* uasAction = new QAction(icon, tr("Select %1 for control").arg(uas->getUASName()), ui.menuConnected_Systems); - connect(uas, SIGNAL(systemRemoved()), uasAction, SLOT(deleteLater())); - connect(uasAction, SIGNAL(triggered()), uas, SLOT(setSelected())); - connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(UASSpecsChanged(int))); - - ui.menuConnected_Systems->addAction(uasAction); - - // FIXME Should be not inside the mainwindow - if (debugConsoleDockWidget) - { - DebugConsole *debugConsole = dynamic_cast(debugConsoleDockWidget->widget()); - if (debugConsole) - { - connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), - debugConsole, SLOT(receiveTextMessage(int,int,int,QString))); - } - } - - // Health / System status indicator - if (infoDockWidget) - { - UASInfoWidget *infoWidget = dynamic_cast(infoDockWidget->widget()); - if (infoWidget) - { - infoWidget->addUAS(uas); - } - } - - // UAS List - if (listDockWidget) - { - UASListWidget *listWidget = dynamic_cast(listDockWidget->widget()); - if (listWidget) - { - listWidget->addUAS(uas); - } - } - - switch (uas->getAutopilotType()) - { - case (MAV_AUTOPILOT_SLUGS): - { - // Build Slugs Widgets - buildSlugsWidgets(); - - // Connect Slugs Widgets - connectSlugsWidgets(); - - // Arrange Slugs Centerstack - arrangeSlugsCenterStack(); - - // Connect Slugs Actions - connectSlugsActions(); - - // FIXME: This type checking might be redundant - // if (slugsDataWidget) { - // SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); - // if (dataWidget) { - // SlugsMAV* mav2 = dynamic_cast(uas); - // if (mav2) { - (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); - // //loadSlugsView(); - // loadGlobalOperatorView(); - // } - // } - // } - - } - break; - default: - case (MAV_AUTOPILOT_GENERIC): - case (MAV_AUTOPILOT_ARDUPILOTMEGA): - case (MAV_AUTOPILOT_PIXHAWK): - { - // Build Pixhawk Widgets - buildPxWidgets(); - - // Connect Pixhawk Widgets - connectPxWidgets(); - - // Arrange Pixhawk Centerstack - arrangePxCenterStack(); - - // Connect Pixhawk Actions - connectPxActions(); - } - break; - } - - // Change the view only if this is the first UAS - - // If this is the first connected UAS, it is both created as well as - // the currently active UAS - if (UASManager::instance()->getUASList().size() == 1) - { - //qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM"; - - // Load last view if setting is present - if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED")) - { - clearView(); - int view = settings.value("CURRENT_VIEW_WITH_UAS_CONNECTED").toInt(); - switch (view) - { - case VIEW_ENGINEER: - loadEngineerView(); - break; - case VIEW_MAVLINK: - loadMAVLinkView(); - break; - case VIEW_PILOT: - loadPilotView(); - break; - case VIEW_UNCONNECTED: - loadUnconnectedView(); - break; - case VIEW_OPERATOR: - default: - loadOperatorView(); - break; - } - } - else - { - loadOperatorView(); - } - } - - } - - if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true); - - // Custom widgets, added last to all menus and layouts - buildCustomWidget(); -} - -/** - * Clears the current view completely - */ -void MainWindow::clearView() -{ - // Save current state - if (UASManager::instance()->getUASList().count() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); - settings.setValue(getWindowGeometryKey(), saveGeometry()); - - QAction* temp; - - // Set tool widget visibility settings for this view - foreach (int key, toolsMenuActions.keys()) - { - temp = toolsMenuActions[key]; - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(key), currentView); - - if (temp) - { - qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked(); - settings.setValue(chKey,temp->isChecked()); - } - else - { - qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED"; - settings.setValue(chKey,false); - } - } - - changingViewsFlag = true; - // Remove all dock widgets from main window - QObjectList childList( this->children() ); - - QObjectList::iterator i; - QDockWidget* dockWidget; - for (i = childList.begin(); i != childList.end(); ++i) - { - dockWidget = dynamic_cast(*i); - if (dockWidget) - { - // Remove dock widget from main window - removeDockWidget(dockWidget); - dockWidget->hide(); - //dockWidget->setVisible(false); - } - } - changingViewsFlag = false; -} - -void MainWindow::loadEngineerView() -{ - if (currentView != VIEW_ENGINEER) - { - clearView(); - currentView = VIEW_ENGINEER; - ui.actionEngineersView->setChecked(true); - presentView(); - } -} - -void MainWindow::loadOperatorView() -{ - if (currentView != VIEW_OPERATOR) - { - clearView(); - currentView = VIEW_OPERATOR; - ui.actionOperatorsView->setChecked(true); - presentView(); - } -} - -void MainWindow::loadUnconnectedView() -{ - if (currentView != VIEW_UNCONNECTED) - { - clearView(); - currentView = VIEW_UNCONNECTED; - ui.actionUnconnectedView->setChecked(true); - presentView(); - } -} - -void MainWindow::loadPilotView() -{ - if (currentView != VIEW_PILOT) - { - clearView(); - currentView = VIEW_PILOT; - ui.actionPilotsView->setChecked(true); - presentView(); - } -} - -void MainWindow::loadMAVLinkView() -{ - if (currentView != VIEW_MAVLINK) - { - clearView(); - currentView = VIEW_MAVLINK; - ui.actionMavlinkView->setChecked(true); - presentView(); - } -} - -void MainWindow::presentView() -{ - // LINE CHART - showTheCentralWidget(CENTRAL_LINECHART, currentView); - - // MAP - showTheCentralWidget(CENTRAL_MAP, currentView); - - // PROTOCOL - showTheCentralWidget(CENTRAL_PROTOCOL, currentView); - - // HEAD UP DISPLAY - showTheCentralWidget(CENTRAL_HUD, currentView); - - // GOOGLE EARTH - showTheCentralWidget(CENTRAL_GOOGLE_EARTH, currentView); - - // LOCAL 3D VIEW - showTheCentralWidget(CENTRAL_3D_LOCAL, currentView); - - // GLOBAL 3D VIEW - showTheCentralWidget(CENTRAL_3D_MAP, currentView); - - // DATA PLOT - showTheCentralWidget(CENTRAL_DATA_PLOT, currentView); - - - // Show docked widgets based on current view and autopilot type - - // UAS CONTROL - showTheWidget(MENU_UAS_CONTROL, currentView); - - // UAS LIST - showTheWidget(MENU_UAS_LIST, currentView); - - // WAYPOINT LIST - showTheWidget(MENU_WAYPOINTS, currentView); - - // UAS STATUS - showTheWidget(MENU_STATUS, currentView); - - // DETECTION - showTheWidget(MENU_DETECTION, currentView); - - // DEBUG CONSOLE - showTheWidget(MENU_DEBUG_CONSOLE, currentView); - - // ONBOARD PARAMETERS - showTheWidget(MENU_PARAMETERS, currentView); - - // WATCHDOG - showTheWidget(MENU_WATCHDOG, currentView); - - // HUD - showTheWidget(MENU_HUD, currentView); - if (headUpDockWidget) - { - HUD* tmpHud = dynamic_cast( headUpDockWidget->widget() ); - if (tmpHud) - { - if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HUD,currentView)).toBool()) - { - addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HUD, currentView)).toInt()), - headUpDockWidget); - headUpDockWidget->show(); - } - else - { - headUpDockWidget->hide(); - } - } - } - - - // RC View - showTheWidget(MENU_RC_VIEW, currentView); - - // SLUGS DATA - showTheWidget(MENU_SLUGS_DATA, currentView); - - // SLUGS PID - showTheWidget(MENU_SLUGS_PID, currentView); - - // SLUGS HIL - showTheWidget(MENU_SLUGS_HIL, currentView); - - // SLUGS CAMERA - showTheWidget(MENU_SLUGS_CAMERA, currentView); - - // HORIZONTAL SITUATION INDICATOR - showTheWidget(MENU_HSI, currentView); - - // HEAD DOWN 1 - showTheWidget(MENU_HDD_1, currentView); - - // HEAD DOWN 2 - showTheWidget(MENU_HDD_2, currentView); - - // MAVLINK LOG PLAYER - showTheWidget(MENU_MAVLINK_LOG_PLAYER, currentView); - - // VIDEO 1 - showTheWidget(MENU_VIDEO_STREAM_1, currentView); - - // VIDEO 2 - showTheWidget(MENU_VIDEO_STREAM_2, currentView); - - this->show(); - - // Restore window state - if (UASManager::instance()->getUASList().count() > 0) - { - // Restore the mainwindow size - if (settings.contains(getWindowGeometryKey())) - { - restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray()); - } - - // Restore the widget positions and size - if (settings.contains(getWindowStateKey())) - { - restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); - } - } -} - -void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view) -{ - bool tempVisible; - QWidget* tempWidget = dockWidgets[centralWidget]; - - tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,centralWidget,view), false).toBool(); - qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; - if (toolsMenuActions[centralWidget]) - { - qDebug() << "SETTING TO:" << tempVisible; - toolsMenuActions[centralWidget]->setChecked(tempVisible); - } - - if (centerStack && tempWidget && tempVisible) - { - qDebug() << "ACTIVATING MAIN WIDGET"; - centerStack->setCurrentWidget(tempWidget); - } -} - -void MainWindow::loadDataView(QString fileName) -{ - clearView(); - - // Unload line chart - QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_LINECHART), currentView); - settings.setValue(chKey,false); - - // Set data plot in settings as current widget and then run usual update procedure - chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_DATA_PLOT), currentView); - settings.setValue(chKey,true); - - presentView(); - - // Plot is now selected, now load data from file - if (dataplotWidget) - { - dataplotWidget->loadFile(fileName); - } -// QStackedWidget *centerStack = dynamic_cast(centralWidget()); -// if (centerStack) -// { -// centerStack->setCurrentWidget(dataplotWidget); -// dataplotWidget->loadFile(fileName); -// } -// } -} - - +/*=================================================================== +======================================================================*/ + +/** + * @file + * @brief Implementation of class MainWindow + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "MG.h" +#include "QGC.h" +#include "MAVLinkSimulationLink.h" +#include "SerialLink.h" +#include "UDPLink.h" +#include "MAVLinkProtocol.h" +#include "CommConfigurationWindow.h" +#include "QGCWaypointListMulti.h" +#include "MainWindow.h" +#include "JoystickWidget.h" +#include "GAudioOutput.h" +#include "QGCToolWidget.h" +#include "QGCMAVLinkLogPlayer.h" + +#ifdef QGC_OSG_ENABLED +#include "Q3DWidgetFactory.h" +#endif + +// FIXME Move +#include "PxQuadMAV.h" +#include "SlugsMAV.h" + + +#include "LogCompressor.h" + +MainWindow* MainWindow::instance() +{ + static MainWindow* _instance = 0; + if(_instance == 0) + { + _instance = new MainWindow(); + + /* Set the application as parent to ensure that this object + * will be destroyed when the main application exits */ + //_instance->setParent(qApp); + } + return _instance; +} + +/** +* Create new mainwindow. The constructor instantiates all parts of the user +* interface. It does NOT show the mainwindow. To display it, call the show() +* method. +* +* @see QMainWindow::show() +**/ +MainWindow::MainWindow(QWidget *parent): + QMainWindow(parent), + toolsMenuActions(), + currentView(VIEW_UNCONNECTED), + aboutToCloseFlag(false), + changingViewsFlag(false) +{ + if (!settings.contains("CURRENT_VIEW")) + { + // Set this view as default view + settings.setValue("CURRENT_VIEW", currentView); + } + else + { + // LOAD THE LAST VIEW + VIEW_SECTIONS currentViewCandidate = (VIEW_SECTIONS) settings.value("CURRENT_VIEW", currentView).toInt(); + if (currentViewCandidate != VIEW_ENGINEER && + currentViewCandidate != VIEW_OPERATOR && + currentViewCandidate != VIEW_PILOT) + { + currentView = currentViewCandidate; + } + } + + setDefaultSettingsForAp(); + + settings.sync(); + + // Setup user interface + ui.setupUi(this); + + setVisible(false); + + buildCommonWidgets(); + + connectCommonWidgets(); + + arrangeCommonCenterStack(); + + configureWindowName(); + + // Set the application style (not the same as a style sheet) + // Set the style to Plastique + qApp->setStyle("plastique"); + + // Set style sheet as last step + QFile* styleSheet = new QFile(":/images/style-mission.css"); + if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) + { + QString style = QString(styleSheet->readAll()); + style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); + qApp->setStyleSheet(style); + } + + // Create actions + connectCommonActions(); + + // Set dock options + setDockOptions(AnimatedDocks | AllowTabbedDocks | AllowNestedDocks); + + // Load mavlink view as default widget set + //loadMAVLinkView(); + + statusBar()->setSizeGripEnabled(true); + + // Restore the window position and size + if (settings.contains(getWindowGeometryKey())) + { + // Restore the window geometry + restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray()); + } + else + { + // Adjust the size + adjustSize(); + } + + // Populate link menu + QList links = LinkManager::instance()->getLinks(); + foreach(LinkInterface* link, links) + { + this->addLink(link); + } + + connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addLink(LinkInterface*))); + + // Connect user interface devices + if (!joystick) + { + joystick = new JoystickInput(); + } + + // Enable and update view + presentView(); +} + +MainWindow::~MainWindow() +{ + +} + +/** + * Set default settings for this AP type. + */ +void MainWindow::setDefaultSettingsForAp() +{ + // Check if the settings exist, instantiate defaults if necessary + + // UNCONNECTED VIEW DEFAULT + QString centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_UNCONNECTED); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + + // ENABLE UAS LIST + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST, VIEW_UNCONNECTED), true); + // ENABLE COMMUNICATION CONSOLE + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_DEBUG_CONSOLE, VIEW_UNCONNECTED), true); + } + + // OPERATOR VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_MAP, VIEW_OPERATOR); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + + // ENABLE UAS LIST + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_UAS_LIST,VIEW_OPERATOR), true); + // ENABLE HUD TOOL WIDGET + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HUD,VIEW_OPERATOR), true); + } + + // ENGINEER VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_LINECHART, VIEW_ENGINEER); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + // Enable Parameter widget + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_PARAMETERS,VIEW_ENGINEER), true); + } + + // MAVLINK VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_PROTOCOL, VIEW_MAVLINK); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + } + + // PILOT VIEW DEFAULT + centralKey = buildMenuKey(SUB_SECTION_CHECKED, CENTRAL_HUD, VIEW_PILOT); + if (!settings.contains(centralKey)) + { + settings.setValue(centralKey,true); + // Enable Flight display + settings.setValue(buildMenuKey(SUB_SECTION_CHECKED,MainWindow::MENU_HDD_1,VIEW_PILOT), true); + } +} + +void MainWindow::resizeEvent(QResizeEvent * event) +{ + Q_UNUSED(event); + if (height() < 800) + { + ui.statusBar->setVisible(false); + } + else + { + ui.statusBar->setVisible(true); + } +} + +QString MainWindow::getWindowStateKey() +{ + return QString::number(currentView)+"/windowstate"; +} + +QString MainWindow::getWindowGeometryKey() +{ + return QString::number(currentView)+"/geometry"; +} + +void MainWindow::buildCustomWidget() +{ + // Show custom widgets only if UAS is connected + if (UASManager::instance()->getActiveUAS() != NULL) + { + // Enable custom widgets + ui.actionNewCustomWidget->setEnabled(true); + + // Create custom widgets + QList widgets = QGCToolWidget::createWidgetsFromSettings(this); + + if (widgets.size() > 0) + { + ui.menuTools->addSeparator(); + } + + for(int i = 0; i < widgets.size(); ++i) + { + qDebug() << "ADDING WIDGET #" << i << widgets.at(i); + // Check if this widget already has a parent, do not create it in this case + QDockWidget* dock = dynamic_cast(widgets.at(i)->parentWidget()); + if (!dock) + { + QDockWidget* dock = new QDockWidget(widgets.at(i)->windowTitle(), this); + dock->setWidget(widgets.at(i)); + connect(widgets.at(i), SIGNAL(destroyed()), dock, SLOT(deleteLater())); + QAction* showAction = new QAction(widgets.at(i)->windowTitle(), this); + connect(showAction, SIGNAL(triggered(bool)), dock, SLOT(setVisible(bool))); + connect(dock, SIGNAL(visibilityChanged(bool)), showAction, SLOT(setChecked(bool))); + widgets.at(i)->setMainMenuAction(showAction); + ui.menuTools->addAction(showAction); + addDockWidget(Qt::BottomDockWidgetArea, dock); + } + } + } +} + +void MainWindow::buildCommonWidgets() +{ + //TODO: move protocol outside UI + mavlink = new MAVLinkProtocol(); + connect(mavlink, SIGNAL(protocolStatusMessage(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection); + + // Dock widgets + if (!controlDockWidget) + { + controlDockWidget = new QDockWidget(tr("Control"), this); + controlDockWidget->setObjectName("UNMANNED_SYSTEM_CONTROL_DOCKWIDGET"); + controlDockWidget->setWidget( new UASControlWidget(this) ); + addToToolsMenu (controlDockWidget, tr("Control"), SLOT(showToolWidget(bool)), MENU_UAS_CONTROL, Qt::LeftDockWidgetArea); + } + + if (!listDockWidget) + { + listDockWidget = new QDockWidget(tr("Unmanned Systems"), this); + listDockWidget->setWidget( new UASListWidget(this) ); + listDockWidget->setObjectName("UNMANNED_SYSTEMS_LIST_DOCKWIDGET"); + addToToolsMenu (listDockWidget, tr("Unmanned Systems"), SLOT(showToolWidget(bool)), MENU_UAS_LIST, Qt::RightDockWidgetArea); + } + + if (!waypointsDockWidget) + { + waypointsDockWidget = new QDockWidget(tr("Waypoint List"), this); + waypointsDockWidget->setWidget( new QGCWaypointListMulti(this) ); + waypointsDockWidget->setObjectName("WAYPOINT_LIST_DOCKWIDGET"); + addToToolsMenu (waypointsDockWidget, tr("Waypoints List"), SLOT(showToolWidget(bool)), MENU_WAYPOINTS, Qt::BottomDockWidgetArea); + } + + if (!infoDockWidget) + { + infoDockWidget = new QDockWidget(tr("Status Details"), this); + infoDockWidget->setWidget( new UASInfoWidget(this) ); + infoDockWidget->setObjectName("UAS_STATUS_DETAILS_DOCKWIDGET"); + addToToolsMenu (infoDockWidget, tr("Status Details"), SLOT(showToolWidget(bool)), MENU_STATUS, Qt::RightDockWidgetArea); + } + + if (!debugConsoleDockWidget) + { + debugConsoleDockWidget = new QDockWidget(tr("Communication Console"), this); + debugConsoleDockWidget->setWidget( new DebugConsole(this) ); + debugConsoleDockWidget->setObjectName("COMMUNICATION_DEBUG_CONSOLE_DOCKWIDGET"); + addToToolsMenu (debugConsoleDockWidget, tr("Communication Console"), SLOT(showToolWidget(bool)), MENU_DEBUG_CONSOLE, Qt::BottomDockWidgetArea); + } + + if (!logPlayerDockWidget) + { + logPlayerDockWidget = new QDockWidget(tr("MAVLink Log Player"), this); + logPlayerDockWidget->setWidget( new QGCMAVLinkLogPlayer(mavlink, this) ); + logPlayerDockWidget->setObjectName("MAVLINK_LOG_PLAYER_DOCKWIDGET"); + addToToolsMenu(logPlayerDockWidget, tr("MAVLink Log Replay"), SLOT(showToolWidget(bool)), MENU_MAVLINK_LOG_PLAYER, Qt::RightDockWidgetArea); + } + + // Center widgets + if (!mapWidget) + { + mapWidget = new MapWidget(this); + addToCentralWidgetsMenu (mapWidget, "Maps", SLOT(showCentralWidget()),CENTRAL_MAP); + } + + if (!protocolWidget) + { + protocolWidget = new XMLCommProtocolWidget(this); + addToCentralWidgetsMenu (protocolWidget, "Mavlink Generator", SLOT(showCentralWidget()),CENTRAL_PROTOCOL); + } + + if (!dataplotWidget) + { + dataplotWidget = new QGCDataPlot2D(this); + addToCentralWidgetsMenu (dataplotWidget, "Data Plot", SLOT(showCentralWidget()),CENTRAL_DATA_PLOT); + } +} + + +void MainWindow::buildPxWidgets() +{ + //FIXME: memory of acceptList will never be freed again + QStringList* acceptList = new QStringList(); + acceptList->append("-105,roll,deg,+105,s"); + acceptList->append("-105,pitch,deg,+105,s"); + acceptList->append("-105,yaw,deg,+105,s"); + + acceptList->append("-260,rollspeed,deg/s,+260,s"); + acceptList->append("-260,pitchspeed,deg/s,+260,s"); + acceptList->append("-260,yawspeed,deg/s,+260,s"); + + //FIXME: memory of acceptList2 will never be freed again + QStringList* acceptList2 = new QStringList(); + acceptList2->append("0,abs pressure,hPa,65500"); + acceptList2->append("-999,accel. X,raw,999,s"); + acceptList2->append("-999,accel. Y,raw,999,s"); + + if (!linechartWidget) + { + // Center widgets + linechartWidget = new Linecharts(this); + addToCentralWidgetsMenu(linechartWidget, "Line Plots", SLOT(showCentralWidget()), CENTRAL_LINECHART); + } + + + if (!hudWidget) + { + hudWidget = new HUD(320, 240, this); + addToCentralWidgetsMenu(hudWidget, "HUD", SLOT(showCentralWidget()), CENTRAL_HUD); + } + + if (!dataplotWidget) + { + dataplotWidget = new QGCDataPlot2D(this); + addToCentralWidgetsMenu(dataplotWidget, "Data Plots", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT); + } + +#ifdef QGC_OSG_ENABLED + if (!_3DWidget) + { + _3DWidget = Q3DWidgetFactory::get("PIXHAWK"); + addToCentralWidgetsMenu(_3DWidget, "Local 3D", SLOT(showCentralWidget()), CENTRAL_3D_LOCAL); + } +#endif + +#ifdef QGC_OSGEARTH_ENABLED + if (!_3DMapWidget) + { + _3DMapWidget = Q3DWidgetFactory::get("MAP3D"); + addToCentralWidgetsMenu(_3DMapWidget, "OSG Earth 3D", SLOT(showCentralWidget()), CENTRAL_OSGEARTH); + } +#endif + +#if (defined _MSC_VER) | (defined Q_OS_MAC) + if (!gEarthWidget) + { + gEarthWidget = new QGCGoogleEarthView(this); + addToCentralWidgetsMenu(gEarthWidget, "Google Earth", SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH); + } + +#endif + + // Dock widgets + + if (!detectionDockWidget) + { + detectionDockWidget = new QDockWidget(tr("Object Recognition"), this); + detectionDockWidget->setWidget( new ObjectDetectionView("images/patterns", this) ); + detectionDockWidget->setObjectName("OBJECT_DETECTION_DOCK_WIDGET"); + addToToolsMenu (detectionDockWidget, tr("Object Recognition"), SLOT(showToolWidget(bool)), MENU_DETECTION, Qt::RightDockWidgetArea); + } + + if (!parametersDockWidget) + { + parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this); + parametersDockWidget->setWidget( new ParameterInterface(this) ); + parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET"); + addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea); + } + + if (!watchdogControlDockWidget) + { + watchdogControlDockWidget = new QDockWidget(tr("Process Control"), this); + watchdogControlDockWidget->setWidget( new WatchdogControl(this) ); + watchdogControlDockWidget->setObjectName("WATCHDOG_CONTROL_DOCKWIDGET"); + addToToolsMenu (watchdogControlDockWidget, tr("Process Control"), SLOT(showToolWidget(bool)), MENU_WATCHDOG, Qt::BottomDockWidgetArea); + } + + if (!hsiDockWidget) + { + hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this); + hsiDockWidget->setWidget( new HSIDisplay(this) ); + hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET"); + addToToolsMenu (hsiDockWidget, tr("HSI"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea); + } + + if (!headDown1DockWidget) + { + headDown1DockWidget = new QDockWidget(tr("Flight Display"), this); + headDown1DockWidget->setWidget( new HDDisplay(acceptList, "Flight Display", this) ); + headDown1DockWidget->setObjectName("HEAD_DOWN_DISPLAY_1_DOCK_WIDGET"); + addToToolsMenu (headDown1DockWidget, tr("Flight Display"), SLOT(showToolWidget(bool)), MENU_HDD_1, Qt::RightDockWidgetArea); + } + + if (!headDown2DockWidget) + { + headDown2DockWidget = new QDockWidget(tr("Payload Status"), this); + headDown2DockWidget->setWidget( new HDDisplay(acceptList2, "Payload Status", this) ); + headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET"); + addToToolsMenu (headDown2DockWidget, tr("Payload Status"), SLOT(showToolWidget(bool)), MENU_HDD_2, Qt::RightDockWidgetArea); + } + + if (!rcViewDockWidget) + { + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + } + + if (!headUpDockWidget) + { + headUpDockWidget = new QDockWidget(tr("HUD"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); + addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea); + } + + if (!video1DockWidget) + { + video1DockWidget = new QDockWidget(tr("Video Stream 1"), this); + HUD* video1 = new HUD(160, 120, this); + video1->enableHUDInstruments(false); + video1->enableVideo(true); + // FIXME select video stream as well + video1DockWidget->setWidget(video1); + video1DockWidget->setObjectName("VIDEO_STREAM_1_DOCK_WIDGET"); + addToToolsMenu (video1DockWidget, tr("Video Stream 1"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_1, Qt::LeftDockWidgetArea); + } + + if (!video2DockWidget) + { + video2DockWidget = new QDockWidget(tr("Video Stream 2"), this); + HUD* video2 = new HUD(160, 120, this); + video2->enableHUDInstruments(false); + video2->enableVideo(true); + // FIXME select video stream as well + video2DockWidget->setWidget(video2); + video2DockWidget->setObjectName("VIDEO_STREAM_2_DOCK_WIDGET"); + addToToolsMenu (video2DockWidget, tr("Video Stream 2"), SLOT(showToolWidget(bool)), MENU_VIDEO_STREAM_2, Qt::LeftDockWidgetArea); + } + + // Dialogue widgets + //FIXME: free memory in destructor +} + +void MainWindow::buildSlugsWidgets() +{ + if (!linechartWidget) + { + // Center widgets + linechartWidget = new Linecharts(this); + } + + if (!headUpDockWidget) + { + // Dock widgets + headUpDockWidget = new QDockWidget(tr("Control Indicator"), this); + headUpDockWidget->setWidget( new HUD(320, 240, this)); + headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET"); + addToToolsMenu (headUpDockWidget, tr("HUD"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::LeftDockWidgetArea); + } + + if (!rcViewDockWidget) + { + rcViewDockWidget = new QDockWidget(tr("Radio Control"), this); + rcViewDockWidget->setWidget( new QGCRemoteControlView(this) ); + rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET"); + addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea); + } + +// if (!slugsDataWidget) +// { +// // Dialog widgets +// slugsDataWidget = new QDockWidget(tr("Slugs Data"), this); +// slugsDataWidget->setWidget( new SlugsDataSensorView(this)); +// slugsDataWidget->setObjectName("SLUGS_DATA_DOCK_WIDGET"); +// addToToolsMenu (slugsDataWidget, tr("Telemetry Data"), SLOT(showToolWidget()), MENU_SLUGS_DATA, Qt::RightDockWidgetArea); +// } + +// if (!slugsPIDControlWidget) +// { +// slugsPIDControlWidget = new QDockWidget(tr("Slugs PID Control"), this); +// slugsPIDControlWidget->setWidget(new SlugsPIDControl(this)); +// slugsPIDControlWidget->setObjectName("SLUGS_PID_CONTROL_DOCK_WIDGET"); +// addToToolsMenu (slugsPIDControlWidget, tr("PID Configuration"), SLOT(showToolWidget()), MENU_SLUGS_PID, Qt::LeftDockWidgetArea); +// } + +// if (!slugsHilSimWidget) +// { +// slugsHilSimWidget = new QDockWidget(tr("Slugs Hil Sim"), this); +// slugsHilSimWidget->setWidget( new SlugsHilSim(this)); +// slugsHilSimWidget->setObjectName("SLUGS_HIL_SIM_DOCK_WIDGET"); +// addToToolsMenu (slugsHilSimWidget, tr("HIL Sim Configuration"), SLOT(showToolWidget()), MENU_SLUGS_HIL, Qt::LeftDockWidgetArea); +// } + +// if (!slugsCamControlWidget) +// { +// slugsCamControlWidget = new QDockWidget(tr("Slugs Video Camera Control"), this); +// slugsCamControlWidget->setWidget(new SlugsVideoCamControl(this)); +// slugsCamControlWidget->setObjectName("SLUGS_CAM_CONTROL_DOCK_WIDGET"); +// addToToolsMenu (slugsCamControlWidget, tr("Camera Control"), SLOT(showToolWidget()), MENU_SLUGS_CAMERA, Qt::BottomDockWidgetArea); +// } +} + + +void MainWindow::addToCentralWidgetsMenu ( QWidget* widget, + const QString title, + const char * slotName, + TOOLS_WIDGET_NAMES centralWidget) +{ + QAction* tempAction; + + +// Not needed any more - separate menu now available + +// // Add the separator that will separate tools from central Widgets +// if (!toolsMenuActions[CENTRAL_SEPARATOR]) +// { +// tempAction = ui.menuTools->addSeparator(); +// toolsMenuActions[CENTRAL_SEPARATOR] = tempAction; +// tempAction->setData(CENTRAL_SEPARATOR); +// } + + tempAction = ui.menuMain->addAction(title); + + tempAction->setCheckable(true); + tempAction->setData(centralWidget); + + // populate the Hashes + toolsMenuActions[centralWidget] = tempAction; + dockWidgets[centralWidget] = widget; + + QString chKey = buildMenuKey(SUB_SECTION_CHECKED, centralWidget, currentView); + + if (!settings.contains(chKey)) + { + settings.setValue(chKey,false); + tempAction->setChecked(false); + } + else + { + tempAction->setChecked(settings.value(chKey).toBool()); + } + + // connect the action + connect(tempAction,SIGNAL(triggered(bool)),this, slotName); +} + + +void MainWindow::showCentralWidget() +{ + QAction* senderAction = qobject_cast(sender()); + + // Block sender action while manipulating state + senderAction->blockSignals(true); + + int tool = senderAction->data().toInt(); + QString chKey; + + // check the current action + + if (senderAction && dockWidgets[tool]) + { + // uncheck all central widget actions + QHashIterator i(toolsMenuActions); + while (i.hasNext()) + { + i.next(); + //qDebug() << "shCW" << i.key() << "read"; + if (i.value() && i.value()->data().toInt() > 255) + { + // Block signals and uncheck action + // firing would be unneccesary + i.value()->blockSignals(true); + i.value()->setChecked(false); + i.value()->blockSignals(false); + + // update the settings + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.value()->data().toInt()), currentView); + settings.setValue(chKey,false); + } + } + + // check the current action + //qDebug() << senderAction->text(); + senderAction->setChecked(true); + + // update the central widget + centerStack->setCurrentWidget(dockWidgets[tool]); + + // store the selected central widget + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(tool), currentView); + settings.setValue(chKey,true); + + // Unblock sender action + senderAction->blockSignals(false); + + presentView(); + } +} + +/** + * Adds a widget to the tools menu and sets it visible if it was + * enabled last time. + */ +void MainWindow::addToToolsMenu ( QWidget* widget, + const QString title, + const char * slotName, + TOOLS_WIDGET_NAMES tool, + Qt::DockWidgetArea location) +{ + QAction* tempAction; + QString posKey, chKey; + + + if (toolsMenuActions[CENTRAL_SEPARATOR]) + { + tempAction = new QAction(title, this); + ui.menuTools->insertAction(toolsMenuActions[CENTRAL_SEPARATOR], + tempAction); + } + else + { + tempAction = ui.menuTools->addAction(title); + } + + tempAction->setCheckable(true); + tempAction->setData(tool); + + // populate the Hashes + toolsMenuActions[tool] = tempAction; + dockWidgets[tool] = widget; + //qDebug() << widget; + + posKey = buildMenuKey (SUB_SECTION_LOCATION,tool, currentView); + + if (!settings.contains(posKey)) + { + settings.setValue(posKey,location); + dockWidgetLocations[tool] = location; + } + else + { + dockWidgetLocations[tool] = static_cast (settings.value(posKey, Qt::RightDockWidgetArea).toInt()); + } + + chKey = buildMenuKey(SUB_SECTION_CHECKED,tool, currentView); + + if (!settings.contains(chKey)) + { + settings.setValue(chKey,false); + tempAction->setChecked(false); + widget->setVisible(false); + } + else + { + tempAction->setChecked(settings.value(chKey, false).toBool()); + widget->setVisible(settings.value(chKey, false).toBool()); + } + + // connect the action + connect(tempAction,SIGNAL(toggled(bool)),this, slotName); + + connect(qobject_cast (dockWidgets[tool]), + SIGNAL(visibilityChanged(bool)), this, SLOT(showToolWidget(bool))); + + // connect(qobject_cast (dockWidgets[tool]), + // SIGNAL(visibilityChanged(bool)), this, SLOT(updateVisibilitySettings(bool))); + + connect(qobject_cast (dockWidgets[tool]), + SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(updateLocationSettings(Qt::DockWidgetArea))); +} + +void MainWindow::showToolWidget(bool visible) +{ + if (!aboutToCloseFlag && !changingViewsFlag) + { + QAction* action = qobject_cast(sender()); + + // Prevent this to fire if undocked + if (action) + { + int tool = action->data().toInt(); + + QDockWidget* dockWidget = qobject_cast (dockWidgets[tool]); + + qDebug() << "DATA:" << tool << "FLOATING" << dockWidget->isFloating() << "checked" << action->isChecked() << "visible" << dockWidget->isVisible() << "action vis:" << action->isVisible(); + if (dockWidget && dockWidget->isVisible() != visible) + { + if (visible) + { + qDebug() << "DOCK WIDGET ADDED"; + addDockWidget(dockWidgetLocations[tool], dockWidget); + dockWidget->show(); + } + else + { + qDebug() << "DOCK WIDGET REMOVED"; + removeDockWidget(dockWidget); + //dockWidget->hide(); + } + + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == dockWidget) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,visible); + qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; + break; + } + } + } + } + + QDockWidget* dockWidget = qobject_cast(QObject::sender()); + + qDebug() << "Trying to cast dockwidget" << dockWidget << "isvisible" << visible; + + if (dockWidget) + { + // Get action + int tool = dockWidgets.key(dockWidget); + + qDebug() << "Updating widget setting" << tool << "to" << visible; + + QAction* action = toolsMenuActions[tool]; + action->blockSignals(true); + action->setChecked(visible); + action->blockSignals(false); + + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == dockWidget) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,visible); + qDebug() << "showToolWidget(): Set key" << chKey << "to" << visible; + break; + } + } + } + } +} + + +void MainWindow::showTheWidget (TOOLS_WIDGET_NAMES widget, VIEW_SECTIONS view) +{ + bool tempVisible; + Qt::DockWidgetArea tempLocation; + QDockWidget* tempWidget = static_cast (dockWidgets[widget]); + + tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,widget,view), false).toBool(); + + qDebug() << "showTheWidget(): Set key" << buildMenuKey(SUB_SECTION_CHECKED,widget,view) << "to" << tempVisible; + + if (tempWidget) + { + toolsMenuActions[widget]->setChecked(tempVisible); + } + + + //qDebug() << buildMenuKey (SUB_SECTION_CHECKED,widget,view) << tempVisible; + + tempLocation = static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,widget, view), QVariant(Qt::RightDockWidgetArea)).toInt()); + + // if (widget == MainWindow::MENU_UAS_LIST) + // { + // if (!settings.contains(buildMenuKey (SUB_SECTION_LOCATION,widget, view))) + // { + // tempLocation = Qt::RightDockWidgetArea; + // } + // } + + if (tempWidget != NULL) + { + if (tempVisible) + { + addDockWidget(tempLocation, tempWidget); + tempWidget->show(); + } + } +} + +QString MainWindow::buildMenuKey(SETTINGS_SECTIONS section, TOOLS_WIDGET_NAMES tool, VIEW_SECTIONS view) +{ + // Key is built as follows: autopilot_type/section_menu/view/tool/section + int apType; + +// apType = (UASManager::instance() && UASManager::instance()->silentGetActiveUAS())? +// UASManager::instance()->getActiveUAS()->getAutopilotType(): +// -1; + + apType = 1; + + return (QString::number(apType) + "_" + + QString::number(SECTION_MENU) + "_" + + QString::number(view) + "_" + + QString::number(tool) + "_" + + QString::number(section) + "_" ); +} + +void MainWindow::closeEvent(QCloseEvent *event) +{ + settings.setValue(getWindowGeometryKey(), saveGeometry()); + //settings.setValue("windowState", saveState()); + aboutToCloseFlag = true; + // Save the last current view in any case + settings.setValue("CURRENT_VIEW", currentView); + // Save the current window state, but only if a system is connected (else no real number of widgets would be present) + if (UASManager::instance()->getActiveUAS()) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); + // Save the current view only if a UAS is connected + if (UASManager::instance()->getUASList().length() > 0) settings.setValue("CURRENT_VIEW_WITH_UAS_CONNECTED", currentView); + settings.sync(); + QMainWindow::closeEvent(event); +} + +void MainWindow::showDockWidget (bool vis) +{ + if (!aboutToCloseFlag && !changingViewsFlag) + { + QDockWidget* temp = qobject_cast(sender()); + + if (temp) + { + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,vis); + toolsMenuActions[i.key()]->setChecked(vis); + break; + } + } + } + } +} + +void MainWindow::updateVisibilitySettings (bool vis) +{ + if (!aboutToCloseFlag && !changingViewsFlag) + { + QDockWidget* temp = qobject_cast(sender()); + + if (temp) + { + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp) + { + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(i.key()), currentView); + settings.setValue(chKey,vis); + toolsMenuActions[i.key()]->setChecked(vis); + break; + } + } + } + } +} + +void MainWindow::updateLocationSettings (Qt::DockWidgetArea location) +{ + QDockWidget* temp = qobject_cast(sender()); + + QHashIterator i(dockWidgets); + while (i.hasNext()) + { + i.next(); + if ((static_cast (dockWidgets[i.key()])) == temp) + { + QString posKey = buildMenuKey (SUB_SECTION_LOCATION,static_cast(i.key()), currentView); + settings.setValue(posKey,location); + break; + } + } +} + + +/** + * Connect the signals and slots of the common window widgets + */ +void MainWindow::connectCommonWidgets() +{ + if (infoDockWidget && infoDockWidget->widget()) + { + connect(mavlink, SIGNAL(receiveLossChanged(int, float)), + infoDockWidget->widget(), SLOT(updateSendLoss(int, float))); + } + + if (mapWidget && waypointsDockWidget->widget()) + { + + // + connect(waypointsDockWidget->widget(), SIGNAL(changePointList()), mapWidget, SLOT(clearWaypoints())); + + + + + } + + //TODO temporaly debug + if (slugsHilSimWidget && slugsHilSimWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); + } +} + +void MainWindow::createCustomWidget() +{ + //qDebug() << "ADDING CUSTOM WIDGET"; + QGCToolWidget* tool = new QGCToolWidget("Unnamed Tool", this); + + if (QGCToolWidget::instances()->size() < 2) + { + // This is the first widget + ui.menuTools->addSeparator(); + } + + QDockWidget* dock = new QDockWidget("Unnamed Tool", this); + connect(tool, SIGNAL(destroyed()), dock, SLOT(deleteLater())); + dock->setWidget(tool); + + QAction* showAction = new QAction("Show Unnamed Tool", this); + connect(dock, SIGNAL(visibilityChanged(bool)), showAction, SLOT(setChecked(bool))); + connect(showAction, SIGNAL(triggered(bool)), dock, SLOT(setVisible(bool))); + tool->setMainMenuAction(showAction); + ui.menuTools->addAction(showAction); + this->addDockWidget(Qt::BottomDockWidgetArea, dock); + dock->setVisible(true); +} + +void MainWindow::connectPxWidgets() +{ + // No special connections necessary at this point +} + +void MainWindow::connectSlugsWidgets() +{ + if (slugsHilSimWidget && slugsHilSimWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsHilSimWidget->widget(), SLOT(activeUasSet(UASInterface*))); + } + + if (slugsDataWidget && slugsDataWidget->widget()){ + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), + slugsDataWidget->widget(), SLOT(setActiveUAS(UASInterface*))); + } + + +} + +void MainWindow::arrangeCommonCenterStack() +{ + centerStack = new QStackedWidget(this); + centerStack->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding); + + if (!centerStack) return; + + if (mapWidget && (centerStack->indexOf(mapWidget) == -1)) centerStack->addWidget(mapWidget); + if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget); + if (protocolWidget && (centerStack->indexOf(protocolWidget) == -1)) centerStack->addWidget(protocolWidget); + + setCentralWidget(centerStack); +} + +void MainWindow::arrangePxCenterStack() +{ + + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } + + + if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); + +#ifdef QGC_OSG_ENABLED + if (_3DWidget && (centerStack->indexOf(_3DWidget) == -1)) centerStack->addWidget(_3DWidget); +#endif +#ifdef QGC_OSGEARTH_ENABLED + if (_3DMapWidget && (centerStack->indexOf(_3DMapWidget) == -1)) centerStack->addWidget(_3DMapWidget); +#endif +#if (defined _MSC_VER) | (defined Q_OS_MAC) + if (gEarthWidget && (centerStack->indexOf(gEarthWidget) == -1)) centerStack->addWidget(gEarthWidget); +#endif + if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); + if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget); +} + +void MainWindow::arrangeSlugsCenterStack() +{ + + if (!centerStack) { + qDebug() << "Center Stack not Created!"; + return; + } + + if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget); + if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget); + +} + +void MainWindow::configureWindowName() +{ + QList hostAddresses = QNetworkInterface::allAddresses(); + QString windowname = qApp->applicationName() + " " + qApp->applicationVersion(); + bool prevAddr = false; + + windowname.append(" (" + QHostInfo::localHostName() + ": "); + + for (int i = 0; i < hostAddresses.size(); i++) + { + // Exclude loopback IPv4 and all IPv6 addresses + if (hostAddresses.at(i) != QHostAddress("127.0.0.1") && !hostAddresses.at(i).toString().contains(":")) + { + if(prevAddr) windowname.append("/"); + windowname.append(hostAddresses.at(i).toString()); + prevAddr = true; + } + } + + windowname.append(")"); + + setWindowTitle(windowname); + +#ifndef Q_WS_MAC + //qApp->setWindowIcon(QIcon(":/core/images/qtcreator_logo_128.png")); +#endif +} + +void MainWindow::startVideoCapture() +{ + QString format = "bmp"; + QString initialPath = QDir::currentPath() + tr("/untitled.") + format; + + QString screenFileName = QFileDialog::getSaveFileName(this, tr("Save As"), + initialPath, + tr("%1 Files (*.%2);;All Files (*)") + .arg(format.toUpper()) + .arg(format)); + delete videoTimer; + videoTimer = new QTimer(this); + //videoTimer->setInterval(40); + //connect(videoTimer, SIGNAL(timeout()), this, SLOT(saveScreen())); + //videoTimer->stop(); +} + +void MainWindow::stopVideoCapture() +{ + videoTimer->stop(); + + // TODO Convert raw images to PNG +} + +void MainWindow::saveScreen() +{ + QPixmap window = QPixmap::grabWindow(this->winId()); + QString format = "bmp"; + + if (!screenFileName.isEmpty()) + { + window.save(screenFileName, format.toAscii()); + } +} + +/** + * Reload the style sheet from disk. The function tries to load "qgroundcontrol.css" from the application + * directory (which by default does not exist). If it fails, it will load the bundled default CSS + * from memory. + * To customize the application, just create a qgroundcontrol.css file in the application directory + */ +void MainWindow::reloadStylesheet() +{ + QString fileName = QCoreApplication::applicationDirPath() + "/style-indoor.css"; + + // Let user select style sheet + fileName = QFileDialog::getOpenFileName(this, tr("Specify stylesheet"), fileName, tr("CSS Stylesheet (*.css);;")); + + if (!fileName.endsWith(".css")) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Information); + msgBox.setText(tr("QGroundControl did lot load a new style")); + msgBox.setInformativeText(tr("No suitable .css file selected. Please select a valid .css file.")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + return; + } + + // Load style sheet + QFile* styleSheet = new QFile(fileName); + if (!styleSheet->exists()) + { + styleSheet = new QFile(":/images/style-mission.css"); + } + if (styleSheet->open(QIODevice::ReadOnly | QIODevice::Text)) + { + QString style = QString(styleSheet->readAll()); + style.replace("ICONDIR", QCoreApplication::applicationDirPath()+ "/images/"); + qApp->setStyleSheet(style); + } + else + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Information); + msgBox.setText(tr("QGroundControl did lot load a new style")); + msgBox.setInformativeText(tr("Stylesheet file %1 was not readable").arg(fileName)); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } + delete styleSheet; +} + +/** + * The status message will be overwritten if a new message is posted to this function + * + * @param status message text + * @param timeout how long the status should be displayed + */ +void MainWindow::showStatusMessage(const QString& status, int timeout) +{ + statusBar()->showMessage(status, timeout); +} + +/** + * The status message will be overwritten if a new message is posted to this function. + * it will be automatically hidden after 5 seconds. + * + * @param status message text + */ +void MainWindow::showStatusMessage(const QString& status) +{ + statusBar()->showMessage(status, 20000); +} + +void MainWindow::showCriticalMessage(const QString& title, const QString& message) +{ + QMessageBox msgBox(this); + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText(title); + msgBox.setInformativeText(message); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); +} + +void MainWindow::showInfoMessage(const QString& title, const QString& message) +{ + QMessageBox msgBox(this); + msgBox.setIcon(QMessageBox::Information); + msgBox.setText(title); + msgBox.setInformativeText(message); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); +} + +/** +* @brief Create all actions associated to the main window +* +**/ +void MainWindow::connectCommonActions() +{ + ui.actionNewCustomWidget->setEnabled(false); + + // Bind together the perspective actions + QActionGroup* perspectives = new QActionGroup(ui.menuPerspectives); + perspectives->addAction(ui.actionEngineersView); + perspectives->addAction(ui.actionMavlinkView); + perspectives->addAction(ui.actionPilotsView); + perspectives->addAction(ui.actionOperatorsView); + perspectives->addAction(ui.actionUnconnectedView); + perspectives->setExclusive(true); + + // Mark the right one as selected + if (currentView == VIEW_ENGINEER) ui.actionEngineersView->setChecked(true); + if (currentView == VIEW_MAVLINK) ui.actionMavlinkView->setChecked(true); + if (currentView == VIEW_PILOT) ui.actionPilotsView->setChecked(true); + if (currentView == VIEW_OPERATOR) ui.actionOperatorsView->setChecked(true); + if (currentView == VIEW_UNCONNECTED) ui.actionUnconnectedView->setChecked(true); + + // The pilot, engineer and operator view are not available on startup + // since they only make sense with a system connected. + ui.actionPilotsView->setEnabled(false); + ui.actionOperatorsView->setEnabled(false); + ui.actionEngineersView->setEnabled(false); + // The UAS actions are not enabled without connection to system + ui.actionLiftoff->setEnabled(false); + ui.actionLand->setEnabled(false); + ui.actionEmergency_Kill->setEnabled(false); + ui.actionEmergency_Land->setEnabled(false); + ui.actionShutdownMAV->setEnabled(false); + + // Connect actions from ui + connect(ui.actionAdd_Link, SIGNAL(triggered()), this, SLOT(addLink())); + + // Connect internal actions + connect(UASManager::instance(), SIGNAL(UASCreated(UASInterface*)), this, SLOT(UASCreated(UASInterface*))); + connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); + + // Unmanned System controls + connect(ui.actionLiftoff, SIGNAL(triggered()), UASManager::instance(), SLOT(launchActiveUAS())); + connect(ui.actionLand, SIGNAL(triggered()), UASManager::instance(), SLOT(returnActiveUAS())); + connect(ui.actionEmergency_Land, SIGNAL(triggered()), UASManager::instance(), SLOT(stopActiveUAS())); + connect(ui.actionEmergency_Kill, SIGNAL(triggered()), UASManager::instance(), SLOT(killActiveUAS())); + connect(ui.actionShutdownMAV, SIGNAL(triggered()), UASManager::instance(), SLOT(shutdownActiveUAS())); + connect(ui.actionConfiguration, SIGNAL(triggered()), UASManager::instance(), SLOT(configureActiveUAS())); + + // Views actions + connect(ui.actionPilotsView, SIGNAL(triggered()), this, SLOT(loadPilotView())); + connect(ui.actionEngineersView, SIGNAL(triggered()), this, SLOT(loadEngineerView())); + connect(ui.actionOperatorsView, SIGNAL(triggered()), this, SLOT(loadOperatorView())); + connect(ui.actionUnconnectedView, SIGNAL(triggered()), this, SLOT(loadUnconnectedView())); + + connect(ui.actionMavlinkView, SIGNAL(triggered()), this, SLOT(loadMAVLinkView())); + connect(ui.actionReloadStyle, SIGNAL(triggered()), this, SLOT(reloadStylesheet())); + + // Help Actions + connect(ui.actionOnline_Documentation, SIGNAL(triggered()), this, SLOT(showHelp())); + connect(ui.actionDeveloper_Credits, SIGNAL(triggered()), this, SLOT(showCredits())); + connect(ui.actionProject_Roadmap_2, SIGNAL(triggered()), this, SLOT(showRoadMap())); + + // Custom widget actions + connect(ui.actionNewCustomWidget, SIGNAL(triggered()), this, SLOT(createCustomWidget())); + + // Audio output + ui.actionMuteAudioOutput->setChecked(GAudioOutput::instance()->isMuted()); + connect(ui.actionMuteAudioOutput, SIGNAL(triggered(bool)), GAudioOutput::instance(), SLOT(mute(bool))); + + // User interaction + // NOTE: Joystick thread is not started and + // configuration widget is not instantiated + // unless it is actually used + // so no ressources spend on this. + ui.actionJoystickSettings->setVisible(true); + // Joystick configuration + connect(ui.actionJoystickSettings, SIGNAL(triggered()), this, SLOT(configure())); +} + +void MainWindow::connectPxActions() +{ + + +} + +void MainWindow::connectSlugsActions() +{ + +} + +void MainWindow::showHelp() +{ + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/users/"))) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("Could not open help in browser"); + msgBox.setInformativeText("To get to the online help, please open http://qgroundcontrol.org/user_guide in a browser."); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } +} + +void MainWindow::showCredits() +{ + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/credits/"))) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("Could not open credits in browser"); + msgBox.setInformativeText("To get to the online help, please open http://qgroundcontrol.org/credits in a browser."); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } +} + +void MainWindow::showRoadMap() +{ + if(!QDesktopServices::openUrl(QUrl("http://qgroundcontrol.org/roadmap/"))) + { + QMessageBox msgBox; + msgBox.setIcon(QMessageBox::Critical); + msgBox.setText("Could not open roadmap in browser"); + msgBox.setInformativeText("To get to the online help, please open http://qgroundcontrol.org/roadmap in a browser."); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.setDefaultButton(QMessageBox::Ok); + msgBox.exec(); + } +} + +void MainWindow::configure() +{ + if (!joystickWidget) + { + if (!joystick->isRunning()) + { + joystick->start(); + } + joystickWidget = new JoystickWidget(joystick); + } + joystickWidget->show(); +} + +void MainWindow::addLink() +{ + SerialLink* link = new SerialLink(); + // TODO This should be only done in the dialog itself + + LinkManager::instance()->add(link); + LinkManager::instance()->addProtocol(link, mavlink); + + // Go fishing for this link's configuration window + QList actions = ui.menuNetwork->actions(); + + foreach (QAction* act, actions) + { + if (act->data().toInt() == LinkManager::instance()->getLinks().indexOf(link)) + { + act->trigger(); + break; + } + } +} + +void MainWindow::addLink(LinkInterface *link) +{ + LinkManager::instance()->add(link); + LinkManager::instance()->addProtocol(link, mavlink); + + CommConfigurationWindow* commWidget = new CommConfigurationWindow(link, mavlink, this); + ui.menuNetwork->addAction(commWidget->getAction()); + + // Error handling + connect(link, SIGNAL(communicationError(QString,QString)), this, SLOT(showCriticalMessage(QString,QString)), Qt::QueuedConnection); + + //qDebug() << "ADDING LINK:" << link->getName() << "ACTION IS: " << commWidget->getAction(); + + // Special case for simulationlink + MAVLinkSimulationLink* sim = dynamic_cast(link); + if (sim) + { + //connect(sim, SIGNAL(valueChanged(int,QString,double,quint64)), linechart, SLOT(appendData(int,QString,double,quint64))); + connect(ui.actionSimulate, SIGNAL(triggered(bool)), sim, SLOT(connectLink(bool))); + } +} + +void MainWindow::setActiveUAS(UASInterface* uas) +{ + // Enable and rename menu + ui.menuUnmanned_System->setTitle(uas->getUASName()); + if (!ui.menuUnmanned_System->isEnabled()) ui.menuUnmanned_System->setEnabled(true); +} + +void MainWindow::UASSpecsChanged(int uas) +{ + UASInterface* activeUAS = UASManager::instance()->getActiveUAS(); + if (activeUAS) + { + if (activeUAS->getUASID() == uas) + { + ui.menuUnmanned_System->setTitle(activeUAS->getUASName()); + } + } +} + +void MainWindow::UASCreated(UASInterface* uas) +{ + + // Connect the UAS to the full user interface + + if (uas != NULL) + { + // Set default settings + setDefaultSettingsForAp(); + + // The pilot, operator and engineer views were not available on startup, enable them now + ui.actionPilotsView->setEnabled(true); + ui.actionOperatorsView->setEnabled(true); + ui.actionEngineersView->setEnabled(true); + // The UAS actions are not enabled without connection to system + ui.actionLiftoff->setEnabled(true); + ui.actionLand->setEnabled(true); + ui.actionEmergency_Kill->setEnabled(true); + ui.actionEmergency_Land->setEnabled(true); + ui.actionShutdownMAV->setEnabled(true); + + QIcon icon; + // Set matching icon + switch (uas->getSystemType()) + { + case 0: + icon = QIcon(":/images/mavs/generic.svg"); + break; + case 1: + icon = QIcon(":/images/mavs/fixed-wing.svg"); + break; + case 2: + icon = QIcon(":/images/mavs/quadrotor.svg"); + break; + case 3: + icon = QIcon(":/images/mavs/coaxial.svg"); + break; + case 4: + icon = QIcon(":/images/mavs/helicopter.svg"); + break; + case 5: + icon = QIcon(":/images/mavs/groundstation.svg"); + break; + default: + icon = QIcon(":/images/mavs/unknown.svg"); + break; + } + + QAction* uasAction = new QAction(icon, tr("Select %1 for control").arg(uas->getUASName()), ui.menuConnected_Systems); + connect(uas, SIGNAL(systemRemoved()), uasAction, SLOT(deleteLater())); + connect(uasAction, SIGNAL(triggered()), uas, SLOT(setSelected())); + connect(uas, SIGNAL(systemSpecsChanged(int)), this, SLOT(UASSpecsChanged(int))); + + ui.menuConnected_Systems->addAction(uasAction); + + // FIXME Should be not inside the mainwindow + if (debugConsoleDockWidget) + { + DebugConsole *debugConsole = dynamic_cast(debugConsoleDockWidget->widget()); + if (debugConsole) + { + connect(uas, SIGNAL(textMessageReceived(int,int,int,QString)), + debugConsole, SLOT(receiveTextMessage(int,int,int,QString))); + } + } + + // Health / System status indicator + if (infoDockWidget) + { + UASInfoWidget *infoWidget = dynamic_cast(infoDockWidget->widget()); + if (infoWidget) + { + infoWidget->addUAS(uas); + } + } + + // UAS List + if (listDockWidget) + { + UASListWidget *listWidget = dynamic_cast(listDockWidget->widget()); + if (listWidget) + { + listWidget->addUAS(uas); + } + } + + switch (uas->getAutopilotType()) + { + case (MAV_AUTOPILOT_SLUGS): + { + // Build Slugs Widgets + buildSlugsWidgets(); + + // Connect Slugs Widgets + connectSlugsWidgets(); + + // Arrange Slugs Centerstack + arrangeSlugsCenterStack(); + + // Connect Slugs Actions + connectSlugsActions(); + + // FIXME: This type checking might be redundant +// // if (slugsDataWidget) { +// // SlugsDataSensorView* dataWidget = dynamic_cast(slugsDataWidget->widget()); +// // if (dataWidget) { +// // SlugsMAV* mav2 = dynamic_cast(uas); +// // if (mav2) { +// (dynamic_cast(slugsDataWidget->widget()))->addUAS(uas); +// // //loadSlugsView(); +// // loadGlobalOperatorView(); +// // } +// // } +// // } + + } + break; + default: + case (MAV_AUTOPILOT_GENERIC): + case (MAV_AUTOPILOT_ARDUPILOTMEGA): + case (MAV_AUTOPILOT_PIXHAWK): + { + // Build Pixhawk Widgets + buildPxWidgets(); + + // Connect Pixhawk Widgets + connectPxWidgets(); + + // Arrange Pixhawk Centerstack + arrangePxCenterStack(); + + // Connect Pixhawk Actions + connectPxActions(); + } + break; + } + + // Change the view only if this is the first UAS + + // If this is the first connected UAS, it is both created as well as + // the currently active UAS + if (UASManager::instance()->getUASList().size() == 1) + { + //qDebug() << "UPDATING THE VIEW SINCE THIS IS THE FIRST CONNECTED SYSTEM"; + + // Load last view if setting is present + if (settings.contains("CURRENT_VIEW_WITH_UAS_CONNECTED")) + { + clearView(); + int view = settings.value("CURRENT_VIEW_WITH_UAS_CONNECTED").toInt(); + switch (view) + { + case VIEW_ENGINEER: + loadEngineerView(); + break; + case VIEW_MAVLINK: + loadMAVLinkView(); + break; + case VIEW_PILOT: + loadPilotView(); + break; + case VIEW_UNCONNECTED: + loadUnconnectedView(); + break; + case VIEW_OPERATOR: + default: + loadOperatorView(); + break; + } + } + else + { + loadOperatorView(); + } + } + + } + + if (!ui.menuConnected_Systems->isEnabled()) ui.menuConnected_Systems->setEnabled(true); + + // Custom widgets, added last to all menus and layouts + buildCustomWidget(); +} + +/** + * Clears the current view completely + */ +void MainWindow::clearView() +{ + // Save current state + if (UASManager::instance()->getUASList().count() > 0) settings.setValue(getWindowStateKey(), saveState(QGC::applicationVersion())); + settings.setValue(getWindowGeometryKey(), saveGeometry()); + + QAction* temp; + + // Set tool widget visibility settings for this view + foreach (int key, toolsMenuActions.keys()) + { + temp = toolsMenuActions[key]; + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(key), currentView); + + if (temp) + { + qDebug() << "TOOL:" << chKey << "IS:" << temp->isChecked(); + settings.setValue(chKey,temp->isChecked()); + } + else + { + qDebug() << "TOOL:" << chKey << "IS DEFAULT AND UNCHECKED"; + settings.setValue(chKey,false); + } + } + + changingViewsFlag = true; + // Remove all dock widgets from main window + QObjectList childList( this->children() ); + + QObjectList::iterator i; + QDockWidget* dockWidget; + for (i = childList.begin(); i != childList.end(); ++i) + { + dockWidget = dynamic_cast(*i); + if (dockWidget) + { + // Remove dock widget from main window + removeDockWidget(dockWidget); + dockWidget->hide(); + //dockWidget->setVisible(false); + } + } + changingViewsFlag = false; +} + +void MainWindow::loadEngineerView() +{ + if (currentView != VIEW_ENGINEER) + { + clearView(); + currentView = VIEW_ENGINEER; + ui.actionEngineersView->setChecked(true); + presentView(); + } +} + +void MainWindow::loadOperatorView() +{ + if (currentView != VIEW_OPERATOR) + { + clearView(); + currentView = VIEW_OPERATOR; + ui.actionOperatorsView->setChecked(true); + presentView(); + } +} + +void MainWindow::loadUnconnectedView() +{ + if (currentView != VIEW_UNCONNECTED) + { + clearView(); + currentView = VIEW_UNCONNECTED; + ui.actionUnconnectedView->setChecked(true); + presentView(); + } +} + +void MainWindow::loadPilotView() +{ + if (currentView != VIEW_PILOT) + { + clearView(); + currentView = VIEW_PILOT; + ui.actionPilotsView->setChecked(true); + presentView(); + } +} + +void MainWindow::loadMAVLinkView() +{ + if (currentView != VIEW_MAVLINK) + { + clearView(); + currentView = VIEW_MAVLINK; + ui.actionMavlinkView->setChecked(true); + presentView(); + } +} + +void MainWindow::presentView() +{ + // LINE CHART + showTheCentralWidget(CENTRAL_LINECHART, currentView); + + // MAP + showTheCentralWidget(CENTRAL_MAP, currentView); + + // PROTOCOL + showTheCentralWidget(CENTRAL_PROTOCOL, currentView); + + // HEAD UP DISPLAY + showTheCentralWidget(CENTRAL_HUD, currentView); + + // GOOGLE EARTH + showTheCentralWidget(CENTRAL_GOOGLE_EARTH, currentView); + + // LOCAL 3D VIEW + showTheCentralWidget(CENTRAL_3D_LOCAL, currentView); + + // GLOBAL 3D VIEW + showTheCentralWidget(CENTRAL_3D_MAP, currentView); + + // DATA PLOT + showTheCentralWidget(CENTRAL_DATA_PLOT, currentView); + + + // Show docked widgets based on current view and autopilot type + + // UAS CONTROL + showTheWidget(MENU_UAS_CONTROL, currentView); + + // UAS LIST + showTheWidget(MENU_UAS_LIST, currentView); + + // WAYPOINT LIST + showTheWidget(MENU_WAYPOINTS, currentView); + + // UAS STATUS + showTheWidget(MENU_STATUS, currentView); + + // DETECTION + showTheWidget(MENU_DETECTION, currentView); + + // DEBUG CONSOLE + showTheWidget(MENU_DEBUG_CONSOLE, currentView); + + // ONBOARD PARAMETERS + showTheWidget(MENU_PARAMETERS, currentView); + + // WATCHDOG + showTheWidget(MENU_WATCHDOG, currentView); + + // HUD + showTheWidget(MENU_HUD, currentView); + if (headUpDockWidget) + { + HUD* tmpHud = dynamic_cast( headUpDockWidget->widget() ); + if (tmpHud) + { + if (settings.value(buildMenuKey (SUB_SECTION_CHECKED,MENU_HUD,currentView)).toBool()) + { + addDockWidget(static_cast (settings.value(buildMenuKey (SUB_SECTION_LOCATION,MENU_HUD, currentView)).toInt()), + headUpDockWidget); + headUpDockWidget->show(); + } + else + { + headUpDockWidget->hide(); + } + } + } + + + // RC View + showTheWidget(MENU_RC_VIEW, currentView); + + // SLUGS DATA + showTheWidget(MENU_SLUGS_DATA, currentView); + + // SLUGS PID + showTheWidget(MENU_SLUGS_PID, currentView); + + // SLUGS HIL + showTheWidget(MENU_SLUGS_HIL, currentView); + + // SLUGS CAMERA + showTheWidget(MENU_SLUGS_CAMERA, currentView); + + // HORIZONTAL SITUATION INDICATOR + showTheWidget(MENU_HSI, currentView); + + // HEAD DOWN 1 + showTheWidget(MENU_HDD_1, currentView); + + // HEAD DOWN 2 + showTheWidget(MENU_HDD_2, currentView); + + // MAVLINK LOG PLAYER + showTheWidget(MENU_MAVLINK_LOG_PLAYER, currentView); + + // VIDEO 1 + showTheWidget(MENU_VIDEO_STREAM_1, currentView); + + // VIDEO 2 + showTheWidget(MENU_VIDEO_STREAM_2, currentView); + + this->show(); + + // Restore window state + if (UASManager::instance()->getUASList().count() > 0) + { + // Restore the mainwindow size + if (settings.contains(getWindowGeometryKey())) + { + restoreGeometry(settings.value(getWindowGeometryKey()).toByteArray()); + } + + // Restore the widget positions and size + if (settings.contains(getWindowStateKey())) + { + restoreState(settings.value(getWindowStateKey()).toByteArray(), QGC::applicationVersion()); + } + } +} + +void MainWindow::showTheCentralWidget (TOOLS_WIDGET_NAMES centralWidget, VIEW_SECTIONS view) +{ + bool tempVisible; + QWidget* tempWidget = dockWidgets[centralWidget]; + + tempVisible = settings.value(buildMenuKey(SUB_SECTION_CHECKED,centralWidget,view), false).toBool(); + qDebug() << buildMenuKey (SUB_SECTION_CHECKED,centralWidget,view) << tempVisible; + if (toolsMenuActions[centralWidget]) + { + qDebug() << "SETTING TO:" << tempVisible; + toolsMenuActions[centralWidget]->setChecked(tempVisible); + } + + if (centerStack && tempWidget && tempVisible) + { + qDebug() << "ACTIVATING MAIN WIDGET"; + centerStack->setCurrentWidget(tempWidget); + } +} + +void MainWindow::loadDataView(QString fileName) +{ + clearView(); + + // Unload line chart + QString chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_LINECHART), currentView); + settings.setValue(chKey,false); + + // Set data plot in settings as current widget and then run usual update procedure + chKey = buildMenuKey (SUB_SECTION_CHECKED,static_cast(CENTRAL_DATA_PLOT), currentView); + settings.setValue(chKey,true); + + presentView(); + + // Plot is now selected, now load data from file + if (dataplotWidget) + { + dataplotWidget->loadFile(fileName); + } +// QStackedWidget *centerStack = dynamic_cast(centralWidget()); +// if (centerStack) +// { +// centerStack->setCurrentWidget(dataplotWidget); +// dataplotWidget->loadFile(fileName); +// } +// } +} + + diff --git a/src/ui/WaypointList.cc b/src/ui/WaypointList.cc index 5159b4e6372d6ff850a65daed59095fc0e53d382..ff6510b69dabeda7929bc66d38fa26b4b096fd7f 100644 --- a/src/ui/WaypointList.cc +++ b/src/ui/WaypointList.cc @@ -1,485 +1,485 @@ -/*===================================================================== - -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 . - -======================================================================*/ - -/** - * @file - * @brief Waypoint list widget - * - * @author Lorenz Meier - * @author Benjamin Knecht - * @author Petri Tanskanen - * - */ - -#include "WaypointList.h" -#include "ui_WaypointList.h" -#include -#include -#include -#include -#include -#include - -WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : - QWidget(parent), - uas(NULL), - mavX(0.0), - mavY(0.0), - mavZ(0.0), - mavYaw(0.0), - m_ui(new Ui::WaypointList) -{ - m_ui->setupUi(this); - - listLayout = new QVBoxLayout(m_ui->listWidget); - listLayout->setSpacing(6); - listLayout->setMargin(0); - listLayout->setAlignment(Qt::AlignTop); - m_ui->listWidget->setLayout(listLayout); - - // ADD WAYPOINT - // Connect add action, set right button icon and connect action to this class - connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered())); - connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(add())); - - // ADD WAYPOINT AT CURRENT POSITION - connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositonWaypoint())); - - // SEND WAYPOINTS - connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit())); - - // REQUEST WAYPOINTS - connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read())); - - // SAVE/LOAD WAYPOINTS - connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints())); - connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints())); - - //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); - - - - // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED - setUAS(uas); - - // STATUS LABEL - updateStatusLabel(""); - - this->setVisible(false); - loadFileGlobalWP = false; - readGlobalWP = false; - centerMapCoordinate.setX(0.0); - centerMapCoordinate.setY(0.0); - -} - -WaypointList::~WaypointList() -{ - delete m_ui; -} - -void WaypointList::updatePosition(UASInterface* uas, double x, double y, double z, quint64 usec) -{ - Q_UNUSED(uas); - Q_UNUSED(usec); - mavX = x; - mavY = y; - mavZ = z; -} - -void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec) -{ - Q_UNUSED(uas); - Q_UNUSED(usec); - Q_UNUSED(roll); - Q_UNUSED(pitch); - mavYaw = yaw; -} - -void WaypointList::setUAS(UASInterface* uas) -{ - if (this->uas == NULL && uas != NULL) - { - this->uas = uas; - - connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updatePosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); - - connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); - connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void))); - connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int,Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); - connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); - connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); - - } -} - -void WaypointList::saveWaypoints() -{ - if (uas) - { - QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); - uas->getWaypointManager()->saveWaypoints(fileName); - } -} - -void WaypointList::loadWaypoints() -{ - if (uas) - { - - - QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); - uas->getWaypointManager()->loadWaypoints(fileName); - } -} - -void WaypointList::transmit() -{ - if (uas) - { - uas->getWaypointManager()->writeWaypoints(); - } -} - -void WaypointList::read() -{ - if (uas) - { - uas->getWaypointManager()->readWaypoints(); - } -} - -void WaypointList::add() -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager().getWaypointList(); - Waypoint *wp; - - - if (waypoints.size() > 0) - { - // Create waypoint with last frame - Waypoint *last = waypoints.at(waypoints.size()-1); - wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), - last->getHoldTime(), last->getFrame(), last->getAction()); - uas->getWaypointManager()->addWaypoint(wp); - } - else - { - // Create global frame waypoint per default - wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); - uas->getWaypointManager()->addWaypoint(wp); - } -// if (wp->getFrame() == MAV_FRAME_GLOBAL) -// { -// emit createWaypointAtMap(QPointF(wp->getX(), wp->getY())); -// } - - } -} - -void WaypointList::addCurrentPositonWaypoint() -{ - /* TODO: implement with new waypoint structure - if (uas) - { - // For Global Waypoints - //if(isGlobalWP) - //{ - //isLocalWP = false; - //} - //else - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - if (waypoints.size() > 0) - { - Waypoint *last = waypoints.at(waypoints.size()-1); - Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); - uas->getWaypointManager()->addWaypoint(wp); - } - else - { - Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000); - uas->getWaypointManager()->addWaypoint(wp); - } - - //isLocalWP = true; - } - } - */ -} - -void WaypointList::updateStatusLabel(const QString &string) -{ - m_ui->statusLabel->setText(string); - - -} - -void WaypointList::changeCurrentWaypoint(quint16 seq) -{ - if (uas) - { - uas->getWaypointManager()->setCurrentWaypoint(seq); - } -} - -void WaypointList::currentWaypointChanged(quint16 seq) -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - - if (seq < waypoints.size()) - { - for(int i = 0; i < waypoints.size(); i++) - { - WaypointView* widget = wpViews.find(waypoints[i]).value(); - - if (waypoints[i]->getId() == seq) - { - widget->setCurrent(true); - } - else - { - widget->setCurrent(false); - } - } - } - } -} - -void WaypointList::updateWaypoint(int uas, Waypoint* wp) -{ - Q_UNUSED(uas); - WaypointView *wpv = wpViews.value(wp); - wpv->updateValues(); -} - -void WaypointList::waypointListChanged() -{ - if (uas) - { - // Prevent updates to prevent visual flicker - this->setUpdatesEnabled(false); - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - - if (!wpViews.empty()) - { - QMapIterator viewIt(wpViews); - viewIt.toFront(); - while(viewIt.hasNext()) - { - viewIt.next(); - Waypoint *cur = viewIt.key(); - int i; - for (i = 0; i < waypoints.size(); i++) - { - if (waypoints[i] == cur) - { - break; - } - } - if (i == waypoints.size()) - { - WaypointView* widget = wpViews.find(cur).value(); - widget->hide(); - listLayout->removeWidget(widget); - wpViews.remove(cur); - } - } - } - - // then add/update the views for each waypoint in the list - for(int i = 0; i < waypoints.size(); i++) - { - Waypoint *wp = waypoints[i]; - if (!wpViews.contains(wp)) - { - WaypointView* wpview = new WaypointView(wp, this); - wpViews.insert(wp, wpview); - connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); - connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); - connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); - connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); - connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); - } - WaypointView *wpv = wpViews.value(wp); - wpv->updateValues(); // update the values of the ui elements in the view - listLayout->addWidget(wpv); - - } - this->setUpdatesEnabled(true); - } - - - loadFileGlobalWP = false; -} - -void WaypointList::moveUp(Waypoint* wp) -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - - //get the current position of wp in the local storage - int i; - for (i = 0; i < waypoints.size(); i++) - { - if (waypoints[i] == wp) - break; - } - - // if wp was found and its not the first entry, move it - if (i < waypoints.size() && i > 0) - { - uas->getWaypointManager()->moveWaypoint(i, i-1); - } - } - - //emitir seņal de cambio orden en la lista, - //la debe capturar el mapwidget para volver a dibujar la ruta -} - -void WaypointList::moveDown(Waypoint* wp) -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - - //get the current position of wp in the local storage - int i; - for (i = 0; i < waypoints.size(); i++) - { - if (waypoints[i] == wp) - break; - } - - // if wp was found and its not the last entry, move it - if (i < waypoints.size()-1) - { - uas->getWaypointManager()->moveWaypoint(i, i+1); - } - } -} - -void WaypointList::removeWaypoint(Waypoint* wp) -{ - if (uas) - { - uas->getWaypointManager()->removeWaypoint(wp->getId()); - } -} - -void WaypointList::changeEvent(QEvent *e) -{ - switch (e->type()) { - case QEvent::LanguageChange: - m_ui->retranslateUi(this); - break; - default: - break; - } -} - - - -void WaypointList::on_clearWPListButton_clicked() -{ - - - if (uas) - { - emit clearPathclicked(); - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) - { - WaypointView* widget = wpViews.find(waypoints[0]).value(); - widget->remove(); - } - } -} - -/** @brief The MapWidget informs that a waypoint global was changed on the map */ - -void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - if (waypoints.size() > 0) - { - Waypoint *temp = waypoints.at(indexWP); - - temp->setX(coordinate.x()); - temp->setY(coordinate.y()); - - //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); - //widget->updateValues(); - } - } - - -} - -///** @brief The MapWidget informs that a waypoint global was changed on the map */ - -//void WaypointList::waypointGlobalPositionChanged(Waypoint* wp) -//{ -// QPointF coordinate; -// coordinate.setX(wp->getX()); -// coordinate.setY(wp->getY()); - -// emit ChangeWaypointGlobalPosition(wp->getId(), coordinate); - - -//} - -void WaypointList::clearWPWidget() -{ - if (uas) - { - const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); - while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) - { - WaypointView* widget = wpViews.find(waypoints[0]).value(); - widget->remove(); - } - } - //emit changePositionWPBySpinBox(wp->getId(), wp->getY(), wp->getX()); -} - -void WaypointList::setIsLoadFileWP() -{ - loadFileGlobalWP = true; -} - -void WaypointList::setIsReadGlobalWP(bool value) -{ - // FIXME James Check this - Q_UNUSED(value); - // readGlobalWP = value; -} +/*===================================================================== + +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 . + +======================================================================*/ + +/** + * @file + * @brief Waypoint list widget + * + * @author Lorenz Meier + * @author Benjamin Knecht + * @author Petri Tanskanen + * + */ + +#include "WaypointList.h" +#include "ui_WaypointList.h" +#include +#include +#include +#include +#include +#include + +WaypointList::WaypointList(QWidget *parent, UASInterface* uas) : + QWidget(parent), + uas(NULL), + mavX(0.0), + mavY(0.0), + mavZ(0.0), + mavYaw(0.0), + m_ui(new Ui::WaypointList) +{ + m_ui->setupUi(this); + + listLayout = new QVBoxLayout(m_ui->listWidget); + listLayout->setSpacing(6); + listLayout->setMargin(0); + listLayout->setAlignment(Qt::AlignTop); + m_ui->listWidget->setLayout(listLayout); + + // ADD WAYPOINT + // Connect add action, set right button icon and connect action to this class + connect(m_ui->addButton, SIGNAL(clicked()), m_ui->actionAddWaypoint, SIGNAL(triggered())); + connect(m_ui->actionAddWaypoint, SIGNAL(triggered()), this, SLOT(add())); + + // ADD WAYPOINT AT CURRENT POSITION + connect(m_ui->positionAddButton, SIGNAL(clicked()), this, SLOT(addCurrentPositonWaypoint())); + + // SEND WAYPOINTS + connect(m_ui->transmitButton, SIGNAL(clicked()), this, SLOT(transmit())); + + // REQUEST WAYPOINTS + connect(m_ui->readButton, SIGNAL(clicked()), this, SLOT(read())); + + // SAVE/LOAD WAYPOINTS + connect(m_ui->saveButton, SIGNAL(clicked()), this, SLOT(saveWaypoints())); + connect(m_ui->loadButton, SIGNAL(clicked()), this, SLOT(loadWaypoints())); + + //connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setUAS(UASInterface*))); + + + + // SET UAS AFTER ALL SIGNALS/SLOTS ARE CONNECTED + setUAS(uas); + + // STATUS LABEL + updateStatusLabel(""); + + this->setVisible(false); + loadFileGlobalWP = false; + readGlobalWP = false; + centerMapCoordinate.setX(0.0); + centerMapCoordinate.setY(0.0); + +} + +WaypointList::~WaypointList() +{ + delete m_ui; +} + +void WaypointList::updatePosition(UASInterface* uas, double x, double y, double z, quint64 usec) +{ + Q_UNUSED(uas); + Q_UNUSED(usec); + mavX = x; + mavY = y; + mavZ = z; +} + +void WaypointList::updateAttitude(UASInterface* uas, double roll, double pitch, double yaw, quint64 usec) +{ + Q_UNUSED(uas); + Q_UNUSED(usec); + Q_UNUSED(roll); + Q_UNUSED(pitch); + mavYaw = yaw; +} + +void WaypointList::setUAS(UASInterface* uas) +{ + if (this->uas == NULL && uas != NULL) + { + this->uas = uas; + + connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updatePosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(attitudeChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateAttitude(UASInterface*,double,double,double,quint64))); + + connect(uas->getWaypointManager(), SIGNAL(updateStatusString(const QString &)), this, SLOT(updateStatusLabel(const QString &))); + connect(uas->getWaypointManager(), SIGNAL(waypointListChanged(void)), this, SLOT(waypointListChanged(void))); + connect(uas->getWaypointManager(), SIGNAL(waypointChanged(int,Waypoint*)), this, SLOT(updateWaypoint(int,Waypoint*))); + connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(uas->getWaypointManager(),SIGNAL(loadWPFile()),this,SLOT(setIsLoadFileWP())); + connect(uas->getWaypointManager(),SIGNAL(readGlobalWPFromUAS(bool)),this,SLOT(setIsReadGlobalWP(bool))); + + } +} + +void WaypointList::saveWaypoints() +{ + if (uas) + { + QString fileName = QFileDialog::getSaveFileName(this, tr("Save File"), "./waypoints.txt", tr("Waypoint File (*.txt)")); + uas->getWaypointManager()->saveWaypoints(fileName); + } +} + +void WaypointList::loadWaypoints() +{ + if (uas) + { + + + QString fileName = QFileDialog::getOpenFileName(this, tr("Load File"), ".", tr("Waypoint File (*.txt)")); + uas->getWaypointManager()->loadWaypoints(fileName); + } +} + +void WaypointList::transmit() +{ + if (uas) + { + uas->getWaypointManager()->writeWaypoints(); + } +} + +void WaypointList::read() +{ + if (uas) + { + uas->getWaypointManager()->readWaypoints(); + } +} + +void WaypointList::add() +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + Waypoint *wp; + + + if (waypoints.size() > 0) + { + // Create waypoint with last frame + Waypoint *last = waypoints.at(waypoints.size()-1); + wp = new Waypoint(0, last->getX(), last->getY(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), + last->getHoldTime(), last->getFrame(), last->getAction()); + uas->getWaypointManager()->addWaypoint(wp); + } + else + { + // Create global frame waypoint per default + wp = new Waypoint(0, uas->getLongitude(), uas->getLatitude(), uas->getAltitude(), 0.0, true, true, 0.15, 0, MAV_FRAME_GLOBAL, MAV_ACTION_NAVIGATE); + uas->getWaypointManager()->addWaypoint(wp); + } +// if (wp->getFrame() == MAV_FRAME_GLOBAL) +// { +// emit createWaypointAtMap(QPointF(wp->getX(), wp->getY())); +// } + + } +} + +void WaypointList::addCurrentPositonWaypoint() +{ + /* TODO: implement with new waypoint structure + if (uas) + { + // For Global Waypoints + //if(isGlobalWP) + //{ + //isLocalWP = false; + //} + //else + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + if (waypoints.size() > 0) + { + Waypoint *last = waypoints.at(waypoints.size()-1); + Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime()); + uas->getWaypointManager()->addWaypoint(wp); + } + else + { + Waypoint *wp = new Waypoint(0, (qRound(mavX*100))/100., (qRound(mavY*100))/100., (qRound(mavZ*100))/100., (qRound(mavYaw*100))/100., true, true, 0.15, 2000); + uas->getWaypointManager()->addWaypoint(wp); + } + + //isLocalWP = true; + } + } + */ +} + +void WaypointList::updateStatusLabel(const QString &string) +{ + m_ui->statusLabel->setText(string); + + +} + +void WaypointList::changeCurrentWaypoint(quint16 seq) +{ + if (uas) + { + uas->getWaypointManager()->setCurrentWaypoint(seq); + } +} + +void WaypointList::currentWaypointChanged(quint16 seq) +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + + if (seq < waypoints.size()) + { + for(int i = 0; i < waypoints.size(); i++) + { + WaypointView* widget = wpViews.find(waypoints[i]).value(); + + if (waypoints[i]->getId() == seq) + { + widget->setCurrent(true); + } + else + { + widget->setCurrent(false); + } + } + } + } +} + +void WaypointList::updateWaypoint(int uas, Waypoint* wp) +{ + Q_UNUSED(uas); + WaypointView *wpv = wpViews.value(wp); + wpv->updateValues(); +} + +void WaypointList::waypointListChanged() +{ + if (uas) + { + // Prevent updates to prevent visual flicker + this->setUpdatesEnabled(false); + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + + if (!wpViews.empty()) + { + QMapIterator viewIt(wpViews); + viewIt.toFront(); + while(viewIt.hasNext()) + { + viewIt.next(); + Waypoint *cur = viewIt.key(); + int i; + for (i = 0; i < waypoints.size(); i++) + { + if (waypoints[i] == cur) + { + break; + } + } + if (i == waypoints.size()) + { + WaypointView* widget = wpViews.find(cur).value(); + widget->hide(); + listLayout->removeWidget(widget); + wpViews.remove(cur); + } + } + } + + // then add/update the views for each waypoint in the list + for(int i = 0; i < waypoints.size(); i++) + { + Waypoint *wp = waypoints[i]; + if (!wpViews.contains(wp)) + { + WaypointView* wpview = new WaypointView(wp, this); + wpViews.insert(wp, wpview); + connect(wpview, SIGNAL(moveDownWaypoint(Waypoint*)), this, SLOT(moveDown(Waypoint*))); + connect(wpview, SIGNAL(moveUpWaypoint(Waypoint*)), this, SLOT(moveUp(Waypoint*))); + connect(wpview, SIGNAL(removeWaypoint(Waypoint*)), this, SLOT(removeWaypoint(Waypoint*))); + connect(wpview, SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointChanged(quint16))); + connect(wpview, SIGNAL(changeCurrentWaypoint(quint16)), this, SLOT(changeCurrentWaypoint(quint16))); + } + WaypointView *wpv = wpViews.value(wp); + wpv->updateValues(); // update the values of the ui elements in the view + listLayout->addWidget(wpv); + + } + this->setUpdatesEnabled(true); + } + + + loadFileGlobalWP = false; +} + +void WaypointList::moveUp(Waypoint* wp) +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + + //get the current position of wp in the local storage + int i; + for (i = 0; i < waypoints.size(); i++) + { + if (waypoints[i] == wp) + break; + } + + // if wp was found and its not the first entry, move it + if (i < waypoints.size() && i > 0) + { + uas->getWaypointManager()->moveWaypoint(i, i-1); + } + } + + //emitir seņal de cambio orden en la lista, + //la debe capturar el mapwidget para volver a dibujar la ruta +} + +void WaypointList::moveDown(Waypoint* wp) +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + + //get the current position of wp in the local storage + int i; + for (i = 0; i < waypoints.size(); i++) + { + if (waypoints[i] == wp) + break; + } + + // if wp was found and its not the last entry, move it + if (i < waypoints.size()-1) + { + uas->getWaypointManager()->moveWaypoint(i, i+1); + } + } +} + +void WaypointList::removeWaypoint(Waypoint* wp) +{ + if (uas) + { + uas->getWaypointManager()->removeWaypoint(wp->getId()); + } +} + +void WaypointList::changeEvent(QEvent *e) +{ + switch (e->type()) { + case QEvent::LanguageChange: + m_ui->retranslateUi(this); + break; + default: + break; + } +} + + + +void WaypointList::on_clearWPListButton_clicked() +{ + + + if (uas) + { + emit clearPathclicked(); + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) + { + WaypointView* widget = wpViews.find(waypoints[0]).value(); + widget->remove(); + } + } +} + +/** @brief The MapWidget informs that a waypoint global was changed on the map */ + +void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP) +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + if (waypoints.size() > 0) + { + Waypoint *temp = waypoints.at(indexWP); + + temp->setX(coordinate.x()); + temp->setY(coordinate.y()); + + //WaypointGlobalView* widget = wpGlobalViews.find(waypoints[indexWP]).value(); + //widget->updateValues(); + } + } + + +} + +///** @brief The MapWidget informs that a waypoint global was changed on the map */ + +//void WaypointList::waypointGlobalPositionChanged(Waypoint* wp) +//{ +// QPointF coordinate; +// coordinate.setX(wp->getX()); +// coordinate.setY(wp->getY()); + +// emit ChangeWaypointGlobalPosition(wp->getId(), coordinate); + + +//} + +void WaypointList::clearWPWidget() +{ + if (uas) + { + const QVector &waypoints = uas->getWaypointManager()->getWaypointList(); + while(!waypoints.isEmpty())//for(int i = 0; i <= waypoints.size(); i++) + { + WaypointView* widget = wpViews.find(waypoints[0]).value(); + widget->remove(); + } + } + //emit changePositionWPBySpinBox(wp->getId(), wp->getY(), wp->getX()); +} + +void WaypointList::setIsLoadFileWP() +{ + loadFileGlobalWP = true; +} + +void WaypointList::setIsReadGlobalWP(bool value) +{ + // FIXME James Check this + Q_UNUSED(value); + // readGlobalWP = value; +} diff --git a/src/ui/designer/QGCActionButton.cc b/src/ui/designer/QGCActionButton.cc index 462cad4ac67ae159f4ec3f083452e8fd2b88da5f..b32b4f06f117f6e8edc2ad6fe209cb60ddb214f9 100644 --- a/src/ui/designer/QGCActionButton.cc +++ b/src/ui/designer/QGCActionButton.cc @@ -1,139 +1,139 @@ -#include "QGCActionButton.h" -#include "ui_QGCActionButton.h" - -#include "MAVLinkProtocol.h" -#include "UASManager.h" - -const char* kActionLabels[MAV_ACTION_NB] = -{"HOLD", - "START MOTORS", - "LAUNCH", - "RETURN", - "EMERGENCY LAND", - "EMERGENCY KILL", - "CONFIRM KILL", - "CONTINUE", - "STOP MOTORS", - "HALT", - "SHUTDOWN", - "REBOOT", - "SET MANUAL", - "SET AUTO", - "READ STORAGE", - "WRITE STORAGE", - "CALIBRATE RC", - "CALIBRATE GYRO", - "CALIBRATE MAG", - "CALIBRATE ACC", - "CALIBRATE PRESSURE", - "START REC", - "PAUSE REC", - "STOP REC", - "TAKEOFF", - "NAVIGATE", - "LAND", - "LOITER", - "SET ORIGIN", - "RELAY ON", - //"RELAY OFF", - //"GET IMAGE", - //"START VIDEO", - //"STOP VIDEO", - "RESET MAP", - "RESET PLAN"}; - -QGCActionButton::QGCActionButton(QWidget *parent) : - QGCToolWidgetItem("Button", parent), - ui(new Ui::QGCActionButton), - uas(NULL) -{ - ui->setupUi(this); - - connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction())); - connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); - connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString))); - connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString))); - - // Hide all edit items - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // add action labels to combobox - for (int i = 0; i < MAV_ACTION_NB; i++) - { - ui->editActionComboBox->addItem(kActionLabels[i]); - } -} - -QGCActionButton::~QGCActionButton() -{ - delete ui; -} - -void QGCActionButton::sendAction() -{ - if (QGCToolWidgetItem::uas) - { - MAV_ACTION action = static_cast( - ui->editActionComboBox->currentIndex()); - - QGCToolWidgetItem::uas->setAction(action); - } - else - { - qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; - } -} - -void QGCActionButton::setActionButtonName(QString text) -{ - ui->actionButton->setText(text); -} - -void QGCActionButton::startEditMode() -{ - ui->editActionComboBox->show(); - ui->editActionsRefreshButton->show(); - ui->editFinishButton->show(); - ui->editNameLabel->show(); - ui->editButtonName->show(); - isInEditMode = true; -} - -void QGCActionButton::endEditMode() -{ - ui->editActionComboBox->hide(); - ui->editActionsRefreshButton->hide(); - ui->editFinishButton->hide(); - ui->editNameLabel->hide(); - ui->editButtonName->hide(); - - // Write to settings - emit editingFinished(); - - isInEditMode = false; -} - -void QGCActionButton::writeSettings(QSettings& settings) -{ - settings.setValue("TYPE", "BUTTON"); - settings.setValue("QGC_ACTION_BUTTON_DESCRIPTION", ui->nameLabel->text()); - settings.setValue("QGC_ACTION_BUTTON_BUTTONTEXT", ui->actionButton->text()); - settings.setValue("QGC_ACTION_BUTTON_ACTIONID", ui->editActionComboBox->currentIndex()); - settings.sync(); -} - -void QGCActionButton::readSettings(const QSettings& settings) -{ - ui->editNameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->editButtonName->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - - ui->nameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); - ui->actionButton->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); - ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); - qDebug() << "DONE READING SETTINGS"; -} +#include "QGCActionButton.h" +#include "ui_QGCActionButton.h" + +#include "MAVLinkProtocol.h" +#include "UASManager.h" + +const char* kActionLabels[MAV_ACTION_NB] = +{"HOLD", + "START MOTORS", + "LAUNCH", + "RETURN", + "EMERGENCY LAND", + "EMERGENCY KILL", + "CONFIRM KILL", + "CONTINUE", + "STOP MOTORS", + "HALT", + "SHUTDOWN", + "REBOOT", + "SET MANUAL", + "SET AUTO", + "READ STORAGE", + "WRITE STORAGE", + "CALIBRATE RC", + "CALIBRATE GYRO", + "CALIBRATE MAG", + "CALIBRATE ACC", + "CALIBRATE PRESSURE", + "START REC", + "PAUSE REC", + "STOP REC", + "TAKEOFF", + "NAVIGATE", + "LAND", + "LOITER", + "SET ORIGIN", + "RELAY ON", + //"RELAY OFF", + //"GET IMAGE", + //"START VIDEO", + //"STOP VIDEO", + "RESET MAP", + "RESET PLAN"}; + +QGCActionButton::QGCActionButton(QWidget *parent) : + QGCToolWidgetItem("Button", parent), + ui(new Ui::QGCActionButton), + uas(NULL) +{ + ui->setupUi(this); + + connect(ui->actionButton, SIGNAL(clicked()), this, SLOT(sendAction())); + connect(ui->editFinishButton, SIGNAL(clicked()), this, SLOT(endEditMode())); + connect(ui->editButtonName, SIGNAL(textChanged(QString)), this, SLOT(setActionButtonName(QString))); + connect(ui->editActionComboBox, SIGNAL(currentIndexChanged(QString)), ui->nameLabel, SLOT(setText(QString))); + + // Hide all edit items + ui->editActionComboBox->hide(); + ui->editActionsRefreshButton->hide(); + ui->editFinishButton->hide(); + ui->editNameLabel->hide(); + ui->editButtonName->hide(); + + // add action labels to combobox + for (int i = 0; i < MAV_ACTION_NB; i++) + { + ui->editActionComboBox->addItem(kActionLabels[i]); + } +} + +QGCActionButton::~QGCActionButton() +{ + delete ui; +} + +void QGCActionButton::sendAction() +{ + if (QGCToolWidgetItem::uas) + { + MAV_ACTION action = static_cast( + ui->editActionComboBox->currentIndex()); + + QGCToolWidgetItem::uas->setAction(action); + } + else + { + qDebug() << __FILE__ << __LINE__ << "NO UAS SET, DOING NOTHING"; + } +} + +void QGCActionButton::setActionButtonName(QString text) +{ + ui->actionButton->setText(text); +} + +void QGCActionButton::startEditMode() +{ + ui->editActionComboBox->show(); + ui->editActionsRefreshButton->show(); + ui->editFinishButton->show(); + ui->editNameLabel->show(); + ui->editButtonName->show(); + isInEditMode = true; +} + +void QGCActionButton::endEditMode() +{ + ui->editActionComboBox->hide(); + ui->editActionsRefreshButton->hide(); + ui->editFinishButton->hide(); + ui->editNameLabel->hide(); + ui->editButtonName->hide(); + + // Write to settings + emit editingFinished(); + + isInEditMode = false; +} + +void QGCActionButton::writeSettings(QSettings& settings) +{ + settings.setValue("TYPE", "BUTTON"); + settings.setValue("QGC_ACTION_BUTTON_DESCRIPTION", ui->nameLabel->text()); + settings.setValue("QGC_ACTION_BUTTON_BUTTONTEXT", ui->actionButton->text()); + settings.setValue("QGC_ACTION_BUTTON_ACTIONID", ui->editActionComboBox->currentIndex()); + settings.sync(); +} + +void QGCActionButton::readSettings(const QSettings& settings) +{ + ui->editNameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); + ui->editButtonName->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); + ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); + + ui->nameLabel->setText(settings.value("QGC_ACTION_BUTTON_DESCRIPTION", "ERROR LOADING BUTTON").toString()); + ui->actionButton->setText(settings.value("QGC_ACTION_BUTTON_BUTTONTEXT", "UNKNOWN").toString()); + ui->editActionComboBox->setCurrentIndex(settings.value("QGC_ACTION_BUTTON_ACTIONID", 0).toInt()); + qDebug() << "DONE READING SETTINGS"; +} diff --git a/src/ui/uas/UASView.cc b/src/ui/uas/UASView.cc index 59de83d6a739b1289fbfc4d1a295a4c933e0d399..c83dc078e77dd14c96a2c6dd08fcb905dad6a4a1 100644 --- a/src/ui/uas/UASView.cc +++ b/src/ui/uas/UASView.cc @@ -1,624 +1,628 @@ -/*===================================================================== -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 . - -======================================================================*/ - -/** - * @file - * @brief Implementation of one airstrip - * - * @author Lorenz Meier - * - */ - -#include -#include -#include -#include -#include - -#include "QGC.h" -#include "MG.h" -#include "UASManager.h" -#include "UASView.h" -#include "UASWaypointManager.h" -#include "ui_UASView.h" - -UASView::UASView(UASInterface* uas, QWidget *parent) : - QWidget(parent), - startTime(0), - timeout(false), - iconIsRed(true), - timeRemaining(0), - chargeLevel(0), - uas(uas), - load(0), - state("UNKNOWN"), - stateDesc(tr("Unknown state")), - mode("MAV_MODE_UNKNOWN"), - thrust(0), - isActive(false), - x(0), - y(0), - z(0), - totalSpeed(0), - lat(0), - lon(0), - alt(0), - groundDistance(0), - localFrame(false), - removeAction(new QAction("Delete this system", this)), - renameAction(new QAction("Rename..", this)), - selectAction(new QAction("Select this system", this )), - selectAirframeAction(new QAction("Select Airframe", this)), - m_ui(new Ui::UASView) -{ - m_ui->setupUi(this); - - // Setup communication - //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64))); - connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); - connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); - connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double))); - connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); - connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString))); - connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); - connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double))); - connect(uas, SIGNAL(heartbeatTimeout()), this, SLOT(heartbeatTimeout())); - connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int))); - connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16))); - connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); - connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool))); - - // Setup UAS selection - connect(m_ui->uasViewFrame, SIGNAL(clicked(bool)), this, SLOT(setUASasActive(bool))); - - // Setup user interaction - connect(m_ui->liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); - connect(m_ui->haltButton, SIGNAL(clicked()), uas, SLOT(halt())); - connect(m_ui->continueButton, SIGNAL(clicked()), uas, SLOT(go())); - connect(m_ui->landButton, SIGNAL(clicked()), uas, SLOT(home())); - connect(m_ui->abortButton, SIGNAL(clicked()), uas, SLOT(emergencySTOP())); - connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL())); - connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); - - // Allow to delete this widget - connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater())); - connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); - connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); - connect(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe())); - connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); - - // Name changes - connect(uas, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString))); - - // Set static values - - // Name - if (uas->getUASName() == "") - { - m_ui->nameLabel->setText(tr("UAS") + QString::number(uas->getUASID())); - } - else - { - m_ui->nameLabel->setText(uas->getUASName()); - } - - setBackgroundColor(); - - // Heartbeat fade - refreshTimer = new QTimer(this); - connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh())); - - // Hide kill and shutdown buttons per default - m_ui->killButton->hide(); - m_ui->shutdownButton->hide(); - - setSystemType(uas, uas->getSystemType()); -} - -UASView::~UASView() -{ - delete m_ui; - delete removeAction; - delete renameAction; - delete selectAction; -} - -void UASView::heartbeatTimeout() -{ - timeout = true; -} - -/** - * Set the background color based on the MAV color. If the MAV is selected as the - * currently actively controlled system, the frame color is highlighted - */ -void UASView::setBackgroundColor() -{ - // UAS color - QColor uasColor = uas->getColor(); - QString colorstyle; - QString borderColor = "#4A4A4F"; - if (isActive) - { - borderColor = "#FA4A4F"; - uasColor = uasColor.darker(475); - } - else - { - uasColor = uasColor.darker(675); - } - colorstyle = colorstyle.sprintf("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 2px solid %s; }", - uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str()); - m_ui->uasViewFrame->setStyleSheet(colorstyle); -} - -void UASView::setUASasActive(bool active) -{ - if (active) - { - UASManager::instance()->setActiveUAS(this->uas); - } -} - -void UASView::updateActiveUAS(UASInterface* uas, bool active) -{ - if (uas == this->uas) - { - this->isActive = active; - setBackgroundColor(); - } -} - -void UASView::updateMode(int sysId, QString status, QString description) -{ - Q_UNUSED(description); - if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status); -} - -void UASView::mouseDoubleClickEvent (QMouseEvent * event) -{ - Q_UNUSED(event); - UASManager::instance()->setActiveUAS(uas); - qDebug() << __FILE__ << __LINE__ << "DOUBLECLICKED"; -} - -void UASView::enterEvent(QEvent* event) -{ - if (event->type() == QEvent::MouseMove) - { - emit uasInFocus(uas); - if (uas != UASManager::instance()->getActiveUAS()) - { - grabMouse(QCursor(Qt::PointingHandCursor)); - } - } - qDebug() << __FILE__ << __LINE__ << "IN FOCUS"; - - if (event->type() == QEvent::MouseButtonDblClick) - { - qDebug() << __FILE__ << __LINE__ << "UAS CLICKED!"; - } -} - -void UASView::leaveEvent(QEvent* event) -{ - if (event->type() == QEvent::MouseMove) - { - emit uasOutFocus(uas); - releaseMouse(); - } -} - -void UASView::showEvent(QShowEvent* event) -{ - // React only to internal (pre-display) - // events - Q_UNUSED(event); - refreshTimer->start(updateInterval); -} - -void UASView::hideEvent(QHideEvent* event) -{ - // React only to internal (pre-display) - // events - Q_UNUSED(event); - refreshTimer->stop(); -} - -void UASView::receiveHeartbeat(UASInterface* uas) -{ - Q_UNUSED(uas); - heartbeatColor = QColor(20, 200, 20); - QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }"); - m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name())); - if (timeout) setBackgroundColor(); - timeout = false; -} - -void UASView::updateName(const QString& name) -{ - if (uas) m_ui->nameLabel->setText(name); -} - -/** - * The current system type is represented through the system icon. - * - * @param uas Source system, has to be the same as this->uas - * @param systemType type ID, following the MAVLink system type conventions - * @see http://pixhawk.ethz.ch/software/mavlink - */ -void UASView::setSystemType(UASInterface* uas, unsigned int systemType) -{ - if (uas == this->uas) - { - // Set matching icon - switch (systemType) - { - case 0: - m_ui->typeButton->setIcon(QIcon(":/images/mavs/generic.svg")); - break; - case 1: - m_ui->typeButton->setIcon(QIcon(":/images/mavs/fixed-wing.svg")); - break; - case 2: - m_ui->typeButton->setIcon(QIcon(":/images/mavs/quadrotor.svg")); - break; - case 3: - m_ui->typeButton->setIcon(QIcon(":/images/mavs/coaxial.svg")); - break; - case 4: - m_ui->typeButton->setIcon(QIcon(":/images/mavs/helicopter.svg")); - break; - case 5: - m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg")); - break; - case 6: - { - // A groundstation is a special system type, update widget - QString result; - m_ui->nameLabel->setText(tr("OCU ") + result.sprintf("%03d", uas->getUASID())); - m_ui->waypointLabel->setText(""); - m_ui->timeRemainingLabel->setText("Online:"); - m_ui->batteryBar->hide(); - m_ui->thrustBar->hide(); - m_ui->stateLabel->hide(); - m_ui->statusTextLabel->hide(); - m_ui->waypointLabel->hide(); - m_ui->liftoffButton->hide(); - m_ui->haltButton->hide(); - m_ui->landButton->hide(); - m_ui->shutdownButton->hide(); - m_ui->abortButton->hide(); - m_ui->typeButton->setIcon(QIcon(":/images/mavs/groundstation.svg")); - } - break; - default: - m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg")); - break; - } - } -} - -void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec) -{ - Q_UNUSED(usec); - Q_UNUSED(uas); - this->x = x; - this->y = y; - this->z = z; - if (!localFrame) - { - localFrame = true; - } -} - -void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec) -{ - Q_UNUSED(uas); - Q_UNUSED(usec); - this->lon = lon; - this->lat = lat; - this->alt = alt; -} - -void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec) -{ - Q_UNUSED(usec); - totalSpeed = sqrt((pow(x, 2) + pow(y, 2) + pow(z, 2))); -} - -void UASView::currentWaypointUpdated(quint16 waypoint) -{ - m_ui->waypointLabel->setText(tr("WP") + QString::number(waypoint)); -} - -void UASView::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current) -{ - Q_UNUSED(x); - Q_UNUSED(y); - Q_UNUSED(z); - Q_UNUSED(yaw); - Q_UNUSED(autocontinue); - if (uasId == this->uas->getUASID()) - { - if (current) - { - m_ui->waypointLabel->setText(tr("WP") + QString::number(id)); - } - } -} - -void UASView::selectWaypoint(int uasId, int id) -{ - if (uasId == this->uas->getUASID()) - { - m_ui->waypointLabel->setText(tr("WP") + QString::number(id)); - } -} - -void UASView::updateThrust(UASInterface* uas, double thrust) -{ - if (this->uas == uas) - { - this->thrust = thrust; - } -} - -void UASView::updateBattery(UASInterface* uas, double voltage, double percent, int seconds) -{ - Q_UNUSED(voltage); - if (this->uas == uas) - { - timeRemaining = seconds; - chargeLevel = percent; - } -} - -void UASView::updateState(UASInterface* uas, QString uasState, QString stateDescription) -{ - if (this->uas == uas) - { - state = uasState; - stateDesc = stateDescription; - } -} - -void UASView::updateLoad(UASInterface* uas, double load) -{ - if (this->uas == uas) - { - this->load = load; - } -} - -void UASView::contextMenuEvent (QContextMenuEvent* event) -{ - QMenu menu(this); - menu.addAction(renameAction); - if (timeout) - { - menu.addAction(removeAction); - } - menu.addAction(selectAction); - menu.addAction(selectAirframeAction); - menu.exec(event->globalPos()); -} - -void UASView::rename() -{ - if (uas) - { - bool ok; - QString newName = QInputDialog::getText(this, tr("Rename System %1").arg(uas->getUASName()), - tr("System Name:"), QLineEdit::Normal, - uas->getUASName(), &ok); - - if (ok && !newName.isEmpty()) uas->setUASName(newName); - } -} - -void UASView::selectAirframe() -{ - if (uas) - { - // Get list of airframes from UAS - QStringList airframes; - airframes << "Generic" - << "Multiplex Easystar" - << "Multiplex Twinstar" - << "Multiplex Merlin" - << "Pixhawk Cheetah" - << "Mikrokopter" - << "Reaper" - << "Predator" - << "Coaxial" - << "Pteryx"; - - bool ok; - QString item = QInputDialog::getItem(this, tr("Select Airframe for %1").arg(uas->getUASName()), - tr("Airframe"), airframes, uas->getAirframe(), false, &ok); - if (ok && !item.isEmpty()) - { - // Set this airframe as UAS airframe - uas->setAirframe(airframes.indexOf(item)); - } - } -} - -void UASView::refresh() -{ - //setUpdatesEnabled(false); - //setUpdatesEnabled(true); - //repaint(); - - static quint64 lastupdate = 0; - //qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate; - lastupdate = MG::TIME::getGroundTimeNow(); - - // FIXME - static int generalUpdateCount = 0; - - if (generalUpdateCount == 4) - { -#if (QGC_EVENTLOOP_DEBUG) - qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; -#endif - generalUpdateCount = 0; - //qDebug() << "UPDATING EVERYTHING"; - // State - m_ui->stateLabel->setText(state); - m_ui->statusTextLabel->setText(stateDesc); - - // Battery - m_ui->batteryBar->setValue(static_cast(this->chargeLevel)); - //m_ui->loadBar->setValue(static_cast(this->load)); - m_ui->thrustBar->setValue(this->thrust); - - // Position - QString position; - position = position.sprintf("%05.1f %05.1f %06.1f m", x, y, z); - m_ui->positionLabel->setText(position); - QString globalPosition; - QString latIndicator; - if (lat > 0) - { - latIndicator = "N"; - } - else - { - latIndicator = "S"; - } - QString lonIndicator; - if (lon > 0) - { - lonIndicator = "E"; - } - else - { - lonIndicator = "W"; - } - globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %06.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt); - m_ui->positionLabel->setText(globalPosition); - - // Altitude - if (groundDistance == 0 && alt != 0) - { - m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 6, 'f', 1, '0')); - } - else - { - m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 6, 'f', 1, '0')); - } - - // Speed - QString speed("%1 m/s"); - m_ui->speedLabel->setText(speed.arg(totalSpeed, 4, 'f', 1, '0')); - - // Thrust - m_ui->thrustBar->setValue(thrust * 100); - - if(this->timeRemaining > 1 && this->timeRemaining < QGC::MAX_FLIGHT_TIME) - { - // Filter output to get a higher stability - static double filterTime = static_cast(this->timeRemaining); - filterTime = 0.8 * filterTime + 0.2 * static_cast(this->timeRemaining); - int sec = static_cast(filterTime - static_cast(filterTime / 60.0f) * 60); - int min = static_cast(filterTime / 60); - int hours = static_cast(filterTime - min * 60 - sec); - - QString timeText; - timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); - m_ui->timeRemainingLabel->setText(timeText); - } - else - { - m_ui->timeRemainingLabel->setText(tr("Calc..")); - } - - // Time Elapsed - //QDateTime time = MG::TIME::msecToQDateTime(uas->getUptime()); - - quint64 filterTime = uas->getUptime() / 1000; - int sec = static_cast(filterTime - static_cast(filterTime / 60) * 60); - int min = static_cast(filterTime / 60); - int hours = static_cast(filterTime - min * 60 - sec); - QString timeText; - timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); - m_ui->timeElapsedLabel->setText(timeText); - } - generalUpdateCount++; - - QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }"); - - if (timeout) - { - // CRITICAL CONDITION, NO HEARTBEAT - - QString borderColor = "#FFFF00"; - if (isActive) - { - borderColor = "#FA4A4F"; - } - - if (iconIsRed) - { - QColor warnColor(Qt::red); - m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name())); - QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name()); - m_ui->uasViewFrame->setStyleSheet(style); - } - else - { - QColor warnColor(Qt::black); - m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name())); - QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name()); - m_ui->uasViewFrame->setStyleSheet(style); - } - iconIsRed = !iconIsRed; - } - else - { - // Fade heartbeat icon - // Make color darker - heartbeatColor = heartbeatColor.darker(150); - - //m_ui->heartbeatIcon->setAutoFillBackground(true); - m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name())); - } - //setUpdatesEnabled(true); - - //setUpdatesEnabled(false); -} - -void UASView::changeEvent(QEvent *e) -{ - QWidget::changeEvent(e); - switch (e->type()) { - case QEvent::LanguageChange: - m_ui->retranslateUi(this); - break; - default: - break; - } -} +/*===================================================================== +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 . + +======================================================================*/ + +/** + * @file + * @brief Implementation of one airstrip + * + * @author Lorenz Meier + * + */ + +#include +#include +#include +#include +#include + +#include "QGC.h" +#include "MG.h" +#include "UASManager.h" +#include "UASView.h" +#include "UASWaypointManager.h" +#include "ui_UASView.h" + +UASView::UASView(UASInterface* uas, QWidget *parent) : + QWidget(parent), + startTime(0), + timeout(false), + iconIsRed(true), + timeRemaining(0), + chargeLevel(0), + uas(uas), + load(0), + state("UNKNOWN"), + stateDesc(tr("Unknown state")), + mode("MAV_MODE_UNKNOWN"), + thrust(0), + isActive(false), + x(0), + y(0), + z(0), + totalSpeed(0), + lat(0), + lon(0), + alt(0), + groundDistance(0), + localFrame(false), + removeAction(new QAction("Delete this system", this)), + renameAction(new QAction("Rename..", this)), + selectAction(new QAction("Select this system", this )), + selectAirframeAction(new QAction("Select Airframe", this)), + m_ui(new Ui::UASView) +{ + m_ui->setupUi(this); + + // Setup communication + //connect(uas, SIGNAL(valueChanged(int,QString,double,quint64)), this, SLOT(receiveValue(int,QString,double,quint64))); + connect(uas, SIGNAL(batteryChanged(UASInterface*, double, double, int)), this, SLOT(updateBattery(UASInterface*, double, double, int))); + connect(uas, SIGNAL(heartbeat(UASInterface*)), this, SLOT(receiveHeartbeat(UASInterface*))); + connect(uas, SIGNAL(thrustChanged(UASInterface*, double)), this, SLOT(updateThrust(UASInterface*, double))); + connect(uas, SIGNAL(localPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateLocalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(globalPositionChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateGlobalPosition(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(speedChanged(UASInterface*,double,double,double,quint64)), this, SLOT(updateSpeed(UASInterface*,double,double,double,quint64))); + connect(uas, SIGNAL(statusChanged(UASInterface*,QString,QString)), this, SLOT(updateState(UASInterface*,QString,QString))); + connect(uas, SIGNAL(modeChanged(int,QString,QString)), this, SLOT(updateMode(int,QString,QString))); + connect(uas, SIGNAL(loadChanged(UASInterface*, double)), this, SLOT(updateLoad(UASInterface*, double))); + connect(uas, SIGNAL(heartbeatTimeout()), this, SLOT(heartbeatTimeout())); + connect(uas, SIGNAL(waypointSelected(int,int)), this, SLOT(selectWaypoint(int,int))); + connect(uas->getWaypointManager(), SIGNAL(currentWaypointChanged(quint16)), this, SLOT(currentWaypointUpdated(quint16))); + connect(uas, SIGNAL(systemTypeSet(UASInterface*,uint)), this, SLOT(setSystemType(UASInterface*,uint))); + connect(UASManager::instance(), SIGNAL(activeUASStatusChanged(UASInterface*,bool)), this, SLOT(updateActiveUAS(UASInterface*,bool))); + + // Setup UAS selection + connect(m_ui->uasViewFrame, SIGNAL(clicked(bool)), this, SLOT(setUASasActive(bool))); + + // Setup user interaction + connect(m_ui->liftoffButton, SIGNAL(clicked()), uas, SLOT(launch())); + connect(m_ui->haltButton, SIGNAL(clicked()), uas, SLOT(halt())); + connect(m_ui->continueButton, SIGNAL(clicked()), uas, SLOT(go())); + connect(m_ui->landButton, SIGNAL(clicked()), uas, SLOT(home())); + connect(m_ui->abortButton, SIGNAL(clicked()), uas, SLOT(emergencySTOP())); + connect(m_ui->killButton, SIGNAL(clicked()), uas, SLOT(emergencyKILL())); + connect(m_ui->shutdownButton, SIGNAL(clicked()), uas, SLOT(shutdown())); + + // Allow to delete this widget + connect(removeAction, SIGNAL(triggered()), this, SLOT(deleteLater())); + connect(renameAction, SIGNAL(triggered()), this, SLOT(rename())); + connect(selectAction, SIGNAL(triggered()), uas, SLOT(setSelected())); + connect(selectAirframeAction, SIGNAL(triggered()), this, SLOT(selectAirframe())); + connect(uas, SIGNAL(systemRemoved()), this, SLOT(deleteLater())); + + // Name changes + connect(uas, SIGNAL(nameChanged(QString)), this, SLOT(updateName(QString))); + + // Set static values + + // Name + if (uas->getUASName() == "") + { + m_ui->nameLabel->setText(tr("UAS") + QString::number(uas->getUASID())); + } + else + { + m_ui->nameLabel->setText(uas->getUASName()); + } + + setBackgroundColor(); + + // Heartbeat fade + refreshTimer = new QTimer(this); + connect(refreshTimer, SIGNAL(timeout()), this, SLOT(refresh())); + + // Hide kill and shutdown buttons per default + m_ui->killButton->hide(); + m_ui->shutdownButton->hide(); + + setSystemType(uas, uas->getSystemType()); +} + +UASView::~UASView() +{ + delete m_ui; + delete removeAction; + delete renameAction; + delete selectAction; +} + +void UASView::heartbeatTimeout() +{ + timeout = true; +} + +/** + * Set the background color based on the MAV color. If the MAV is selected as the + * currently actively controlled system, the frame color is highlighted + */ +void UASView::setBackgroundColor() +{ + // UAS color + QColor uasColor = uas->getColor(); + QString colorstyle; + QString borderColor = "#4A4A4F"; + if (isActive) + { + borderColor = "#FA4A4F"; + uasColor = uasColor.darker(475); + } + else + { + uasColor = uasColor.darker(675); + } + colorstyle = colorstyle.sprintf("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 2px solid %s; }", + uasColor.red(), uasColor.green(), uasColor.blue(), borderColor.toStdString().c_str()); + m_ui->uasViewFrame->setStyleSheet(colorstyle); +} + +void UASView::setUASasActive(bool active) +{ + if (active) + { + UASManager::instance()->setActiveUAS(this->uas); + } +} + +void UASView::updateActiveUAS(UASInterface* uas, bool active) +{ + if (uas == this->uas) + { + this->isActive = active; + setBackgroundColor(); + } +} + +void UASView::updateMode(int sysId, QString status, QString description) +{ + Q_UNUSED(description); + + int aa=this->uas->getUASID(); + if (sysId == this->uas->getUASID()) m_ui->modeLabel->setText(status); + + m_ui->modeLabel->setText(status); +} + +void UASView::mouseDoubleClickEvent (QMouseEvent * event) +{ + Q_UNUSED(event); + UASManager::instance()->setActiveUAS(uas); + qDebug() << __FILE__ << __LINE__ << "DOUBLECLICKED"; +} + +void UASView::enterEvent(QEvent* event) +{ + if (event->type() == QEvent::MouseMove) + { + emit uasInFocus(uas); + if (uas != UASManager::instance()->getActiveUAS()) + { + grabMouse(QCursor(Qt::PointingHandCursor)); + } + } + qDebug() << __FILE__ << __LINE__ << "IN FOCUS"; + + if (event->type() == QEvent::MouseButtonDblClick) + { + qDebug() << __FILE__ << __LINE__ << "UAS CLICKED!"; + } +} + +void UASView::leaveEvent(QEvent* event) +{ + if (event->type() == QEvent::MouseMove) + { + emit uasOutFocus(uas); + releaseMouse(); + } +} + +void UASView::showEvent(QShowEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event); + refreshTimer->start(updateInterval); +} + +void UASView::hideEvent(QHideEvent* event) +{ + // React only to internal (pre-display) + // events + Q_UNUSED(event); + refreshTimer->stop(); +} + +void UASView::receiveHeartbeat(UASInterface* uas) +{ + Q_UNUSED(uas); + heartbeatColor = QColor(20, 200, 20); + QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }"); + m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name())); + if (timeout) setBackgroundColor(); + timeout = false; +} + +void UASView::updateName(const QString& name) +{ + if (uas) m_ui->nameLabel->setText(name); +} + +/** + * The current system type is represented through the system icon. + * + * @param uas Source system, has to be the same as this->uas + * @param systemType type ID, following the MAVLink system type conventions + * @see http://pixhawk.ethz.ch/software/mavlink + */ +void UASView::setSystemType(UASInterface* uas, unsigned int systemType) +{ + if (uas == this->uas) + { + // Set matching icon + switch (systemType) + { + case 0: + m_ui->typeButton->setIcon(QIcon(":/images/mavs/generic.svg")); + break; + case 1: + m_ui->typeButton->setIcon(QIcon(":/images/mavs/fixed-wing.svg")); + break; + case 2: + m_ui->typeButton->setIcon(QIcon(":/images/mavs/quadrotor.svg")); + break; + case 3: + m_ui->typeButton->setIcon(QIcon(":/images/mavs/coaxial.svg")); + break; + case 4: + m_ui->typeButton->setIcon(QIcon(":/images/mavs/helicopter.svg")); + break; + case 5: + m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg")); + break; + case 6: + { + // A groundstation is a special system type, update widget + QString result; + m_ui->nameLabel->setText(tr("OCU ") + result.sprintf("%03d", uas->getUASID())); + m_ui->waypointLabel->setText(""); + m_ui->timeRemainingLabel->setText("Online:"); + m_ui->batteryBar->hide(); + m_ui->thrustBar->hide(); + m_ui->stateLabel->hide(); + m_ui->statusTextLabel->hide(); + m_ui->waypointLabel->hide(); + m_ui->liftoffButton->hide(); + m_ui->haltButton->hide(); + m_ui->landButton->hide(); + m_ui->shutdownButton->hide(); + m_ui->abortButton->hide(); + m_ui->typeButton->setIcon(QIcon(":/images/mavs/groundstation.svg")); + } + break; + default: + m_ui->typeButton->setIcon(QIcon(":/images/mavs/unknown.svg")); + break; + } + } +} + +void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double z, quint64 usec) +{ + Q_UNUSED(usec); + Q_UNUSED(uas); + this->x = x; + this->y = y; + this->z = z; + if (!localFrame) + { + localFrame = true; + } +} + +void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec) +{ + Q_UNUSED(uas); + Q_UNUSED(usec); + this->lon = lon; + this->lat = lat; + this->alt = alt; +} + +void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec) +{ + Q_UNUSED(usec); + totalSpeed = sqrt((pow(x, 2) + pow(y, 2) + pow(z, 2))); +} + +void UASView::currentWaypointUpdated(quint16 waypoint) +{ + m_ui->waypointLabel->setText(tr("WP") + QString::number(waypoint)); +} + +void UASView::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current) +{ + Q_UNUSED(x); + Q_UNUSED(y); + Q_UNUSED(z); + Q_UNUSED(yaw); + Q_UNUSED(autocontinue); + if (uasId == this->uas->getUASID()) + { + if (current) + { + m_ui->waypointLabel->setText(tr("WP") + QString::number(id)); + } + } +} + +void UASView::selectWaypoint(int uasId, int id) +{ + if (uasId == this->uas->getUASID()) + { + m_ui->waypointLabel->setText(tr("WP") + QString::number(id)); + } +} + +void UASView::updateThrust(UASInterface* uas, double thrust) +{ + if (this->uas == uas) + { + this->thrust = thrust; + } +} + +void UASView::updateBattery(UASInterface* uas, double voltage, double percent, int seconds) +{ + Q_UNUSED(voltage); + if (this->uas == uas) + { + timeRemaining = seconds; + chargeLevel = percent; + } +} + +void UASView::updateState(UASInterface* uas, QString uasState, QString stateDescription) +{ + if (this->uas == uas) + { + state = uasState; + stateDesc = stateDescription; + } +} + +void UASView::updateLoad(UASInterface* uas, double load) +{ + if (this->uas == uas) + { + this->load = load; + } +} + +void UASView::contextMenuEvent (QContextMenuEvent* event) +{ + QMenu menu(this); + menu.addAction(renameAction); + if (timeout) + { + menu.addAction(removeAction); + } + menu.addAction(selectAction); + menu.addAction(selectAirframeAction); + menu.exec(event->globalPos()); +} + +void UASView::rename() +{ + if (uas) + { + bool ok; + QString newName = QInputDialog::getText(this, tr("Rename System %1").arg(uas->getUASName()), + tr("System Name:"), QLineEdit::Normal, + uas->getUASName(), &ok); + + if (ok && !newName.isEmpty()) uas->setUASName(newName); + } +} + +void UASView::selectAirframe() +{ + if (uas) + { + // Get list of airframes from UAS + QStringList airframes; + airframes << "Generic" + << "Multiplex Easystar" + << "Multiplex Twinstar" + << "Multiplex Merlin" + << "Pixhawk Cheetah" + << "Mikrokopter" + << "Reaper" + << "Predator" + << "Coaxial" + << "Pteryx"; + + bool ok; + QString item = QInputDialog::getItem(this, tr("Select Airframe for %1").arg(uas->getUASName()), + tr("Airframe"), airframes, uas->getAirframe(), false, &ok); + if (ok && !item.isEmpty()) + { + // Set this airframe as UAS airframe + uas->setAirframe(airframes.indexOf(item)); + } + } +} + +void UASView::refresh() +{ + //setUpdatesEnabled(false); + //setUpdatesEnabled(true); + //repaint(); + + static quint64 lastupdate = 0; + //qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate; + lastupdate = MG::TIME::getGroundTimeNow(); + + // FIXME + static int generalUpdateCount = 0; + + if (generalUpdateCount == 4) + { +#if (QGC_EVENTLOOP_DEBUG) + qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__; +#endif + generalUpdateCount = 0; + //qDebug() << "UPDATING EVERYTHING"; + // State + m_ui->stateLabel->setText(state); + m_ui->statusTextLabel->setText(stateDesc); + + // Battery + m_ui->batteryBar->setValue(static_cast(this->chargeLevel)); + //m_ui->loadBar->setValue(static_cast(this->load)); + m_ui->thrustBar->setValue(this->thrust); + + // Position + QString position; + position = position.sprintf("%05.1f %05.1f %06.1f m", x, y, z); + m_ui->positionLabel->setText(position); + QString globalPosition; + QString latIndicator; + if (lat > 0) + { + latIndicator = "N"; + } + else + { + latIndicator = "S"; + } + QString lonIndicator; + if (lon > 0) + { + lonIndicator = "E"; + } + else + { + lonIndicator = "W"; + } + globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %06.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt); + m_ui->positionLabel->setText(globalPosition); + + // Altitude + if (groundDistance == 0 && alt != 0) + { + m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 6, 'f', 1, '0')); + } + else + { + m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 6, 'f', 1, '0')); + } + + // Speed + QString speed("%1 m/s"); + m_ui->speedLabel->setText(speed.arg(totalSpeed, 4, 'f', 1, '0')); + + // Thrust + m_ui->thrustBar->setValue(thrust * 100); + + if(this->timeRemaining > 1 && this->timeRemaining < QGC::MAX_FLIGHT_TIME) + { + // Filter output to get a higher stability + static double filterTime = static_cast(this->timeRemaining); + filterTime = 0.8 * filterTime + 0.2 * static_cast(this->timeRemaining); + int sec = static_cast(filterTime - static_cast(filterTime / 60.0f) * 60); + int min = static_cast(filterTime / 60); + int hours = static_cast(filterTime - min * 60 - sec); + + QString timeText; + timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); + m_ui->timeRemainingLabel->setText(timeText); + } + else + { + m_ui->timeRemainingLabel->setText(tr("Calc..")); + } + + // Time Elapsed + //QDateTime time = MG::TIME::msecToQDateTime(uas->getUptime()); + + quint64 filterTime = uas->getUptime() / 1000; + int sec = static_cast(filterTime - static_cast(filterTime / 60) * 60); + int min = static_cast(filterTime / 60); + int hours = static_cast(filterTime - min * 60 - sec); + QString timeText; + timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec); + m_ui->timeElapsedLabel->setText(timeText); + } + generalUpdateCount++; + + QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 2px; border: 0px; background-color: %1; }"); + + if (timeout) + { + // CRITICAL CONDITION, NO HEARTBEAT + + QString borderColor = "#FFFF00"; + if (isActive) + { + borderColor = "#FA4A4F"; + } + + if (iconIsRed) + { + QColor warnColor(Qt::red); + m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name())); + QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name()); + m_ui->uasViewFrame->setStyleSheet(style); + } + else + { + QColor warnColor(Qt::black); + m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name())); + QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name()); + m_ui->uasViewFrame->setStyleSheet(style); + } + iconIsRed = !iconIsRed; + } + else + { + // Fade heartbeat icon + // Make color darker + heartbeatColor = heartbeatColor.darker(150); + + //m_ui->heartbeatIcon->setAutoFillBackground(true); + m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name())); + } + //setUpdatesEnabled(true); + + //setUpdatesEnabled(false); +} + +void UASView::changeEvent(QEvent *e) +{ + QWidget::changeEvent(e); + switch (e->type()) { + case QEvent::LanguageChange: + m_ui->retranslateUi(this); + break; + default: + break; + } +}