diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index 275350f..0d3e7c6 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -2867,6 +2867,17 @@ void UAS::land() } /** +* Order the robot to land on the runway +*/ +void UAS::pairRX(int rxType, int rxSubType) +{ + mavlink_message_t msg; + + mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_ALL, MAV_CMD_START_RX_PAIR, 0, rxType, rxSubType, 0, 0, 0, 0, 0); + sendMessage(msg); +} + +/** * The MAV starts the emergency landing procedure. The behaviour depends on the onboard implementation * and might differ between systems. */ diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 6b5d34f..8a60d34 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -723,6 +723,9 @@ public slots: void home(); /** @brief Order the robot to land **/ void land(); + /** @brief Order the robot to pair its receiver **/ + void pairRX(int rxType, int rxSubType); + void halt(); void go(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index 7a0640e..e97ca67 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -290,6 +290,8 @@ public slots: virtual void home() = 0; /** @brief Order the robot to land **/ virtual void land() = 0; + /** @brief Order the robot to pair its receiver **/ + virtual void pairRX(int rxType, int rxSubType) = 0; /** @brief Halt the system */ virtual void halt() = 0; /** @brief Start/continue the current robot action */ diff --git a/src/ui/QGCPX4VehicleConfig.cc b/src/ui/QGCPX4VehicleConfig.cc index c06fdd5..1a30cda 100644 --- a/src/ui/QGCPX4VehicleConfig.cc +++ b/src/ui/QGCPX4VehicleConfig.cc @@ -343,15 +343,12 @@ void QGCPX4VehicleConfig::toggleSpektrumPairing(bool enabled) warnMsgBox.setStandardButtons(QMessageBox::Ok); warnMsgBox.setDefaultButton(QMessageBox::Ok); (void)warnMsgBox.exec(); + return; } - int mode = 1; // DSM2 - if (ui->dsmxRadioButton->isChecked()) - mode = 2; // DSMX - - mav->getParamManager()->setPendingParam(0, "RC_DSM_BIND", mode, true); - // Do not save this parameter, just set in RAM - mav->getParamManager()->sendPendingParameters(false, true); + UASInterface* mav = UASManager::instance()->getActiveUAS(); + if (mav) + mav->pairRX(0, ui->dsmxRadioButton->isChecked() ? 1 : 0); } void QGCPX4VehicleConfig::setTrimPositions()