Land drone safely

- improved landing
- navigate to the return point defined in JSON file
- land drone safely without timeouts

Issue: https://denyspopov.atlassian.net/browse/AZ-24
Type: New Feature

src/az_drone_controller.cpp

@@ -168,20 +168,13 @@ bool AzDroneController::stateFlyMission(void)
 
 void AzDroneController::missionFinishedSlot(void)
 {
-    qDebug() << "AzDroneController::missionFinishedSlot() Landing the drone.";
-    // TODO!! Check return value and print warnings and errors.
-    mAction->land();
-    // TODO!! This must be improved a lot. Disarming should be done when drone lands.
-    QTimer::singleShot(20000, this, &AzDroneController::disarmDroneSlot);
-}
+    // TODO!! Maybe use Telemetry::subscribe_landed_state() later to get the state of the landing.
+    qDebug() << "AzDroneController::missionFinishedSlot() Mission finished. Landing the drone.";
+    Action::Result result = mAction->land();
 
-void AzDroneController::disarmDroneSlot(void)
-{
-    qDebug() << "AzDroneController::disarmDroneSlot() Disarming the drone and quit.";
-    // TODO!! Check return value and print warnings and errors.
-    mAction->disarm();
-    // TODO!! Check with the team what to do after the landing.
-    QCoreApplication::instance()->quit();
+    if (result != Action::Result::Success) {
+        std::cerr << "mAction->land() failed. Reason: " << result << endl;
+    }
 }
 
 void AzDroneController::droneStateMachineSlot(void)

src/az_drone_controller.h

@@ -64,9 +64,6 @@ private slots:
     // Called at the end of the mission. Lands and disarms the drone.
     void missionFinishedSlot(void);
 
-    // Disarms the drone and exits the application. TODO!! Discuss quitting the application.
-    void disarmDroneSlot(void);
-
     // Gets Telemetry::Health information in prearming.
     void newHealthInfoSlot(Telemetry::Health health);
 

src/az_mission_controller.cpp

@@ -11,6 +11,7 @@ AzMissionController::AzMissionController(AzMission &mission, QObject *parent)
     , mMission(mission)
     , mAction(nullptr)
     , mMissionStarted(false)
+    , mFlyingToReturnPoint(false)
     , mAbsoluteAltitude(-10000)
 {}
 
@@ -37,12 +38,22 @@ bool AzMissionController::flyToNextMissionItem(void)
 {
     mCurrentMissionIndex++;
 
-    if (mCurrentMissionIndex >= (int) mMission.getActionPoints().size()) {
-        qDebug() << "AzMissionController::flyToNextMissionItem() All mission action points are handled.";
-        mMissionStarted = false;
-        emit finished();
+    if (mCurrentMissionIndex == (int) mMission.getActionPoints().size()) {
+        mFlyingToReturnPoint = true;
+        qDebug() << "AzMissionController::flyToNextMissionItem() All action points handled.";
+        qDebug() << "AzMissionController::flyToNextMissionItem() Flying to the JSON return point.";
+        const AzCoordinate &coordinate = mMission.getReturnPoint();
+        Action::Result result
+            = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
+
+        if (result == Action::Result::Success) {
             return true;
         }
+        else {
+            std::cerr << "mAction->goto_location() failed. Reason: " << result << endl;
+            return false;
+        }
+    }
 
     qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" << mCurrentMissionIndex + 1 << "/"
              << mMission.getActionPoints().size();
@@ -51,17 +62,17 @@ bool AzMissionController::flyToNextMissionItem(void)
     qDebug() << "AzMissionController::flyToNextMissionItem() navigating to point" << coordinate.latitude
              << coordinate.longitude;
 
-    qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() starts";
     Action::Result result = mAction->goto_location(coordinate.latitude, coordinate.longitude, mAbsoluteAltitude, 0.0f);
-    qDebug() << "AzMissionController::flyToNextMissionItem() MAVSDK::Action::goto_location() ends";
 
     // TODO!! Check return value and print warnings and errors.
     if (result == Action::Result::Success) {
         emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size());
         return true;
     }
-
+    else {
+        std::cerr << "mAction->goto_location() failed. Reason: " << result << endl;
         return false;
+    }
 }
 
 void AzMissionController::newPosition(Telemetry::Position pos)
@@ -70,14 +81,30 @@ void AzMissionController::newPosition(Telemetry::Position pos)
         return;
     }
 
+    if (mFlyingToReturnPoint == true) {
+        const AzCoordinate &coordinate = mMission.getReturnPoint();
+        float distance
+            = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, coordinate.latitude, coordinate.longitude);
+
+        qDebug() << "AzMissionController::newPosition() distance to return point:" << distance << " km.";
+
+        if (distance <= 0.01) {
+            qDebug() << "AzMissionController::newPosition() return point reached. Mission finished.";
+            mMissionStarted      = false;
+            mFlyingToReturnPoint = false;
+            emit finished();
+        }
+    }
+    else {
         const AzCoordinate &target = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint();
         float distance = AzUtils::distance(pos.latitude_deg, pos.longitude_deg, target.latitude, target.longitude);
 
-    qDebug() << "AzMissionController::newPosition() distance to target: " << distance;
+        qDebug() << "AzMissionController::newPosition() distance to target:" << distance << "km.";
 
         // TODO!! In final application we need to use the camera to find the target.
         if (distance <= 0.01) {
             qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item.";
             flyToNextMissionItem();
         }
+    }
 }

src/az_mission_controller.h

@@ -32,5 +32,6 @@ private:
     Action *mAction;
     int mCurrentMissionIndex;
     bool mMissionStarted;
+    bool mFlyingToReturnPoint;
     float mAbsoluteAltitude;
 };

src/mission.json

@@ -13,6 +13,6 @@
     {"point": [-35.36546294, 149.16479791], "height": 300, "action_enum": "search", "action_specific": {"targets": ["tank"]}},
     {"point": [-35.36364851, 149.16073255], "height": 500, "action_enum": "return"}
   ],
-  "return_point": [-35.36218126, 149.16505887]
+  "return_point": [-35.36286449, 149.16534729]
 }