diff --git a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc index 3987eeff945da9da05a40afdf2f7150491fabebe..475955e42ed200dca1b1c772f703ae809eb641dc 100644 --- a/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc +++ b/src/FirmwarePlugin/APM/APMFirmwarePlugin.cc @@ -444,7 +444,7 @@ void APMFirmwarePlugin::_handleIncomingHeartbeat(Vehicle* vehicle, mavlink_messa } } - vehicle->setFlying(flying); + vehicle->_setFlying(flying); } bool APMFirmwarePlugin::adjustIncomingMavlinkMessage(Vehicle* vehicle, mavlink_message_t* message) diff --git a/src/FlightDisplay/FlightDisplayView.qml b/src/FlightDisplay/FlightDisplayView.qml index 4eaa7672e9ba56d7df67c13ca323362e2896adfa..f670b229939058e27d8ce8566bd720a155e44941 100644 --- a/src/FlightDisplay/FlightDisplayView.qml +++ b/src/FlightDisplay/FlightDisplayView.qml @@ -525,6 +525,12 @@ QGCView { confirmAction(actionResumeMission) } } + + onShowLandAbortChanged: { + if (showLandAbort) { + confirmAction(actionLandAbort) + } + } } GuidedActionConfirm { diff --git a/src/FlightDisplay/GuidedActionsController.qml b/src/FlightDisplay/GuidedActionsController.qml index 967698f646f4d334816d01e135af3d7c5d0fa5ff..7f94db2c6415cef52f8b2230e1f6891bd1d73b8e 100644 --- a/src/FlightDisplay/GuidedActionsController.qml +++ b/src/FlightDisplay/GuidedActionsController.qml @@ -92,7 +92,7 @@ Item { property bool showPause: _activeVehicle && _vehicleArmed && _activeVehicle.pauseVehicleSupported && _vehicleFlying && !_vehiclePaused property bool showChangeAlt: (_activeVehicle && _vehicleFlying) && _activeVehicle.guidedModeSupported && _vehicleArmed && !_missionActive property bool showOrbit: !_hideOrbit && _activeVehicle && _vehicleFlying && _activeVehicle.orbitModeSupported && _vehicleArmed && !_missionActive - property bool showLandAbort: _activeVehicle && _vehicleFlying && _activeVehicle.fixedWing + property bool showLandAbort: _activeVehicle && _vehicleFlying && _activeVehicle.fixedWing && _vehicleLanding property bool showGotoLocation: _activeVehicle && _activeVehicle.guidedMode && _vehicleFlying property var _activeVehicle: QGroundControl.multiVehicleManager.activeVehicle @@ -101,6 +101,7 @@ Item { property bool _missionActive: _activeVehicle ? _vehicleArmed && (_vehicleInLandMode || _vehicleInRTLMode || _vehicleInMissionMode) : false property bool _vehicleArmed: _activeVehicle ? _activeVehicle.armed : false property bool _vehicleFlying: _activeVehicle ? _activeVehicle.flying : false + property bool _vehicleLanding: _activeVehicle ? _activeVehicle.landing : false property bool _vehiclePaused: false property bool _vehicleInMissionMode: false property bool _vehicleInRTLMode: false diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc index db85687c5071a05b4da2b11850b7956a8e2ea6b0..b4ee95a6c25a4bc3d92287ae673249df47b98a54 100644 --- a/src/Vehicle/Vehicle.cc +++ b/src/Vehicle/Vehicle.cc @@ -94,6 +94,7 @@ Vehicle::Vehicle(LinkInterface* link, , _rcRSSIstore(255) , _autoDisconnect(false) , _flying(false) + , _landing(false) , _onboardControlSensorsPresent(0) , _onboardControlSensorsEnabled(0) , _onboardControlSensorsHealth(0) @@ -254,6 +255,7 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, , _rcRSSIstore(255) , _autoDisconnect(false) , _flying(false) + , _landing(false) , _onboardControlSensorsPresent(0) , _onboardControlSensorsEnabled(0) , _onboardControlSensorsHealth(0) @@ -831,14 +833,21 @@ void Vehicle::_handleExtendedSysState(mavlink_message_t& message) mavlink_msg_extended_sys_state_decode(&message, &extendedState); switch (extendedState.landed_state) { - case MAV_LANDED_STATE_UNDEFINED: - break; case MAV_LANDED_STATE_ON_GROUND: - setFlying(false); + _setFlying(false); + _setLanding(false); break; + case MAV_LANDED_STATE_TAKEOFF: case MAV_LANDED_STATE_IN_AIR: - setFlying(true); - return; + _setFlying(true); + _setLanding(false); + break; + case MAV_LANDED_STATE_LANDING: + _setFlying(true); + _setLanding(true); + break; + default: + break; } } @@ -1962,7 +1971,7 @@ void Vehicle::_announceArmedChanged(bool armed) _say(QString("%1 %2").arg(_vehicleIdSpeech()).arg(armed ? QStringLiteral("armed") : QStringLiteral("disarmed"))); } -void Vehicle::setFlying(bool flying) +void Vehicle::_setFlying(bool flying) { if (armed() && _flying != flying) { _flying = flying; @@ -1970,6 +1979,14 @@ void Vehicle::setFlying(bool flying) } } +void Vehicle::_setLanding(bool landing) +{ + if (armed() && _landing != landing) { + _landing = landing; + emit landingChanged(landing); + } +} + bool Vehicle::guidedModeSupported(void) const { return _firmwarePlugin->isCapable(this, FirmwarePlugin::GuidedModeCapability); diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h index 7e723d960257547b2f42704eac1a68387298cdf8..c2fe67819ec4422f96b05c6fb2191bd2c37782d2 100644 --- a/src/Vehicle/Vehicle.h +++ b/src/Vehicle/Vehicle.h @@ -314,10 +314,13 @@ public: Q_PROPERTY(QVariantList cameraList READ cameraList CONSTANT) /// true: Vehicle is flying, false: Vehicle is on ground - Q_PROPERTY(bool flying READ flying WRITE setFlying NOTIFY flyingChanged) + Q_PROPERTY(bool flying READ flying NOTIFY flyingChanged) + + /// true: Vehicle is flying, false: Vehicle is on ground + Q_PROPERTY(bool landing READ landing NOTIFY landingChanged) /// true: Vehicle is in Guided mode and can respond to guided commands, false: vehicle cannot respond to direct control commands - Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged) + Q_PROPERTY(bool guidedMode READ guidedMode WRITE setGuidedMode NOTIFY guidedModeChanged) /// true: Guided mode commands are supported by this vehicle Q_PROPERTY(bool guidedModeSupported READ guidedModeSupported CONSTANT) @@ -516,7 +519,6 @@ public: bool supportsCalibratePressure(void) const; bool supportsMotorInterference(void) const; - void setFlying(bool flying); void setGuidedMode(bool guidedMode); QString prearmError(void) const { return _prearmError; } @@ -566,6 +568,7 @@ public: uint messagesSent () { return _messagesSent; } uint messagesLost () { return _messagesLost; } bool flying () const { return _flying; } + bool landing () const { return _landing; } bool guidedMode () const; uint8_t baseMode () const { return _base_mode; } uint32_t customMode () const { return _custom_mode; } @@ -674,6 +677,8 @@ public: /// @return: true: initial request is complete, false: initial request is still in progress; bool initialPlanRequestComplete(void) const { return _initialPlanRequestComplete; } + void _setFlying(bool flying); + void _setLanding(bool landing); void _setHomePosition(QGeoCoordinate& homeCoord); signals: @@ -693,6 +698,7 @@ signals: void connectionLostEnabledChanged(bool connectionLostEnabled); void autoDisconnectChanged(bool autoDisconnectChanged); void flyingChanged(bool flying); + void landingChanged(bool landing); void guidedModeChanged(bool guidedMode); void prearmErrorChanged(const QString& prearmError); void soloFirmwareChanged(bool soloFirmware); @@ -880,6 +886,7 @@ private: double _rcRSSIstore; bool _autoDisconnect; ///< true: Automatically disconnect vehicle when last connection goes away or lost heartbeat bool _flying; + bool _landing; uint32_t _onboardControlSensorsPresent; uint32_t _onboardControlSensorsEnabled; uint32_t _onboardControlSensorsHealth;