Improve taking off in Autopilot

MAVSDK/ArduPilot never reached take-off altitude. Added simple logic to
start the mission when 90% of set take-off altitude has been reached.

Type: Improvement
Issue: https://denyspopov.atlassian.net/browse/AZ-22

src/az_drone_controller.cpp

@@ -159,16 +159,24 @@ bool AzDroneController::stateArm(void)
 
 bool AzDroneController::stateTakeoff(void)
 {
-    // TODO!! Drone never reaches the target altitude with ArduPilot. Investigate and fix.
-    mAction->set_takeoff_altitude(50);
+    // Drone never reaches the target altitude with ArduPilot. This seems
+    // to be a bug in MAVSDK/ArduPilot, because this doesn't happen with PX4.
+    // Added extra check to AzDroneController::stateFlyMission() to not start the
+    // mission before 90% of AZ_RELATIVE_FLY_ALTITUDE is reached.
 
-    // TODO!! Check return value and print warnings and errors.
+    mAction->set_takeoff_altitude(AZ_RELATIVE_FLY_ALTITUDE);
     Action::Result result = mAction->takeoff();
+    std::cerr << "MAVSDK::Action::takeoff() failed. Reason: " << result << endl;
     return result == Action::Result::Success;
 }
 
 bool AzDroneController::stateFlyMission(void)
 {
+    if (QCoreApplication::arguments().contains("quadcopter") && mCurrentPosition.relative_altitude_m < AZ_RELATIVE_FLY_ALTITUDE * 0.90) {
+        qDebug() << "AzDroneController::stateFlyMission() 90% of set altitide is not reached yet.";
+        return false;
+    }
+
     // TODO!! Check with the team about fly altitude. Is altitudes in JSON file absolute or relative?
     float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m;
     return mMissionController->startMissions(flight_altitude_abs);
@@ -273,6 +281,8 @@ void AzDroneController::newPositionSlot(Telemetry::Position position)
         mFirstPosition = position;
     }
 
+    mCurrentPosition = position;
+
     // Send new position to the mission controller.
     if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) {
         mMissionController->newPosition(position);

src/az_drone_controller.h

@@ -84,5 +84,6 @@ private:
     Telemetry *mTelemetry;
     Action *mAction;
     Telemetry::Position mFirstPosition;
+    Telemetry::Position mCurrentPosition;
     AzMissionController *mMissionController;
 };