diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index eaab8ac3f6f337b195969cc2917359aae91a297f..4986791ec2f2ae2ce9de78051838ac4c78992b0b 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -3133,6 +3133,40 @@ void Vehicle::guidedModeROI(const QGeoCoordinate& centerCoord) } } +void Vehicle::stopGuidedModeROI() +{ + if (!roiModeSupported()) { + qgcApp()->showMessage(QStringLiteral("ROI mode not supported by Vehicle.")); + return; + } + if (capabilityBits() & MAV_PROTOCOL_CAPABILITY_COMMAND_INT) { + sendMavCommandInt( + defaultComponentId(), + MAV_CMD_DO_SET_ROI_NONE, + MAV_FRAME_GLOBAL, + true, // show error if fails + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN())); // Empty + } else { + sendMavCommand( + defaultComponentId(), + MAV_CMD_DO_SET_ROI_NONE, + true, // show error if fails + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN()), // Empty + static_cast(qQNaN())); // Empty + } +} + void Vehicle::pauseVehicle(void) { if (!pauseVehicleSupported()) { @@ -3401,6 +3435,20 @@ void Vehicle::_handleCommandAck(mavlink_message_t& message) } } + if (ack.command == MAV_CMD_DO_SET_ROI_LOCATION) { + if (ack.result == MAV_RESULT_ACCEPTED) { + _isROIEnabled = true; + emit isROIEnabledChanged(); + } + } + + if (ack.command == MAV_CMD_DO_SET_ROI_NONE) { + if (ack.result == MAV_RESULT_ACCEPTED) { + _isROIEnabled = false; + emit isROIEnabledChanged(); + } + } + #if !defined(NO_ARDUPILOT_DIALECT) if (ack.command == MAV_CMD_FLASH_BOOTLOADER && ack.result == MAV_RESULT_ACCEPTED) { qgcApp()->showMessage(tr("Bootloader flash succeeded")); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 6fc832b936feb8cccecd880b677e67fe8bb5a735..67f782b0b715f39e4aa100e91d72647921578a4f 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -642,6 +642,7 @@ public: Q_PROPERTY(qreal gimbalPitch READ gimbalPitch NOTIFY gimbalPitchChanged) Q_PROPERTY(qreal gimbalYaw READ gimbalYaw NOTIFY gimbalYawChanged) Q_PROPERTY(bool gimbalData READ gimbalData NOTIFY gimbalDataChanged) + Q_PROPERTY(bool isROIEnabled READ isROIEnabled NOTIFY isROIEnabledChanged) // The following properties relate to Orbit status Q_PROPERTY(bool orbitActive READ orbitActive NOTIFY orbitActiveChanged) @@ -741,6 +742,7 @@ public: /// Command vehicle to keep given point as ROI /// @param centerCoord ROI coordinates Q_INVOKABLE void guidedModeROI(const QGeoCoordinate& centerCoord); + Q_INVOKABLE void stopGuidedModeROI(); /// Command vehicle to pause at current location. If vehicle supports guide mode, vehicle will be left /// in guided mode after pause. @@ -1114,6 +1116,7 @@ public: qreal gimbalPitch () { return static_cast(_curGimbalPitch); } qreal gimbalYaw () { return static_cast(_curGinmbalYaw); } bool gimbalData () { return _haveGimbalData; } + bool isROIEnabled () { return _isROIEnabled; } public slots: void setVtolInFwdFlight (bool vtolInFwdFlight); @@ -1229,6 +1232,7 @@ signals: void gimbalPitchChanged (); void gimbalYawChanged (); void gimbalDataChanged (); + void isROIEnabledChanged (); private slots: void _mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message); @@ -1495,6 +1499,7 @@ private: float _curGimbalPitch = 0.0f; float _curGinmbalYaw = 0.0f; bool _haveGimbalData = false; + bool _isROIEnabled = false; Joystick* _activeJoystick = nullptr; int _firmwareMajorVersion;