diff --git a/src/uas/UAS.cc b/src/uas/UAS.cc index d9ce1c45d0e90ab1a2b6b1bafadb72509e955df5..f9da2dd021b24c5195ed650d58f46e30f1366a87 100644 --- a/src/uas/UAS.cc +++ b/src/uas/UAS.cc @@ -1661,7 +1661,15 @@ void UAS::startRadioControlCalibration() { 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, MAV_COMP_ID_IMU, 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, 1, 0, 0, 0); + sendMessage(msg); +} + +void UAS::endRadioControlCalibration() +{ + 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, 0, 0, 0, 0); sendMessage(msg); } diff --git a/src/uas/UAS.h b/src/uas/UAS.h index 6ea24ad2e255c6ebcc3deaccf07880dc404e32cc..d938100129b70e375924042bcd37cb4e30d5958e 100644 --- a/src/uas/UAS.h +++ b/src/uas/UAS.h @@ -912,6 +912,7 @@ public slots: void setLocalPositionOffset(float x, float y, float z, float yaw); void startRadioControlCalibration(); + void endRadioControlCalibration(); void startMagnetometerCalibration(); void startGyroscopeCalibration(); void startPressureCalibration(); diff --git a/src/uas/UASInterface.h b/src/uas/UASInterface.h index e3d59cfc79186f0c958cd18ec30e9b0f616adb8a..024a31c38047a00e25d5060fe3de6056f4a3d2c7 100644 --- a/src/uas/UASInterface.h +++ b/src/uas/UASInterface.h @@ -371,6 +371,7 @@ public slots: virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0; virtual void startRadioControlCalibration() = 0; + virtual void endRadioControlCalibration() = 0; virtual void startMagnetometerCalibration() = 0; virtual void startGyroscopeCalibration() = 0; virtual void startPressureCalibration() = 0; diff --git a/src/ui/QGCPX4VehicleConfig.cc b/src/ui/QGCPX4VehicleConfig.cc index 5ab600af7c35cfc2021bf2435173c1578d2d6884..78349ff9f3dd06b0d99132cd4e61e4ea4139a914 100644 --- a/src/ui/QGCPX4VehicleConfig.cc +++ b/src/ui/QGCPX4VehicleConfig.cc @@ -481,6 +481,8 @@ void QGCPX4VehicleConfig::startCalibrationRC() return; } + mav->startRadioControlCalibration(); + // reset all channel mappings above Ch 5 to invalid/unused value before starting calibration for (unsigned int j= 5; j < chanMappedMax; j++) { rcMapping[j] = -1; @@ -529,6 +531,8 @@ 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);