diff --git a/files/styles/style-dark.css b/files/styles/style-dark.css index a0b8d910877f0e4086e73b63a151335ec099cf0d..0818591a8e4f01b2b6132708d22601f4ae43895a 100644 --- a/files/styles/style-dark.css +++ b/files/styles/style-dark.css @@ -249,7 +249,12 @@ QLabel#noUas { } QMessageBox QLabel { - font-size: 12pt; + font-size: 14pt; +} + +QMessageBox { + min-width: 400px; + min-height: 300px; } QLabel#calibrationExplanationLabel { diff --git a/src/comm/MAVLinkProtocol.cc b/src/comm/MAVLinkProtocol.cc index 8684e0a6687ee516b42e7e306c89c1af4c1f4997..3a24378912923812b12aec1026dbf07b597ca857 100644 --- a/src/comm/MAVLinkProtocol.cc +++ b/src/comm/MAVLinkProtocol.cc @@ -250,9 +250,9 @@ void MAVLinkProtocol::receiveBytes(LinkInterface* link, QByteArray b) if (decodeState == 0 && !decodedFirstPacket) { nonmavlinkCount++; - if (nonmavlinkCount > 500 && !warnedUserNonMavlink) + if (nonmavlinkCount > 2000 && !warnedUserNonMavlink) { - //500 bytes with no mavlink message. Are we connected to a mavlink capable device? + //2000 bytes with no mavlink message. Are we connected to a mavlink capable device? if (!checkedUserNonMavlink) { link->requestReset(); diff --git a/src/comm/SerialLink.cc b/src/comm/SerialLink.cc index 1fcb664f1194bc3cd314cf2e528a3f52bfb525e6..3af32a6b813e027ec03fbbd0f73749f5dda2daef 100644 --- a/src/comm/SerialLink.cc +++ b/src/comm/SerialLink.cc @@ -183,7 +183,7 @@ void SerialLink::run() if (m_transmitBuffer.count() > 0) { QMutexLocker writeLocker(&m_writeMutex); int numWritten = m_port->write(m_transmitBuffer); - bool txSuccess = m_port->waitForBytesWritten(1); + bool txSuccess = m_port->waitForBytesWritten(5); if (!txSuccess || (numWritten != m_transmitBuffer.count())) { linkErrorCount++; qDebug() << "TX Error! wrote" << numWritten << ", asked for " << m_transmitBuffer.count() << "bytes"; diff --git a/src/ui/QGCPX4VehicleConfig.cc b/src/ui/QGCPX4VehicleConfig.cc index 593f410ed3192f64ccd6ff01f74b533eea6d70e4..cf71c3cdcfd5d77199986de9cc6059c056cdfe2e 100644 --- a/src/ui/QGCPX4VehicleConfig.cc +++ b/src/ui/QGCPX4VehicleConfig.cc @@ -1182,12 +1182,11 @@ void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float fval) // Raw value float mappedVal = rcMappedValue[rcToFunctionMapping[chan]]; - bool isMapped = (((float)UINT16_MAX) != mappedVal); + float deltaRaw = fabsf(fval - rcValue[chan]); float delta = fabsf(fval - mappedVal); if (!configEnabled && !calibrationEnabled && - (!isMapped || - (delta < 12.0f && rcValue[chan] > 800 && rcValue[chan] < 2200) ) - ) { + (deltaRaw < 12.0f && delta < 12.0f && rcValue[chan] > 800 && rcValue[chan] < 2200)) + { //ignore tiny jitter values return; } diff --git a/src/ui/QGCPX4VehicleConfig.ui b/src/ui/QGCPX4VehicleConfig.ui index a9f062e9cf22592f45abe67ae662656e69edd7e4..7e258f4cd45351ed7822322dbb6fce4ac4cc6651 100644 --- a/src/ui/QGCPX4VehicleConfig.ui +++ b/src/ui/QGCPX4VehicleConfig.ui @@ -798,40 +798,6 @@ - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - 13 - 50 - false - - - - Load Platform Defaults - - - - - @@ -853,8 +819,8 @@ 0 0 - 98 - 28 + 492 + 828 @@ -890,8 +856,8 @@ 0 0 - 98 - 28 + 491 + 828 @@ -914,33 +880,6 @@ - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - Load Platform Defaults - - - - - diff --git a/src/ui/px4_configuration/QGCPX4AirframeConfig.cc b/src/ui/px4_configuration/QGCPX4AirframeConfig.cc index 82694e6b820dd7f800594683621b4dbcd81eee27..4f3ecd2b513be1ba2b522017b250c89638ec5236 100644 --- a/src/ui/px4_configuration/QGCPX4AirframeConfig.cc +++ b/src/ui/px4_configuration/QGCPX4AirframeConfig.cc @@ -1,5 +1,7 @@ #include +#include #include +#include #include "QGCPX4AirframeConfig.h" #include "ui_QGCPX4AirframeConfig.h" @@ -7,10 +9,14 @@ #include "UASManager.h" #include "LinkManager.h" #include "UAS.h" +#include "QGC.h" QGCPX4AirframeConfig::QGCPX4AirframeConfig(QWidget *parent) : QWidget(parent), mav(NULL), + progress(NULL), + pendingParams(0), + configState(CONFIG_STATE_ABORT), selectedId(-1), ui(new Ui::QGCPX4AirframeConfig) { @@ -75,6 +81,7 @@ void QGCPX4AirframeConfig::parameterChanged(int uas, int component, QString para setAirframeID(index); ui->statusLabel->setText(tr("Onboard start script ID: #%1").arg(index)); } else { + uncheckAll(); ui->statusLabel->setText(tr("System not configured for autostart.")); } } @@ -171,12 +178,14 @@ void QGCPX4AirframeConfig::applyAndReboot() if (paramMgr->countOnboardParams() == 0 && paramMgr->countPendingParams() == 0) { - paramMgr->requestParameterListIfEmpty(); - QGC::SLEEP::msleep(100); + paramMgr->requestParameterList(); + QGC::SLEEP::msleep(300); } + QList components = paramMgr->getComponentForParam("SYS_AUTOSTART"); + // Guard against the case of an edit where we didn't receive all params yet - if (paramMgr->countPendingParams() > 0) + if (paramMgr->countPendingParams() > 0 || components.count() == 0) { QMessageBox msgBox; msgBox.setText(tr("Parameter sync with UAS not yet complete")); @@ -188,8 +197,6 @@ void QGCPX4AirframeConfig::applyAndReboot() return; } - QList components = paramMgr->getComponentForParam("SYS_AUTOSTART"); - // Guard against multiple components responding - this will never show in practice if (components.count() != 1) { QMessageBox msgBox; @@ -202,32 +209,137 @@ void QGCPX4AirframeConfig::applyAndReboot() return; } - qDebug() << "Setting comp" << components.first() << "SYS_AUTOSTART" << (qint32)selectedId; - - paramMgr->setPendingParam(components.first(),"SYS_AUTOSTART", (qint32)selectedId); - - //need to set autoconfig in order for PX4 to pick up the selected airframe params - if (ui->defaultGainsCheckBox->checkState() == Qt::Checked) - setAutoConfig(true); - - // Send pending params and then write them to persistent storage when done - paramMgr->sendPendingParameters(true); - - // Reboot - //TODO right now this relies upon the above send & persist finishing before the reboot command is received... - QMessageBox msgBox(this); - msgBox.setText(tr("Storing Parameters and Rebooting Autopilot")); - msgBox.setInformativeText(tr("Please wait a few seconds for the reboot to complete.")); - msgBox.setStandardButtons(QMessageBox::NoButton); - msgBox.setModal(false); - msgBox.show(); - QGC::SLEEP::sleep(2); - mav->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); - QGC::SLEEP::msleep(200); - LinkManager::instance()->disconnectAll(); - QGC::SLEEP::sleep(8); - LinkManager::instance()->connectAll(); - msgBox.close(); + // This is really evil: 'fake' a thread by + // periodic work queue calls and clock + // through a small state machine + // ugh.. if we just had time to do this properly. + + // To the reader who can't program and wants to whine: + // this is not beautiful, but technically completely + // sound. If you want to fix it, you'd be welcome + // to rebase the link, param manager and UI classes + // on a proper threading framework - which I'd love to do + // if I just had more time.. + + configState = CONFIG_STATE_SEND; + QTimer::singleShot(200, this, SLOT(checkConfigState())); +} + +void QGCPX4AirframeConfig::checkConfigState() +{ + + if (configState == CONFIG_STATE_SEND) + { + QList components = paramMgr->getComponentForParam("SYS_AUTOSTART"); + qDebug() << "Setting comp" << components.first() << "SYS_AUTOSTART" << (qint32)selectedId; + + paramMgr->setPendingParam(components.first(),"SYS_AUTOSTART", (qint32)selectedId); + + //need to set autoconfig in order for PX4 to pick up the selected airframe params + if (ui->defaultGainsCheckBox->checkState() == Qt::Checked) + setAutoConfig(true); + + // Send pending params and then write them to persistent storage when done + paramMgr->sendPendingParameters(true); + + configState = CONFIG_STATE_WAIT_PENDING; + pendingParams = 0; + QTimer::singleShot(2000, this, SLOT(checkConfigState())); + return; + } + + if (configState == CONFIG_STATE_WAIT_PENDING) { + // Guard against the case of an edit where we didn't receive all params yet + if (paramMgr->countPendingParams() > 0) + { + if (pendingParams == 0) { + + pendingParams = paramMgr->countPendingParams(); + + if (progress) + delete progress; + + progress = new QProgressDialog("Writing parameters", "Abort Send", 0, pendingParams, this); + progress->setWindowModality(Qt::WindowModal); + progress->setMinimumDuration(2000); + } + + qDebug() << "PENDING" << paramMgr->countPendingParams() << "PROGRESS" << pendingParams - paramMgr->countPendingParams(); + progress->setValue(pendingParams - paramMgr->countPendingParams()); + + if (progress->wasCanceled()) { + configState = CONFIG_STATE_ABORT; + pendingParams = 0; + return; + } + } else { + pendingParams = 0; + configState = CONFIG_STATE_REBOOT; + } + + qDebug() << "PENDING PARAMS WAIT PENDING: " << paramMgr->countPendingParams(); + QTimer::singleShot(1000, this, SLOT(checkConfigState())); + return; + } + + if (configState == CONFIG_STATE_REBOOT) { + + // Reboot + //TODO right now this relies upon the above send & persist finishing before the reboot command is received... + + unsigned pendingMax = 20; + + qDebug() << "PENDING PARAMS REBOOT BEFORE" << pendingParams; + + if (pendingParams == 0) { + pendingParams = 1; + + if (progress) + delete progress; + + progress = new QProgressDialog("Waiting for autopilot reboot", "Abort", 0, pendingMax, this); + progress->setWindowModality(Qt::WindowModal); + qDebug() << "Waiting for reboot, pending" << pendingParams; + } + + if (pendingParams == 3) { + qDebug() << "REQUESTING REBOOT"; + mav->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); + mav->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); + } + + if (pendingParams == 4) { + qDebug() << "DISCONNECT AIRFRAME"; + LinkManager::instance()->disconnectAll(); + } + + if (pendingParams == 14) { + qDebug() << "CONNECT AIRFRAME"; + LinkManager::instance()->connectAll(); + } + if (pendingParams == 15) { + qDebug() << "DISCONNECT AIRFRAME"; + LinkManager::instance()->disconnectAll(); + } + if (pendingParams == 16) { + qDebug() << "CONNECT AIRFRAME"; + LinkManager::instance()->connectAll(); + } + + if (pendingParams < pendingMax) { + progress->setValue(pendingParams); + QTimer::singleShot(1000, this, SLOT(checkConfigState())); + } else { + paramMgr->requestParameterList(); + progress->setValue(pendingMax); + configState = CONFIG_STATE_ABORT; + pendingParams = 0; + return; + } + qDebug() << "PENDING PARAMS REBOOT AFTER:" << pendingParams; + pendingParams++; + return; + } } void QGCPX4AirframeConfig::setAutoConfig(bool enabled) diff --git a/src/ui/px4_configuration/QGCPX4AirframeConfig.h b/src/ui/px4_configuration/QGCPX4AirframeConfig.h index 346076e1849ee3e3324e52085149c3d9bb2acbaa..dcbf4abc142faa3349eb7cc153e5a70149a5512d 100644 --- a/src/ui/px4_configuration/QGCPX4AirframeConfig.h +++ b/src/ui/px4_configuration/QGCPX4AirframeConfig.h @@ -4,6 +4,8 @@ #include #include +class QProgressDialog; + namespace Ui { class QGCPX4AirframeConfig; } @@ -67,6 +69,9 @@ public slots: */ void applyAndReboot(); +protected slots: + void checkConfigState(); + protected: /** @@ -83,9 +88,19 @@ protected: void uncheckAll(); + enum CONFIG_STATE { + CONFIG_STATE_ABORT = 0, + CONFIG_STATE_SEND, + CONFIG_STATE_WAIT_PENDING, + CONFIG_STATE_REBOOT + }; + private: UASInterface* mav; QGCUASParamManager *paramMgr; + QProgressDialog* progress; + unsigned pendingParams; + enum CONFIG_STATE configState; int selectedId; Ui::QGCPX4AirframeConfig *ui; };