From 7c802106c7d8d23f7d8c59c301fc3b98fc356195 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tuomas=20J=C3=A4rvinen?= Date: Wed, 17 Jul 2024 19:17:35 +0200 Subject: [PATCH] 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 | 16 +++++++++++++--- src/az_drone_controller.h | 1 + 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/az_drone_controller.cpp b/src/az_drone_controller.cpp index a50915d..d651b18 100644 --- a/src/az_drone_controller.cpp +++ b/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); diff --git a/src/az_drone_controller.h b/src/az_drone_controller.h index 072f587..4d7af44 100644 --- a/src/az_drone_controller.h +++ b/src/az_drone_controller.h @@ -84,5 +84,6 @@ private: Telemetry *mTelemetry; Action *mAction; Telemetry::Position mFirstPosition; + Telemetry::Position mCurrentPosition; AzMissionController *mMissionController; };