diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index f9da2dd021b24c5195ed650d58f46e30f1366a87..9e9ee8241472a0f27a6c0da2ad9a253c8fb06d98 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1657,11 +1657,11 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw) #endif } -void UAS::startRadioControlCalibration() +void UAS::startRadioControlCalibration(int param) { mavlink_message_t msg; // Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio - mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 1, 0, 0, 0); + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, param, 0, 0, 0); sendMessage(msg); } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index d938100129b70e375924042bcd37cb4e30d5958e..9db1399a78ccd600a3cf356945aa47b2c4821b10 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -911,7 +911,7 @@ public slots: /** @brief Add an offset in body frame to the setpoint */ void setLocalPositionOffset(float x, float y, float z, float yaw); - void startRadioControlCalibration(); + void startRadioControlCalibration(int param=1); void endRadioControlCalibration(); void startMagnetometerCalibration(); void startGyroscopeCalibration(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 024a31c38047a00e25d5060fe3de6056f4a3d2c7..5a863a982ca888caebcd93f810d517a971606304 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -370,7 +370,7 @@ public slots: virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0; virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0; - virtual void startRadioControlCalibration() = 0; + virtual void startRadioControlCalibration(int param=1) = 0; virtual void endRadioControlCalibration() = 0; virtual void startMagnetometerCalibration() = 0; virtual void startGyroscopeCalibration() = 0; diff --git a/src/ui/QGCPX4VehicleConfig.cc b/src/ui/QGCPX4VehicleConfig.cc index 78349ff9f3dd06b0d99132cd4e61e4ea4139a914..6a9d0fa1b77f6d30cdb231764b931c855df65d7c 100644 --- a/src/ui/QGCPX4VehicleConfig.cc +++ b/src/ui/QGCPX4VehicleConfig.cc @@ -150,6 +150,8 @@ QGCPX4VehicleConfig::QGCPX4VehicleConfig(QWidget *parent) : connect(ui->rcMenuButton,SIGNAL(clicked()), this,SLOT(rcMenuButtonClicked())); + connect(ui->rcCopyTrimButton, SIGNAL(clicked()), + this, SLOT(copyAttitudeTrim())); connect(ui->sensorMenuButton,SIGNAL(clicked()), this,SLOT(sensorMenuButtonClicked())); connect(ui->generalMenuButton,SIGNAL(clicked()), @@ -381,6 +383,33 @@ void QGCPX4VehicleConfig::toggleSpektrumPairing(bool enabled) } } +void QGCPX4VehicleConfig::copyAttitudeTrim() { + if (configEnabled) { + + QMessageBox warnMsgBox; + warnMsgBox.setText(tr("Attitude trim denied during RC calibration")); + warnMsgBox.setInformativeText(tr("Please end the RC calibration before doing attitude trim.")); + warnMsgBox.setStandardButtons(QMessageBox::Ok); + warnMsgBox.setDefaultButton(QMessageBox::Ok); + (void)warnMsgBox.exec(); + } + + // Not aborted, but warn user + + msgBox.setText(tr("Confirm Attitude Trim")); + msgBox.setInformativeText(tr("On clicking OK, the current Roll / Pitch / Yaw stick positions will be set as trim values in auto flight. Do NOT reset your trim values after this step.")); + msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);//allow user to cancel upload after reviewing values + int msgBoxResult = msgBox.exec(); + if (QMessageBox::Cancel == msgBoxResult) { + return; + // do not execute + } + + mav->startRadioControlCalibration(2); + QGC::SLEEP::msleep(100); + mav->endRadioControlCalibration(); +} + void QGCPX4VehicleConfig::setTrimPositions() { int rollMap = rcMapping[0]; @@ -473,7 +502,7 @@ void QGCPX4VehicleConfig::detectChannelInversion(int aert_index) void QGCPX4VehicleConfig::startCalibrationRC() { - if (chanCount < 5) { + if (chanCount < 5 && !mav) { QMessageBox::warning(0, tr("RC not Connected"), tr("Is the RC receiver connected and transmitter turned on? Detected %1 radio channels. To operate PX4, you need at least 5 channels. ").arg(chanCount)); @@ -481,7 +510,8 @@ void QGCPX4VehicleConfig::startCalibrationRC() return; } - mav->startRadioControlCalibration(); + // XXX magic number: Set to 1 for radio input disable + mav->startRadioControlCalibration(1); // reset all channel mappings above Ch 5 to invalid/unused value before starting calibration for (unsigned int j= 5; j < chanMappedMax; j++) { @@ -500,7 +530,6 @@ void QGCPX4VehicleConfig::startCalibrationRC() //QMessageBox::information(0,"Information","Additional channels have not been mapped, but can be mapped in the channel table below."); configEnabled = false; - QMessageBox::information(0, tr("Information"),tr("Click OK, then move all sticks to their extreme positions, watching the min/max values to ensure you get the most range from your controller. This includes all switches")); ui->rcCalibrationButton->setText(tr("Finish RC Calibration")); resetCalibrationRC(); calibrationEnabled = true; @@ -523,7 +552,15 @@ void QGCPX4VehicleConfig::startCalibrationRC() ui->radio17Widget->showMinMax(); ui->radio18Widget->showMinMax(); - QMessageBox::information(0, tr("Information"), tr("Please click on the button once finished")); + msgBox.setText(tr("Information")); + msgBox.setInformativeText(tr("Please move the sticks to their extreme positions, including all switches. Then click on the OK button once finished")); + msgBox.setStandardButtons(QMessageBox::Ok); + msgBox.show(); + msgBox.move((frameGeometry().width() - msgBox.width()) / 4.0f,(frameGeometry().height() - msgBox.height()) / 1.5f); + int msgBoxResult = msgBox.exec(); + if (QMessageBox::Ok == msgBoxResult) { + stopCalibrationRC(); + } } void QGCPX4VehicleConfig::stopCalibrationRC() @@ -531,18 +568,19 @@ void QGCPX4VehicleConfig::stopCalibrationRC() if (!calibrationEnabled) return; - mav->endRadioControlCalibration(); - // Try to identify inverted channels, but only for R/P/Y/T for (int i = 0; i < 4; i++) { detectChannelInversion(i); } - QMessageBox::information(0,"Trims","Ensure all controls are centered and throttle is in the lowest position. Click OK to continue"); + QMessageBox::information(0,"Trims","Ensure THROTTLE is in the LOWEST position and roll / pitch / yaw are CENTERED. Click OK to continue"); calibrationEnabled = false; configEnabled = false; ui->rcCalibrationButton->setText(tr("Start RC Calibration")); + ui->rcCalibrationButton->blockSignals(true); + ui->rcCalibrationButton->setChecked(false); + ui->rcCalibrationButton->blockSignals(false); ui->rollWidget->hideMinMax(); ui->pitchWidget->hideMinMax(); @@ -585,17 +623,20 @@ void QGCPX4VehicleConfig::stopCalibrationRC() statusstr += QString::number(i) +"\t"+ QString::number(rcMin[i]) +"\t"+ QString::number(rcValue[i]) +"\t"+ QString::number(rcMax[i]) +"\n"; } - msgBox.setText(tr("Confirm Calibration")); msgBox.setInformativeText(statusstr); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);//allow user to cancel upload after reviewing values int msgBoxResult = msgBox.exec(); + + // Done, exit calibration mode now + mav->endRadioControlCalibration(); + if (QMessageBox::Cancel == msgBoxResult) { - return;//don't commit these values + return; //don't commit these values + } else { + QMessageBox::information(0,"Uploading the RC Calibration","The configuration will now be uploaded and permanently stored."); + writeCalibrationRC(); } - - QMessageBox::information(0,"Uploading the RC Calibration","The configuration will now be uploaded and permanently stored."); - writeCalibrationRC(); } void QGCPX4VehicleConfig::loadQgcConfig(bool primary) diff --git a/src/ui/QGCPX4VehicleConfig.h b/src/ui/QGCPX4VehicleConfig.h index 10e2956fd4763bcee90e937852224fa14d63ddf0..a223717388104a22e2557678b07d6772e0b0cb4d 100644 --- a/src/ui/QGCPX4VehicleConfig.h +++ b/src/ui/QGCPX4VehicleConfig.h @@ -62,6 +62,8 @@ public slots: void toggleCalibrationRC(bool enabled); /** Start/stop the Spektrum pair routine */ void toggleSpektrumPairing(bool enabled); + /** Set the current trim values as attitude trim values */ + void copyAttitudeTrim(); /** Set trim positions */ void setTrimPositions(); /** Detect which channels need to be inverted */ diff --git a/src/ui/QGCPX4VehicleConfig.ui b/src/ui/QGCPX4VehicleConfig.ui index c8483e9e67a3dd15b8b0d34dde3b32cb471cd4ad..afe1571a55dc7a4a61729752763e16950a1fe0a9 100644 --- a/src/ui/QGCPX4VehicleConfig.ui +++ b/src/ui/QGCPX4VehicleConfig.ui @@ -6,7 +6,7 @@ 0 0 - 1272 + 1293 1132 @@ -20,7 +20,16 @@ Form - + + 0 + + + 0 + + + 0 + + 0 @@ -619,7 +628,7 @@ - + @@ -699,6 +708,16 @@ + + + + Copy the trim values from roll / pitch / yaw from manual flight to the autonomous flight modes. + + + Copy Attitude Trims + + + @@ -986,7 +1005,16 @@ Configuration - + + 0 + + + 0 + + + 0 + + 0 @@ -999,12 +1027,21 @@ 0 0 - 16 - 16 + 98 + 28 - + + 0 + + + 0 + + + 0 + + 0 @@ -1023,7 +1060,16 @@ Configuration - + + 0 + + + 0 + + + 0 + + 0 @@ -1036,12 +1082,21 @@ 0 0 - 16 - 16 + 98 + 28 - + + 0 + + + 0 + + + 0 + + 0 @@ -1074,7 +1129,16 @@ Onboard Configuration - + + 0 + + + 0 + + + 0 + + 0 @@ -1087,12 +1151,21 @@ 0 0 - 16 - 16 + 98 + 28 - + + 0 + + + 0 + + + 0 + + 0 @@ -1198,7 +1271,16 @@ - + + 0 + + + 0 + + + 0 + + 0