diff --git a/src/PX4FirmwareUpgradeWorker.cc b/src/PX4FirmwareUpgradeWorker.cc index 461b82d7eff7831e5417c6292ef5ae3a016b1597..b4a6755dec682c9229ffcb29872040c97654cd8d 100644 --- a/src/PX4FirmwareUpgradeWorker.cc +++ b/src/PX4FirmwareUpgradeWorker.cc @@ -1,3 +1,6 @@ +#include +#include + #include "PX4FirmwareUpgradeWorker.h" #include @@ -49,21 +52,28 @@ PX4FirmwareUpgradeWorker* PX4FirmwareUpgradeWorker::putWorkerInThread(QObject *p // Starts an event loop, and emits workerThread->started() workerThread->start(); + return worker; } -bool PX4FirmwareUpgradeWorker::startContinousScan() +void PX4FirmwareUpgradeWorker::startContinousScan() { - while (true) { - if (detect()) { - break; - } + exitThread = false; + while (!exitThread) { +// if (detect()) { +// break; +// } + QGC::SLEEP::msleep(20); } - return true; + if (exitThread) { + link->disconnect(); + delete link; + exit(0); + } } -bool PX4FirmwareUpgradeWorker::detect() +void PX4FirmwareUpgradeWorker::detect() { if (!link) { @@ -80,7 +90,7 @@ bool PX4FirmwareUpgradeWorker::detect() // Ignore known wrong link names if (ports->at(i).contains("Bluetooth")) { - continue; + //continue; } link->setPortName(ports->at(i)); @@ -101,12 +111,6 @@ bool PX4FirmwareUpgradeWorker::detect() break; } - if (insync) { - return true; - } else { - return false; - } - //ui.portName->setCurrentIndex(ui.baudRate->findText(QString("%1").arg(this->link->getPortName()))); // Set port @@ -119,11 +123,15 @@ void PX4FirmwareUpgradeWorker::receiveBytes(LinkInterface* link, QByteArray b) { for (int position = 0; position < b.size(); position++) { qDebug() << "BYTES"; - qDebug() << std::hex << (char)(b[position]); + qDebug() << (char)(b[position]); if (((const char)b[position]) == INSYNC) { qDebug() << "SYNC"; insync = true; + } + + if (insync && ((const char)b[position]) == OK) + { emit detectionStatusChanged("Found PX4 board"); } } @@ -131,7 +139,31 @@ void PX4FirmwareUpgradeWorker::receiveBytes(LinkInterface* link, QByteArray b) printf("\n"); } -bool PX4FirmwareUpgradeWorker::upgrade(const QString &filename) +void PX4FirmwareUpgradeWorker::loadFirmware(const QString &filename) +{ + qDebug() << __FILE__ << __LINE__ << "LOADING FW"; + QFile f(filename); + if (f.open(QIODevice::ReadOnly)) + { + QByteArray buf = f.readAll(); + f.close(); + firmware = QJsonDocument::fromBinaryData(buf); + if (firmware.isNull()) { + emit upgradeStatusChanged(tr("Failed decoding file %1").arg(filename)); + } else { + emit upgradeStatusChanged(tr("Ready to flash %1").arg(filename)); + } + } else { + emit upgradeStatusChanged(tr("Failed opening file %1").arg(filename)); + } +} + +void PX4FirmwareUpgradeWorker::upgrade() { + emit upgradeStatusChanged(tr("Starting firmware upgrade..")); +} +void PX4FirmwareUpgradeWorker::abort() +{ + exitThread = true; } diff --git a/src/PX4FirmwareUpgradeWorker.h b/src/PX4FirmwareUpgradeWorker.h index 88c1acda3dca866fa968af6c0f7ed11d9abff6af..873eadc357f2ccf139b8916e8204e9b1a0bb973f 100644 --- a/src/PX4FirmwareUpgradeWorker.h +++ b/src/PX4FirmwareUpgradeWorker.h @@ -2,6 +2,7 @@ #define PX4FIRMWAREUPGRADEWORKER_H #include +#include #include @@ -24,7 +25,7 @@ public slots: * @brief Continously scan for bootloaders * @return */ - bool startContinousScan(); + void startContinousScan(); /** * @brief Detect connected PX4 bootloaders @@ -34,14 +35,19 @@ public slots: * * @return true if found on one link, false else */ - bool detect(); + void detect(); /** * @brief Upgrade the firmware using the currently connected link * @param filename file name / path of the firmware file - * @return true if upgrade succeeds, false else */ - bool upgrade(const QString &filename); + void upgrade(); + + /** + * @brief Load firmware from disk into memory + * @param filename + */ + void loadFirmware(const QString &filename); /** * @brief Receive bytes from a link @@ -50,9 +56,18 @@ public slots: */ void receiveBytes(LinkInterface* link, QByteArray b); + /** + * @brief Abort upgrade worker + */ + void abort(); + +protected: + bool exitThread; + private: SerialLink *link; bool insync; + QJsonDocument firmware; }; #endif // PX4FIRMWAREUPGRADEWORKER_H diff --git a/src/apps/qupgrade/QUpgradeApp.cc b/src/apps/qupgrade/QUpgradeApp.cc index 1ca955b3ac9c4202e8e80a2caf06f847eada1412..558845aeb8819bb234d054db7306c8f5f393aa38 100644 --- a/src/apps/qupgrade/QUpgradeApp.cc +++ b/src/apps/qupgrade/QUpgradeApp.cc @@ -72,13 +72,21 @@ QUpgradeApp::QUpgradeApp(int &argc, char* argv[]) : QApplication(argc, argv) // Create main window QUpgradeMainWindow* window = new QUpgradeMainWindow(); + PX4FirmwareUpgrader *upgrader = new PX4FirmwareUpgrader(window); + window->setCentralWidget(upgrader); // Get PX4 upgrade widget and instantiate worker thread PX4FirmwareUpgradeWorker* worker = PX4FirmwareUpgradeWorker::putWorkerInThread(this); - connect(worker, SIGNAL(detectionStatusChanged(QString)), window->firmwareUpgrader(), SLOT(setDetectionStatusText(QString))); - connect(worker, SIGNAL(upgradeStatusChanged(QString)), window->firmwareUpgrader(), SLOT(setFlashStatusText(QString))); - connect(worker, SIGNAL(upgradeProgressChanged(int)), window->firmwareUpgrader(), SLOT(setFlashProgress(int))); - connect(worker, SIGNAL(validPortFound(QString)), window->firmwareUpgrader(), SLOT(setPortName(QString))); + + connect(worker, SIGNAL(detectionStatusChanged(QString)), upgrader, SLOT(setDetectionStatusText(QString)), Qt::QueuedConnection); + connect(worker, SIGNAL(upgradeStatusChanged(QString)), upgrader, SLOT(setFlashStatusText(QString)), Qt::QueuedConnection); + connect(worker, SIGNAL(upgradeProgressChanged(int)), upgrader, SLOT(setFlashProgress(int)), Qt::QueuedConnection); + connect(worker, SIGNAL(validPortFound(QString)), upgrader, SLOT(setPortName(QString))); + connect(upgrader, SIGNAL(firmwareFileNameSet(QString)), worker, SLOT(loadFirmware(QString)), Qt::QueuedConnection); + connect(upgrader, SIGNAL(upgrade()), worker, SLOT(upgrade()), Qt::QueuedConnection); + connect(this, SIGNAL(lastWindowClosed()), worker, SLOT(abort()), Qt::QueuedConnection); + + worker->loadFirmware("abc"); window->setWindowTitle(applicationName() + " " + applicationVersion()); window->show(); diff --git a/src/apps/qupgrade/QUpgradeMainWindow.cc b/src/apps/qupgrade/QUpgradeMainWindow.cc index ad0bb2d186b5b027672e0c715ca637b96c015ed1..fea3841df3082351e327665584aff0caa9e3fbad 100644 --- a/src/apps/qupgrade/QUpgradeMainWindow.cc +++ b/src/apps/qupgrade/QUpgradeMainWindow.cc @@ -42,10 +42,6 @@ QUpgradeMainWindow::QUpgradeMainWindow(QWidget *parent) : ui->setupUi(this); } -PX4FirmwareUpgrader* QUpgradeMainWindow::firmwareUpgrader() { - return ui->centralwidget; -} - QUpgradeMainWindow::~QUpgradeMainWindow() { delete ui; diff --git a/src/apps/qupgrade/QUpgradeMainWindow.h b/src/apps/qupgrade/QUpgradeMainWindow.h index dbb523779376effbf358151189a31bfe49164e01..c012600dc6b3bb1e6cf8f63bdfe24f3e55b1ffa0 100644 --- a/src/apps/qupgrade/QUpgradeMainWindow.h +++ b/src/apps/qupgrade/QUpgradeMainWindow.h @@ -48,8 +48,6 @@ public: explicit QUpgradeMainWindow(QWidget *parent = 0); ~QUpgradeMainWindow(); - PX4FirmwareUpgrader* firmwareUpgrader(); - public slots: protected: diff --git a/src/apps/qupgrade/QUpgradeMainWindow.ui b/src/apps/qupgrade/QUpgradeMainWindow.ui index 75f16d006b14cb3624646d56886aee5f9e741a00..5c806bbfffa382604cc10da748d05a0ab6712e19 100644 --- a/src/apps/qupgrade/QUpgradeMainWindow.ui +++ b/src/apps/qupgrade/QUpgradeMainWindow.ui @@ -13,7 +13,7 @@ MainWindow - + @@ -28,14 +28,6 @@ - - - PX4FirmwareUpgrader - QWidget -
PX4FirmwareUpgrader.h
- 1 - - diff --git a/src/comm/QGCXPlaneLink.cc b/src/comm/QGCXPlaneLink.cc index 1657152915be7ff4417f684c20420a1188af22d5..521beb5b6092b00da986324a3fa1f652f37a96fb 100644 --- a/src/comm/QGCXPlaneLink.cc +++ b/src/comm/QGCXPlaneLink.cc @@ -49,7 +49,10 @@ QGCXPlaneLink::QGCXPlaneLink(UASInterface* mav, QString remoteHost, QHostAddress terraSync(NULL), airframeID(QGCXPlaneLink::AIRFRAME_UNKNOWN), xPlaneConnected(false), - xPlaneVersion(0) + xPlaneVersion(0), + simUpdateLast(QGC::groundTimeMilliseconds()), + simUpdateLastText(QGC::groundTimeMilliseconds()), + simUpdateHz(0) { this->localHost = localHost; this->localPort = localPort/*+mav->getUASID()*/; @@ -374,7 +377,7 @@ void QGCXPlaneLink::readBytes() // Only emit updates on attitude message bool emitUpdate = false; - const qint64 maxLength = 65536; + const qint64 maxLength = 1000; char data[maxLength]; QHostAddress sender; quint16 senderPort; @@ -509,8 +512,18 @@ void QGCXPlaneLink::readBytes() } // Send updated state - if (emitUpdate) + if (emitUpdate && (QGC::groundTimeMilliseconds() - simUpdateLast) > 3) { + simUpdateHz = simUpdateHz * 0.9f + 0.1f * (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); + if (QGC::groundTimeMilliseconds() - simUpdateLastText > 2000) { + emit statusMessage(tr("Receiving from XPlane at %1 Hz").arg(static_cast(simUpdateHz))); + // Reset lowpass with current value + simUpdateHz = (1000.0f / (QGC::groundTimeMilliseconds() - simUpdateLast)); + // Set state + simUpdateLastText = QGC::groundTimeMilliseconds(); + } + simUpdateLast = QGC::groundTimeMilliseconds(); + emit hilStateChanged(QGC::groundTimeUsecs(), roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat*1E7, lon*1E7, alt*1E3, vx, vy, vz, xacc*1000, yacc*1000, zacc*1000); @@ -518,7 +531,7 @@ void QGCXPlaneLink::readBytes() if (!oldConnectionState && xPlaneConnected) { - emit statusMessage("Receiving from XPlane."); + emit statusMessage(tr("Receiving from XPlane.")); } // // Echo data for debugging purposes diff --git a/src/comm/QGCXPlaneLink.h b/src/comm/QGCXPlaneLink.h index e389b7b8f2669f9f45ae0d0d6a7068e32bffe6aa..d89e98763a1ec68fec527c1c763e8197540e1780 100644 --- a/src/comm/QGCXPlaneLink.h +++ b/src/comm/QGCXPlaneLink.h @@ -190,6 +190,9 @@ protected: enum AIRFRAME airframeID; bool xPlaneConnected; unsigned int xPlaneVersion; + quint64 simUpdateLast; + quint64 simUpdateLastText; + float simUpdateHz; void setName(QString name); diff --git a/src/ui/PX4FirmwareUpgrader.cc b/src/ui/PX4FirmwareUpgrader.cc index 5d9fabff88dc60ec418b4263702e1b6851c43578..c2ae3fae80cee52f1bab56edc0ee55a8e5f99482 100644 --- a/src/ui/PX4FirmwareUpgrader.cc +++ b/src/ui/PX4FirmwareUpgrader.cc @@ -1,9 +1,14 @@ +#include +#include +#include + #include "PX4FirmwareUpgrader.h" #include "ui_PX4FirmwareUpgrader.h" #include #include + PX4FirmwareUpgrader::PX4FirmwareUpgrader(QWidget *parent) : QWidget(parent), ui(new Ui::PX4FirmwareUpgrader) @@ -21,7 +26,16 @@ PX4FirmwareUpgrader::~PX4FirmwareUpgrader() void PX4FirmwareUpgrader::selectFirmwareFile() { - + QSettings settings; + QString path = settings.value("PX4_FIRMWARE_PATH", + QDesktopServices::storageLocation(QDesktopServices::DesktopLocation)).toString(); + const QString widgetFileExtension(".px4"); + QString fileName = QFileDialog::getOpenFileName(this, tr("Specify File Name"), + path, + tr("PX4 Firmware (*%1);;").arg(widgetFileExtension)); + settings.setValue("PX4_FIRMWARE_PATH", fileName); + qDebug() << "EMITTING SIGNAL"; + emit firmwareFileNameSet(fileName); } void PX4FirmwareUpgrader::setDetectionStatusText(const QString &text) @@ -32,6 +46,7 @@ void PX4FirmwareUpgrader::setDetectionStatusText(const QString &text) void PX4FirmwareUpgrader::setFlashStatusText(const QString &text) { ui->flashProgressLabel->setText(text); + qDebug() << __FILE__ << __LINE__ << "LABEL" << text; } void PX4FirmwareUpgrader::setFlashProgress(int percent) diff --git a/src/ui/QGCHilConfiguration.cc b/src/ui/QGCHilConfiguration.cc index a83397ce172f2c2fd71edde401a633115861d9f7..d6253cefbc4f6f55da997e684b3bb2d4a9778b74 100644 --- a/src/ui/QGCHilConfiguration.cc +++ b/src/ui/QGCHilConfiguration.cc @@ -30,7 +30,12 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index) mav->enableHilFlightGear(false, ""); QGCHilFlightGearConfiguration* hfgconf = new QGCHilFlightGearConfiguration(mav, this); hfgconf->show(); - ui->simulatorConfigurationDockWidget->setWidget(hfgconf); + ui->simulatorConfigurationLayout->addWidget(hfgconf); + QGCFlightGearLink* fg = dynamic_cast(mav->getHILSimulation()); + if (fg) + { + connect(fg, SIGNAL(statusMessage(QString)), ui->statusLabel, SLOT(setText(QString))); + } } else if(2 == index || 3 == index) @@ -39,13 +44,14 @@ void QGCHilConfiguration::on_simComboBox_currentIndexChanged(int index) mav->enableHilXPlane(false); QGCHilXPlaneConfiguration* hxpconf = new QGCHilXPlaneConfiguration(mav->getHILSimulation(), this); hxpconf->show(); - ui->simulatorConfigurationDockWidget->setWidget(hxpconf); + ui->simulatorConfigurationLayout->addWidget(hxpconf); // Select correct version of XPlane QGCXPlaneLink* xplane = dynamic_cast(mav->getHILSimulation()); if (xplane) { xplane->setVersion((index == 2) ? 10 : 9); + connect(xplane, SIGNAL(statusMessage(QString)), ui->statusLabel, SLOT(setText(QString))); } } } diff --git a/src/ui/QGCHilConfiguration.ui b/src/ui/QGCHilConfiguration.ui index ace3110769acf16b2497c253695458ed782390c0..da8adaf2769fecdc06e15c4dcc803e2c33fa4a01 100644 --- a/src/ui/QGCHilConfiguration.ui +++ b/src/ui/QGCHilConfiguration.ui @@ -6,8 +6,8 @@ 0 0 - 305 - 355 + 366 + 301 @@ -19,22 +19,22 @@ Form - - - + + + - Simulator + Status - - + + - Status + Simulator - + true @@ -61,35 +61,12 @@ - - - - - 0 - 0 - - - - - 20 - 150 - - - - QDockWidget::NoDockWidgetFeatures + + + + 0 - - Simulator Options - - - - - 0 - 0 - - - - + diff --git a/src/ui/QGCVehicleConfig.cc b/src/ui/QGCVehicleConfig.cc index 318ef8d756329efccc9165f47564ee2f618d5a73..39c421fcb3f9ca8fa29d4989f687b2e177a1e59c 100644 --- a/src/ui/QGCVehicleConfig.cc +++ b/src/ui/QGCVehicleConfig.cc @@ -11,6 +11,7 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) : QWidget(parent), mav(NULL), + chanCount(0), changed(true), rc_mode(RC_MODE_2), rcRoll(0.0f), @@ -21,6 +22,7 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) : rcAux1(0.0f), rcAux2(0.0f), rcAux3(0.0f), + calibrationEnabled(false), ui(new Ui::QGCVehicleConfig) { setObjectName("QGC_VEHICLECONFIG"); @@ -31,9 +33,31 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) : ui->rcModeComboBox->setCurrentIndex((int)rc_mode - 1); + ui->rcCalibrationButton->setCheckable(true); connect(ui->rcCalibrationButton, SIGNAL(clicked(bool)), this, SLOT(toggleCalibrationRC(bool))); connect(ui->storeButton, SIGNAL(clicked()), this, SLOT(writeParameters())); connect(ui->rcModeComboBox, SIGNAL(currentIndexChanged(int)), this, SLOT(setRCModeIndex(int))); + connect(ui->setTrimButton, SIGNAL(clicked()), this, SLOT(setTrimPositions())); + + /* Connect RC mapping assignments */ + connect(ui->rollSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setRollChan(int))); + connect(ui->pitchSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setPitchChan(int))); + connect(ui->yawSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setYawChan(int))); + connect(ui->throttleSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setThrottleChan(int))); + connect(ui->modeSpinBox, SIGNAL(valueChanged(int)), this, SLOT(setModeChan(int))); + connect(ui->aux1SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux1Chan(int))); + connect(ui->aux2SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux2Chan(int))); + connect(ui->aux3SpinBox, SIGNAL(valueChanged(int)), this, SLOT(setAux3Chan(int))); + + // Connect RC reverse assignments + connect(ui->invertCheckBox, SIGNAL(clicked(bool)), this, SLOT(setRollInverted(bool))); + connect(ui->invertCheckBox_2, SIGNAL(clicked(bool)), this, SLOT(setPitchInverted(bool))); + connect(ui->invertCheckBox_3, SIGNAL(clicked(bool)), this, SLOT(setYawInverted(bool))); + connect(ui->invertCheckBox_4, SIGNAL(clicked(bool)), this, SLOT(setThrottleInverted(bool))); + connect(ui->invertCheckBox_5, SIGNAL(clicked(bool)), this, SLOT(setModeInverted(bool))); + connect(ui->invertCheckBox_6, SIGNAL(clicked(bool)), this, SLOT(setAux1Inverted(bool))); + connect(ui->invertCheckBox_7, SIGNAL(clicked(bool)), this, SLOT(setAux2Inverted(bool))); + connect(ui->invertCheckBox_8, SIGNAL(clicked(bool)), this, SLOT(setAux3Inverted(bool))); connect(UASManager::instance(), SIGNAL(activeUASSet(UASInterface*)), this, SLOT(setActiveUAS(UASInterface*))); @@ -41,7 +65,7 @@ QGCVehicleConfig::QGCVehicleConfig(QWidget *parent) : for (unsigned int i = 0; i < chanMax; i++) { - rcValue[i] = 1500; + rcValue[i] = UINT16_MAX; } updateTimer.setInterval(150); @@ -75,15 +99,35 @@ void QGCVehicleConfig::toggleCalibrationRC(bool enabled) } } +void QGCVehicleConfig::setTrimPositions() +{ + // Set trim for roll, pitch, yaw, throttle + rcTrim[rcMapping[0]] = rcValue[0]; // roll + rcTrim[rcMapping[1]] = rcValue[1]; // pitch + rcTrim[rcMapping[2]] = rcValue[2]; // yaw + rcTrim[rcMapping[3]] = rcMin[3]; // throttle + rcTrim[rcMapping[4]] = ((rcMax[4] - rcMin[4]) / 2.0f) + rcMin[4]; // mode sw + rcTrim[rcMapping[5]] = ((rcMax[5] - rcMin[5]) / 2.0f) + rcMin[5]; // aux 1 + rcTrim[rcMapping[5]] = ((rcMax[6] - rcMin[6]) / 2.0f) + rcMin[6]; // aux 2 + rcTrim[rcMapping[5]] = ((rcMax[7] - rcMin[7]) / 2.0f) + rcMin[7]; // aux 3 +} + +void QGCVehicleConfig::detectChannelInversion() +{ + +} + void QGCVehicleConfig::startCalibrationRC() { ui->rcTypeComboBox->setEnabled(false); ui->rcCalibrationButton->setText(tr("Stop RC Calibration")); resetCalibrationRC(); + calibrationEnabled = true; } void QGCVehicleConfig::stopCalibrationRC() { + calibrationEnabled = false; ui->rcTypeComboBox->setEnabled(true); ui->rcCalibrationButton->setText(tr("Start RC Calibration")); } @@ -111,6 +155,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active) // Reset current state resetCalibrationRC(); + chanCount = 0; + // Connect new system mav = active; connect(active, SIGNAL(remoteControlChannelRawChanged(int,float)), this, @@ -130,13 +176,18 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active) { toolWidgets.append(tool); ui->sensorLayout->addWidget(tool); + } else { + delete tool; } + // Load multirotor attitude pid tool = new QGCToolWidget("", this); if (tool->loadSettings(defaultsDir + "px4_mc_attitude_pid_params.qgw", false)) { toolWidgets.append(tool); ui->multiRotorAttitudeLayout->addWidget(tool); + } else { + delete tool; } // Load multirotor position pid @@ -145,6 +196,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active) { toolWidgets.append(tool); ui->multiRotorPositionLayout->addWidget(tool); + } else { + delete tool; } // Load fixed wing attitude pid @@ -153,6 +206,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active) { toolWidgets.append(tool); ui->fixedWingAttitudeLayout->addWidget(tool); + } else { + delete tool; } // Load fixed wing position pid @@ -161,6 +216,8 @@ void QGCVehicleConfig::setActiveUAS(UASInterface* active) { toolWidgets.append(tool); ui->fixedWingPositionLayout->addWidget(tool); + } else { + delete tool; } updateStatus(QString("Reading from system %1").arg(mav->getUASName())); @@ -190,23 +247,36 @@ void QGCVehicleConfig::writeCalibrationRC() // Do not write the RC type, as these values depend on this // active onboard parameter - for (unsigned int i = 0; i < chanMax; ++i) + for (unsigned int i = 0; i < chanCount; ++i) { + qDebug() << "SENDING" << minTpl.arg(i+1) << rcMin[i]; mav->setParameter(0, minTpl.arg(i+1), rcMin[i]); + QGC::SLEEP::usleep(50000); mav->setParameter(0, trimTpl.arg(i+1), rcTrim[i]); + QGC::SLEEP::usleep(50000); mav->setParameter(0, maxTpl.arg(i+1), rcMax[i]); - mav->setParameter(0, revTpl.arg(i+1), (rcRev[i]) ? -1 : 1); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, revTpl.arg(i+1), (rcRev[i]) ? -1.0f : 1.0f); + QGC::SLEEP::usleep(50000); } // Write mappings - mav->setParameter(0, "RC_MAP_ROLL", rcMapping[0]); - mav->setParameter(0, "RC_MAP_PITCH", rcMapping[1]); - mav->setParameter(0, "RC_MAP_THROTTLE", rcMapping[2]); - mav->setParameter(0, "RC_MAP_YAW", rcMapping[3]); - mav->setParameter(0, "RC_MAP_MODE_SW", rcMapping[4]); - mav->setParameter(0, "RC_MAP_AUX1", rcMapping[5]); - mav->setParameter(0, "RC_MAP_AUX2", rcMapping[6]); - mav->setParameter(0, "RC_MAP_AUX3", rcMapping[7]); + mav->setParameter(0, "RC_MAP_ROLL", (int32_t)(rcMapping[0]+1)); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, "RC_MAP_PITCH", (int32_t)(rcMapping[1]+1)); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, "RC_MAP_THROTTLE", (int32_t)(rcMapping[2]+1)); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, "RC_MAP_YAW", (int32_t)(rcMapping[3]+1)); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, "RC_MAP_MODE_SW", (int32_t)(rcMapping[4]+1)); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, "RC_MAP_AUX1", (int32_t)(rcMapping[5]+1)); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, "RC_MAP_AUX2", (int32_t)(rcMapping[6]+1)); + QGC::SLEEP::usleep(50000); + mav->setParameter(0, "RC_MAP_AUX3", (int32_t)(rcMapping[7]+1)); + QGC::SLEEP::usleep(50000); } void QGCVehicleConfig::requestCalibrationRC() @@ -238,126 +308,106 @@ void QGCVehicleConfig::writeParameters() { updateStatus(tr("Writing all onboard parameters.")); writeCalibrationRC(); + mav->writeParametersToStorage(); } void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) { -// /* scale around the mid point differently for lower and upper range */ -// if (ppm_buffer[i] > _rc.chan[i].mid + _parameters.dz[i]) { -// _rc.chan[i].scaled = ((ppm_buffer[i] - _parameters.trim[i]) / (_parameters.max[i] - _parameters.trim[i])); -// } else if ((ppm_buffer[i] < _rc_chan[i].mid - _parameters.dz[i])) { -// _rc.chan[i].scaled = -1.0 + ((ppm_buffer[i] - _parameters.min[i]) / (_parameters.trim[i] - _parameters.min[i])); -// } else { -// /* in the configured dead zone, output zero */ -// _rc.chan[i].scaled = 0.0f; -// } - - if (chan < 0 || static_cast(chan) >= chanMax) + // Check if index and values are sane + if (chan < 0 || static_cast(chan) >= chanMax || val < 500 || val > 2500) return; - if (val < rcMin[chan]) - { - rcMin[chan] = val; + if (chan + 1 > chanCount) { + chanCount = chan+1; + } + + // Update calibration data + if (calibrationEnabled) { + if (val < rcMin[chan]) + { + rcMin[chan] = val; + } + + if (val > rcMax[chan]) + { + rcMax[chan] = val; + } } - if (val > rcMax[chan]) + // Normalized value + float normalized; + + if (val >= rcTrim[chan]) { - rcMax[chan] = val; + normalized = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); } + else + { + normalized = -(rcTrim[chan] - val)/(rcTrim[chan] - rcMin[chan]); + } + + // Bound + normalized = qBound(-1.0f, normalized, 1.0f); + // Invert + normalized = (rcRev[chan]) ? -1.0f*normalized : normalized; if (chan == rcMapping[0]) { // ROLL - if (rcRoll >= rcTrim[chan]) - { - rcRoll = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); - } - else - { - rcRoll = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]); - } - rcValue[0] = val; - rcRoll = qBound(-1.0f, rcRoll, 1.0f); + rcRoll = normalized; } else if (chan == rcMapping[1]) { // PITCH - if (rcPitch >= rcTrim[chan]) - { - rcPitch = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); - } - else - { - rcPitch = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]); - } rcValue[1] = val; - rcPitch = qBound(-1.0f, rcPitch, 1.0f); + rcPitch = normalized; } else if (chan == rcMapping[2]) { - // YAW - if (rcYaw >= rcTrim[chan]) - { - rcYaw = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); - } - else - { - rcYaw = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]); - } rcValue[2] = val; - rcYaw = qBound(-1.0f, rcYaw, 1.0f); + rcYaw = normalized; } else if (chan == rcMapping[3]) { // THROTTLE - if (rcThrottle >= rcTrim[chan]) - { - rcThrottle = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); - } - else - { - rcThrottle = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]); + if (rcRev[chan]) { + rcThrottle = 1.0f + normalized; + } else { + rcThrottle = normalized; } + rcValue[3] = val; - rcThrottle = qBound(-1.0f, rcThrottle, 1.0f); + rcThrottle = qBound(0.0f, rcThrottle, 1.0f); } else if (chan == rcMapping[4]) { // MODE SWITCH - if (rcMode >= rcTrim[chan]) - { - rcMode = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); - } - else - { - rcMode = (val - rcMin[chan])/(rcTrim[chan] - rcMin[chan]); - } + rcMode = normalized; rcValue[4] = val; - rcMode = qBound(-1.0f, rcMode, 1.0f); } else if (chan == rcMapping[5]) { // AUX1 - rcAux1 = val; + rcAux1 = normalized; rcValue[5] = val; } else if (chan == rcMapping[6]) { // AUX2 - rcAux2 = val; + rcAux2 = normalized; rcValue[6] = val; } else if (chan == rcMapping[7]) { // AUX3 - rcAux3 = val; + rcAux3 = normalized; rcValue[7] = val; } changed = true; - //qDebug() << "RC CHAN:" << chan << "PPM:" << val; + //qDebug() << "RC CHAN:" << chan << "PPM:" << val << "NORMALIZED:" << normalized; } void QGCVehicleConfig::parameterChanged(int uas, int component, QString parameterName, QVariant value) @@ -381,7 +431,8 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete if (minTpl.exactMatch(parameterName)) { bool ok; unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1; - if (ok && (index > 0) && (index < chanMax)) + qDebug() << "PARAM:" << parameterName << "index:" << index; + if (ok && (index >= 0) && (index < chanMax)) { rcMin[index] = value.toInt(); } @@ -390,7 +441,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete if (maxTpl.exactMatch(parameterName)) { bool ok; unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1; - if (ok && (index > 0) && (index < chanMax)) + if (ok && (index >= 0) && (index < chanMax)) { rcMax[index] = value.toInt(); } @@ -399,7 +450,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete if (trimTpl.exactMatch(parameterName)) { bool ok; unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1; - if (ok && (index > 0) && (index < chanMax)) + if (ok && (index >= 0) && (index < chanMax)) { rcTrim[index] = value.toInt(); } @@ -408,7 +459,7 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete if (revTpl.exactMatch(parameterName)) { bool ok; unsigned int index = parameterName.mid(2, 1).toInt(&ok) - 1; - if (ok && (index > 0) && (index < chanMax)) + if (ok && (index >= 0) && (index < chanMax)) { rcRev[index] = (value.toInt() == -1) ? true : false; @@ -461,77 +512,77 @@ void QGCVehicleConfig::parameterChanged(int uas, int component, QString paramete // Order is: roll, pitch, yaw, throttle, mode sw, aux 1-3 if (parameterName.contains("RC_MAP_ROLL")) { - rcMapping[0] = value.toInt(); - ui->rollSpinBox->setValue(rcMapping[0]); + rcMapping[0] = value.toInt() - 1; + ui->rollSpinBox->setValue(rcMapping[0]+1); } if (parameterName.contains("RC_MAP_PITCH")) { - rcMapping[1] = value.toInt(); - ui->pitchSpinBox->setValue(rcMapping[1]); + rcMapping[1] = value.toInt() - 1; + ui->pitchSpinBox->setValue(rcMapping[1]+1); } if (parameterName.contains("RC_MAP_YAW")) { - rcMapping[2] = value.toInt(); - ui->yawSpinBox->setValue(rcMapping[2]); + rcMapping[2] = value.toInt() - 1; + ui->yawSpinBox->setValue(rcMapping[2]+1); } if (parameterName.contains("RC_MAP_THROTTLE")) { - rcMapping[3] = value.toInt(); - ui->throttleSpinBox->setValue(rcMapping[3]); + rcMapping[3] = value.toInt() - 1; + ui->throttleSpinBox->setValue(rcMapping[3]+1); } if (parameterName.contains("RC_MAP_MODE_SW")) { - rcMapping[4] = value.toInt(); - ui->modeSpinBox->setValue(rcMapping[4]); + rcMapping[4] = value.toInt() - 1; + ui->modeSpinBox->setValue(rcMapping[4]+1); } if (parameterName.contains("RC_MAP_AUX1")) { - rcMapping[5] = value.toInt(); - ui->aux1SpinBox->setValue(rcMapping[5]); + rcMapping[5] = value.toInt() - 1; + ui->aux1SpinBox->setValue(rcMapping[5]+1); } if (parameterName.contains("RC_MAP_AUX2")) { - rcMapping[6] = value.toInt(); - ui->aux1SpinBox->setValue(rcMapping[6]); + rcMapping[6] = value.toInt() - 1; + ui->aux1SpinBox->setValue(rcMapping[6]+1); } if (parameterName.contains("RC_MAP_AUX3")) { - rcMapping[7] = value.toInt(); - ui->aux1SpinBox->setValue(rcMapping[7]); + rcMapping[7] = value.toInt() - 1; + ui->aux1SpinBox->setValue(rcMapping[7]+1); } // Scaling if (parameterName.contains("RC_SCALE_ROLL")) { - rcScaling[0] = value.toInt(); + rcScaling[0] = value.toFloat(); } if (parameterName.contains("RC_SCALE_PITCH")) { - rcScaling[1] = value.toInt(); + rcScaling[1] = value.toFloat(); } if (parameterName.contains("RC_SCALE_YAW")) { - rcScaling[2] = value.toInt(); + rcScaling[2] = value.toFloat(); } if (parameterName.contains("RC_SCALE_THROTTLE")) { - rcScaling[3] = value.toInt(); + rcScaling[3] = value.toFloat(); } if (parameterName.contains("RC_SCALE_MODE_SW")) { - rcScaling[4] = value.toInt(); + rcScaling[4] = value.toFloat(); } if (parameterName.contains("RC_SCALE_AUX1")) { - rcScaling[5] = value.toInt(); + rcScaling[5] = value.toFloat(); } if (parameterName.contains("RC_SCALE_AUX2")) { - rcScaling[6] = value.toInt(); + rcScaling[6] = value.toFloat(); } if (parameterName.contains("RC_SCALE_AUX3")) { - rcScaling[7] = value.toInt(); + rcScaling[7] = value.toFloat(); } } @@ -575,45 +626,62 @@ void QGCVehicleConfig::updateView() { if (rc_mode == RC_MODE_1) { - ui->rollSlider->setValue(rcRoll); - ui->pitchSlider->setValue(rcThrottle); - ui->yawSlider->setValue(rcYaw); - ui->throttleSlider->setValue(rcPitch); + ui->rollSlider->setValue(rcRoll * 50 + 50); + ui->pitchSlider->setValue(rcThrottle * 100); + ui->yawSlider->setValue(rcYaw * 50 + 50); + ui->throttleSlider->setValue(rcPitch * 50 + 50); } else if (rc_mode == RC_MODE_2) { - ui->rollSlider->setValue(rcRoll); - ui->pitchSlider->setValue(rcPitch); - ui->yawSlider->setValue(rcYaw); - ui->throttleSlider->setValue(rcThrottle); + ui->rollSlider->setValue(rcRoll * 50 + 50); + ui->pitchSlider->setValue(rcPitch * 50 + 50); + ui->yawSlider->setValue(rcYaw * 50 + 50); + ui->throttleSlider->setValue(rcThrottle * 100); } else if (rc_mode == RC_MODE_3) { - ui->rollSlider->setValue(rcYaw); - ui->pitchSlider->setValue(rcThrottle); - ui->yawSlider->setValue(rcRoll); - ui->throttleSlider->setValue(rcPitch); + ui->rollSlider->setValue(rcYaw * 50 + 50); + ui->pitchSlider->setValue(rcThrottle * 100); + ui->yawSlider->setValue(rcRoll * 50 + 50); + ui->throttleSlider->setValue(rcPitch * 50 + 50); } else if (rc_mode == RC_MODE_4) { - ui->rollSlider->setValue(rcYaw); - ui->pitchSlider->setValue(rcPitch); - ui->yawSlider->setValue(rcRoll); - ui->throttleSlider->setValue(rcThrottle); + ui->rollSlider->setValue(rcYaw * 50 + 50); + ui->pitchSlider->setValue(rcPitch * 50 + 50); + ui->yawSlider->setValue(rcRoll * 50 + 50); + ui->throttleSlider->setValue(rcThrottle * 100); + } + + ui->chanLabel->setText(QString("%1/%2").arg(rcValue[0]).arg(rcRoll, 5, 'f', 2, QChar(' '))); + ui->chanLabel_2->setText(QString("%1/%2").arg(rcValue[1]).arg(rcPitch, 5, 'f', 2, QChar(' '))); + ui->chanLabel_3->setText(QString("%1/%2").arg(rcValue[2]).arg(rcYaw, 5, 'f', 2, QChar(' '))); + ui->chanLabel_4->setText(QString("%1/%2").arg(rcValue[3]).arg(rcThrottle, 5, 'f', 2, QChar(' '))); + + ui->modeSwitchSlider->setValue(rcMode * 50 + 50); + ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[4]).arg(rcMode, 5, 'f', 2, QChar(' '))); + + if (rcValue[5] != UINT16_MAX) { + ui->aux1Slider->setValue(rcAux1 * 50 + 50); + ui->chanLabel_6->setText(QString("%1/%2").arg(rcValue[5]).arg(rcAux1, 5, 'f', 2, QChar(' '))); + } else { + ui->chanLabel_6->setText(tr("---")); + } + + if (rcValue[6] != UINT16_MAX) { + ui->aux2Slider->setValue(rcAux2 * 50 + 50); + ui->chanLabel_7->setText(QString("%1/%2").arg(rcValue[6]).arg(rcAux2, 5, 'f', 2, QChar(' '))); + } else { + ui->chanLabel_7->setText(tr("---")); + } + + if (rcValue[7] != UINT16_MAX) { + ui->aux3Slider->setValue(rcAux3 * 50 + 50); + ui->chanLabel_8->setText(QString("%1/%2").arg(rcValue[7]).arg(rcAux3, 5, 'f', 2, QChar(' '))); + } else { + ui->chanLabel_8->setText(tr("---")); } - ui->chanLabel->setText(QString("%1/%2").arg(rcValue[0]).arg(rcRoll)); - ui->chanLabel_2->setText(QString("%1/%2").arg(rcValue[1]).arg(rcPitch)); - ui->chanLabel_3->setText(QString("%1/%2").arg(rcValue[2]).arg(rcYaw)); - ui->chanLabel_4->setText(QString("%1/%2").arg(rcValue[3]).arg(rcThrottle)); - ui->modeSwitchSlider->setValue(rcMode); - ui->chanLabel_5->setText(QString("%1/%2").arg(rcValue[4]).arg(rcMode)); - ui->aux1Slider->setValue(rcAux1); - ui->chanLabel_6->setText(QString("%1/%2").arg(rcValue[5]).arg(rcAux1)); - ui->aux2Slider->setValue(rcAux2); - ui->chanLabel_7->setText(QString("%1/%2").arg(rcValue[6]).arg(rcAux2)); - ui->aux3Slider->setValue(rcAux3); - ui->chanLabel_8->setText(QString("%1/%2").arg(rcValue[7]).arg(rcAux3)); changed = false; } } diff --git a/src/ui/QGCVehicleConfig.h b/src/ui/QGCVehicleConfig.h index caec352a16d2c76524477ae37c839002c07730c4..988f7f5edff351051d65a5b804c5c60e771a72f1 100644 --- a/src/ui/QGCVehicleConfig.h +++ b/src/ui/QGCVehicleConfig.h @@ -37,11 +37,81 @@ public slots: void stopCalibrationRC(); /** Start/stop the RC calibration routine */ void toggleCalibrationRC(bool enabled); + /** Set trim positions */ + void setTrimPositions(); + /** Detect which channels need to be inverted */ + void detectChannelInversion(); /** Change the mode setting of the control inputs */ void setRCModeIndex(int newRcMode); /** Render the data updated */ void updateView(); + /** Set the RC channel */ + void setRollChan(int channel) { + rcMapping[0] = channel - 1; + } + /** Set the RC channel */ + void setPitchChan(int channel) { + rcMapping[1] = channel - 1; + } + /** Set the RC channel */ + void setYawChan(int channel) { + rcMapping[2] = channel - 1; + } + /** Set the RC channel */ + void setThrottleChan(int channel) { + rcMapping[3] = channel - 1; + } + /** Set the RC channel */ + void setModeChan(int channel) { + rcMapping[4] = channel - 1; + } + /** Set the RC channel */ + void setAux1Chan(int channel) { + rcMapping[5] = channel - 1; + } + /** Set the RC channel */ + void setAux2Chan(int channel) { + rcMapping[6] = channel - 1; + } + /** Set the RC channel */ + void setAux3Chan(int channel) { + rcMapping[7] = channel - 1; + } + + /** Set channel inversion status */ + void setRollInverted(bool inverted) { + rcRev[rcMapping[0]] = inverted; + } + /** Set channel inversion status */ + void setPitchInverted(bool inverted) { + rcRev[rcMapping[1]] = inverted; + } + /** Set channel inversion status */ + void setYawInverted(bool inverted) { + rcRev[rcMapping[2]] = inverted; + } + /** Set channel inversion status */ + void setThrottleInverted(bool inverted) { + rcRev[rcMapping[3]] = inverted; + } + /** Set channel inversion status */ + void setModeInverted(bool inverted) { + rcRev[rcMapping[4]] = inverted; + } + /** Set channel inversion status */ + void setAux1Inverted(bool inverted) { + rcRev[rcMapping[5]] = inverted; + } + /** Set channel inversion status */ + void setAux2Inverted(bool inverted) { + rcRev[rcMapping[6]] = inverted; + } + /** Set channel inversion status */ + void setAux3Inverted(bool inverted) { + rcRev[rcMapping[7]] = inverted; + } + protected slots: /** Reset the RC calibration */ void resetCalibrationRC(); @@ -64,12 +134,13 @@ protected slots: protected: UASInterface* mav; ///< The current MAV static const unsigned int chanMax = 8; ///< Maximum number of channels + unsigned int chanCount; ///< Actual channels int rcType; ///< Type of the remote control quint64 rcTypeUpdateRequested; ///< Zero if not requested, non-zero if requested static const unsigned int rcTypeTimeout = 5000; ///< 5 seconds timeout, in milliseconds - int rcMin[chanMax]; ///< Minimum values - int rcMax[chanMax]; ///< Maximum values - int rcTrim[chanMax]; ///< Zero-position (center for roll/pitch/yaw, 0 throttle for throttle) + float rcMin[chanMax]; ///< Minimum values + float rcMax[chanMax]; ///< Maximum values + float rcTrim[chanMax]; ///< Zero-position (center for roll/pitch/yaw, 0 throttle for throttle) int rcMapping[chanMax]; ///< PWM to function mappings float rcScaling[chanMax]; ///< Scaling of channel input to control commands bool rcRev[chanMax]; ///< Channel reverse @@ -87,6 +158,7 @@ protected: QTimer updateTimer; ///< Controls update intervals enum RC_MODE rc_mode; ///< Mode of the remote control, according to usual convention QList toolWidgets; ///< Configurable widgets + bool calibrationEnabled; ///< calibration mode on / off private: Ui::QGCVehicleConfig *ui; diff --git a/src/ui/QGCVehicleConfig.ui b/src/ui/QGCVehicleConfig.ui index 89628d17c569e0e88ed2de2738524dd4bf33e019..2cda1d202143962dcaa92f1832fc855e1f00e203 100644 --- a/src/ui/QGCVehicleConfig.ui +++ b/src/ui/QGCVehicleConfig.ui @@ -40,7 +40,7 @@ - 1 + 0 @@ -53,10 +53,10 @@ - -1 + 0 - 1 + 100 0 @@ -69,10 +69,10 @@ - -1 + 0 - 1 + 100 0 @@ -595,10 +595,10 @@ - -1 + 0 - 1 + 100 0 @@ -611,10 +611,10 @@ - -1 + 0 - 1 + 100 0 @@ -650,10 +650,10 @@ - -1 + 0 - 1 + 100 Qt::Vertical @@ -663,10 +663,10 @@ - -1 + 0 - 1 + 100 Qt::Vertical @@ -676,10 +676,10 @@ - -1 + 0 - 1 + 100 Qt::Vertical @@ -689,10 +689,10 @@ - -1 + 0 - 1 + 100 Qt::Vertical @@ -712,6 +712,13 @@ + + + + Set Trim + + + @@ -815,8 +822,8 @@ p, li { white-space: pre-wrap; } 0 0 - 651 - 203 + 98 + 28 @@ -852,8 +859,8 @@ p, li { white-space: pre-wrap; } 0 0 - 651 - 204 + 98 + 28 @@ -899,8 +906,8 @@ p, li { white-space: pre-wrap; } 0 0 - 651 - 203 + 98 + 28 @@ -959,8 +966,8 @@ p, li { white-space: pre-wrap; } 0 0 - 651 - 204 + 98 + 28