From 3345db1e5f8b76b9903c4d128b9c901dea57ddf3 Mon Sep 17 00:00:00 2001
From: Danny Schrader <sirnesma@gmail.com>
Date: Fri, 10 Feb 2017 15:31:56 -0500
Subject: [PATCH] added Abort button (for abort landing).  button sends
 MAV_CMD_DO_GO_AROUND.

---
 src/FirmwarePlugin/FirmwarePlugin.cc           |  7 +++++++
 src/FirmwarePlugin/FirmwarePlugin.h            |  3 +++
 src/FlightDisplay/FlightDisplayViewWidgets.qml | 14 ++++++++++++++
 src/Vehicle/Vehicle.cc                         | 10 ++++++++++
 src/Vehicle/Vehicle.h                          |  3 +++
 5 files changed, 37 insertions(+)

diff --git a/src/FirmwarePlugin/FirmwarePlugin.cc b/src/FirmwarePlugin/FirmwarePlugin.cc
index fb6d02816..f42ed6efd 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.cc
+++ b/src/FirmwarePlugin/FirmwarePlugin.cc
@@ -16,6 +16,7 @@
 static FirmwarePluginFactoryRegister* _instance = NULL;
 
 const char* guided_mode_not_supported_by_vehicle = "Guided mode not supported by Vehicle.";
+const char* landing_aborted = "Landing aborted.";
 
 const char* FirmwarePlugin::px4FollowMeFlightMode = "Follow Me";
 
@@ -231,6 +232,12 @@ void FirmwarePlugin::pauseVehicle(Vehicle* vehicle)
     qgcApp()->showMessage(guided_mode_not_supported_by_vehicle);
 }
 
+void FirmwarePlugin::abortLanding(Vehicle* vehicle)
+{
+    Q_UNUSED(vehicle);
+    qgcApp()->showMessage(landing_aborted);
+}
+
 void FirmwarePlugin::guidedModeRTL(Vehicle* vehicle)
 {
     // Not supported by generic vehicle
diff --git a/src/FirmwarePlugin/FirmwarePlugin.h b/src/FirmwarePlugin/FirmwarePlugin.h
index 0c0eb0f77..f5c27eab0 100644
--- a/src/FirmwarePlugin/FirmwarePlugin.h
+++ b/src/FirmwarePlugin/FirmwarePlugin.h
@@ -105,6 +105,9 @@ public:
     /// If not, vehicle will be left in Loiter.
     virtual void pauseVehicle(Vehicle* vehicle);
 
+    /// Command vehicle to abort landing
+    virtual void abortLanding(Vehicle* vehicle);
+
     /// Command vehicle to return to launch
     virtual void guidedModeRTL(Vehicle* vehicle);
 
diff --git a/src/FlightDisplay/FlightDisplayViewWidgets.qml b/src/FlightDisplay/FlightDisplayViewWidgets.qml
index 9743c72f3..f60854c4a 100644
--- a/src/FlightDisplay/FlightDisplayViewWidgets.qml
+++ b/src/FlightDisplay/FlightDisplayViewWidgets.qml
@@ -191,6 +191,7 @@ Item {
         readonly property int confirmGoTo:          8
         readonly property int confirmRetask:        9
         readonly property int confirmOrbit:         10
+        readonly property int confirmAbort:         11
 
         property int    confirmActionCode
         property real   _showMargin:    _margins
@@ -238,6 +239,9 @@ Item {
                 //-- Center on current flight map position and orbit with a 50m radius (velocity/direction controlled by the RC)
                 //_activeVehicle.guidedModeOrbit(QGroundControl.flightMapPosition, 50.0)
                 break;
+            case confirmAbort:
+                _activeVehicle.abortLanding()
+                break;
             default:
                 console.warn(qsTr("Internal error: unknown confirmActionCode"), confirmActionCode)
             }
@@ -289,6 +293,9 @@ Item {
             case confirmOrbit:
                 guidedModeConfirm.confirmText = qsTr("enter orbit mode")
                 break;
+            case confirmAbort:
+                guidedModeConfirm.confirmText = qsTr("abort landing")
+                break;
             }
             _guidedModeBar.visible = false
             guidedModeConfirm.visible = true
@@ -356,6 +363,13 @@ Item {
                     onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmOrbit)
                 }
 
+                QGCButton {
+                    pointSize:  _guidedModeBar._fontPointSize
+                    text:       qsTr("Abort")
+                    visible:    _activeVehicle && _activeVehicle.flying
+                    onClicked:  _guidedModeBar.confirmAction(_guidedModeBar.confirmAbort)
+                }
+
             } // Row
         } // Column
     } // Rectangle - Guided mode buttons
diff --git a/src/Vehicle/Vehicle.cc b/src/Vehicle/Vehicle.cc
index 4876910d6..796886790 100644
--- a/src/Vehicle/Vehicle.cc
+++ b/src/Vehicle/Vehicle.cc
@@ -1838,6 +1838,16 @@ void Vehicle::pauseVehicle(void)
     _firmwarePlugin->pauseVehicle(this);
 }
 
+void Vehicle::abortLanding(void)
+{
+    sendMavCommand(defaultComponentId(),
+                   MAV_CMD_DO_GO_AROUND,
+                   true,        // show error if fails
+                   50);
+
+    //_firmwarePlugin->abortLanding(this);
+}
+
 bool Vehicle::guidedMode(void) const
 {
     return _firmwarePlugin->isGuidedMode(this);
diff --git a/src/Vehicle/Vehicle.h b/src/Vehicle/Vehicle.h
index a765f876a..9d168eb00 100644
--- a/src/Vehicle/Vehicle.h
+++ b/src/Vehicle/Vehicle.h
@@ -355,6 +355,9 @@ public:
     /// Command vehicle to kill all motors no matter what state
     Q_INVOKABLE void emergencyStop(void);
 
+    /// Command vehicle to abort landing
+    Q_INVOKABLE void abortLanding(void);
+
     /// Alter the current mission item on the vehicle
     Q_INVOKABLE void setCurrentMissionSequence(int seq);
 
-- 
2.22.0