diff --git a/.gitmodules b/.gitmodules index 70b07a4..e69de29 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,9 +0,0 @@ -[submodule "3rd/px4"] - path = 3rd/px4 - url = https://github.com/PX4/PX4-Autopilot.git -[submodule "3rd/mavsdk"] - path = 3rd/mavsdk - url = https://github.com/mavlink/MAVSDK.git -[submodule "3rd/rapidjson"] - path = 3rd/rapidjson - url = https://github.com/Tencent/rapidjson.git diff --git a/3rd/mavsdk b/3rd/mavsdk deleted file mode 160000 index e06de06..0000000 --- a/3rd/mavsdk +++ /dev/null @@ -1 +0,0 @@ -Subproject commit e06de06d9770b94d6d1ae0a2937ee5f2f2a0ebfb diff --git a/3rd/px4 b/3rd/px4 deleted file mode 160000 index 32aa326..0000000 --- a/3rd/px4 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 32aa3263a60d48a960eb8a2ccc50073815250889 diff --git a/README.md b/README.md index f9e503b..d37eb18 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,11 @@ # Azaion Autopilot -A preliminary example of autonomous drone flight. This example is based on the MAVSDK framework and PX4 flight controller software. Running the example requires compiling MAVSDK and PX4 on a Linux image system using cmake. Example has been tested in Ubuntu 20.04 environment. +A preliminary example of autonomous drone flight. This example is based on the MAVSDK framework and ArduPilot flight controller software. Running the example requires installing MAVSDK debian package and compiling ArduPilot. Example has been tested in Ubuntu 20.04 environment. -TODO!! Add support for Docker builds. PX4 installs many dependencies. - ## Install necessary dependencies sudo apt update -sudo apt install ccache git build-essential curl cmake +sudo apt install ccache git build-essential qt5-qmake qtbase5-dev ## Speed up the compilations echo "export MAKEFLAGS=\"-j$(($(nproc)))\"" >> ~/.bashrc @@ -15,33 +13,26 @@ echo "export MAKEFLAGS=\"-j$(($(nproc)))\"" >> ~/.bashrc echo "export PATH=/usr/lib/ccache:\$PATH" >> ~/.bashrc ## Clone source codes. You must add your SSH key before the cloning! +git clone git@github.com:azaion/autopilot.git git clone --recursive git@github.com:azaion/autopilot.git - git submodule update --init --recursive -## Build and install PX4 -cd 3rd/px4 && bash ./Tools/setup/ubuntu.sh && make px4_sitl jmavsim - ## Install MAVSDK for Ubuntu 20.04 -wget https://github.com/mavlink/MAVSDK/releases/download/v2.4.1/libmavsdk-dev_2.4.1_ubuntu20.04_amd64.deb +wget https://github.com/mavlink/MAVSDK/releases/download/v2.9.1/libmavsdk-dev_2.9.1_ubuntu20.04_amd64.deb -sudo dpkg -i ibmavsdk-dev_2.4.1_ubuntu20.04_amd64.deb +sudo dpkg -i libmavsdk-dev_2.9.1_ubuntu20.04_amd64.deb ## Install MAVSDK for Ubuntu 22.04 -wget https://github.com/mavlink/MAVSDK/releases/download/v2.4.1/libmavsdk-dev_2.4.1_ubuntu22.04_amd64.deb +wget https://github.com/mavlink/MAVSDK/releases/download/v2.9.1/libmavsdk-dev_2.9.1_ubuntu22.04_amd64.deb -sudo dpkg -i libmavsdk-dev_2.4.1_ubuntu22.04_amd64.deb +sudo dpkg -i libmavsdk-dev_2.9.1_ubuntu22.04_amd64.deb -## Build example application -cd src && cmake . && make +## Build autopilot application +cd src && qmake && make -## Launch similator -cd 3rd/px4 +## Launch similator in the ArduPilot directory +./Tools/autotest/sim_vehicle.py --map --console -v ArduCopter -PX4_HOME_LAT=52.5314818 PX4_HOME_LON=13.3872884 ./build/px4_sitl_default/bin/px4 - -## Launch example application in the new terminal window -cd src - -make && ./azaion mission.json +## Launch example application in the new terminal window after waiting simulator (around 1 min) to be ready +qmake && make && ./autopilot mission.json diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt deleted file mode 100644 index bfe72f8..0000000 --- a/src/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2) - -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -project(azaion) - -add_executable(azaion - main.cpp - az_mission.h az_mission.cpp - az_action_point.h az_action_point.cpp - az_coordinate.h az_coordinate.cpp - az_fly.h az_fly.cpp - -) - -find_package(MAVSDK REQUIRED) - -target_link_libraries(azaion - MAVSDK::mavsdk - pthread -) - -if(NOT MSVC) - add_compile_options(azaion PRIVATE -Wall -Wextra) -else() - add_compile_options(azaion PRIVATE -WX -W2) -endif() diff --git a/src/Makefile b/src/Makefile deleted file mode 100644 index 34810cc..0000000 --- a/src/Makefile +++ /dev/null @@ -1,298 +0,0 @@ -# CMAKE generated file: DO NOT EDIT! -# Generated by "Unix Makefiles" Generator, CMake Version 3.16 - -# Default target executed when no arguments are given to make. -default_target: all - -.PHONY : default_target - -# Allow only one "make -f Makefile2" at a time, but pass parallelism. -.NOTPARALLEL: - - -#============================================================================= -# Special targets provided by cmake. - -# Disable implicit rules so canonical targets will work. -.SUFFIXES: - - -# Remove some rules from gmake that .SUFFIXES does not remove. -SUFFIXES = - -.SUFFIXES: .hpux_make_needs_suffix_list - - -# Suppress display of executed commands. -$(VERBOSE).SILENT: - - -# A target that is always out of date. -cmake_force: - -.PHONY : cmake_force - -#============================================================================= -# Set environment variables for the build. - -# The shell in which to execute make rules. -SHELL = /bin/sh - -# The CMake executable. -CMAKE_COMMAND = /usr/bin/cmake - -# The command to remove a file. -RM = /usr/bin/cmake -E remove -f - -# Escaping for special characters. -EQUALS = = - -# The top-level source directory on which CMake was run. -CMAKE_SOURCE_DIR = /home/pama/azaion/autopilot/src - -# The top-level build directory on which CMake was run. -CMAKE_BINARY_DIR = /home/pama/azaion/autopilot/src - -#============================================================================= -# Targets provided globally by CMake. - -# Special rule for the target rebuild_cache -rebuild_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..." - /usr/bin/cmake -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) -.PHONY : rebuild_cache - -# Special rule for the target rebuild_cache -rebuild_cache/fast: rebuild_cache - -.PHONY : rebuild_cache/fast - -# Special rule for the target edit_cache -edit_cache: - @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..." - /usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available. -.PHONY : edit_cache - -# Special rule for the target edit_cache -edit_cache/fast: edit_cache - -.PHONY : edit_cache/fast - -# The main all target -all: cmake_check_build_system - $(CMAKE_COMMAND) -E cmake_progress_start /home/pama/azaion/autopilot/src/CMakeFiles /home/pama/azaion/autopilot/src/CMakeFiles/progress.marks - $(MAKE) -f CMakeFiles/Makefile2 all - $(CMAKE_COMMAND) -E cmake_progress_start /home/pama/azaion/autopilot/src/CMakeFiles 0 -.PHONY : all - -# The main clean target -clean: - $(MAKE) -f CMakeFiles/Makefile2 clean -.PHONY : clean - -# The main clean target -clean/fast: clean - -.PHONY : clean/fast - -# Prepare targets for installation. -preinstall: all - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall - -# Prepare targets for installation. -preinstall/fast: - $(MAKE) -f CMakeFiles/Makefile2 preinstall -.PHONY : preinstall/fast - -# clear depends -depend: - $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1 -.PHONY : depend - -#============================================================================= -# Target rules for targets named azaion - -# Build rule for target. -azaion: cmake_check_build_system - $(MAKE) -f CMakeFiles/Makefile2 azaion -.PHONY : azaion - -# fast build rule for target. -azaion/fast: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/build -.PHONY : azaion/fast - -az_action_point.o: az_action_point.cpp.o - -.PHONY : az_action_point.o - -# target to build an object file -az_action_point.cpp.o: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_action_point.cpp.o -.PHONY : az_action_point.cpp.o - -az_action_point.i: az_action_point.cpp.i - -.PHONY : az_action_point.i - -# target to preprocess a source file -az_action_point.cpp.i: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_action_point.cpp.i -.PHONY : az_action_point.cpp.i - -az_action_point.s: az_action_point.cpp.s - -.PHONY : az_action_point.s - -# target to generate assembly for a file -az_action_point.cpp.s: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_action_point.cpp.s -.PHONY : az_action_point.cpp.s - -az_coordinate.o: az_coordinate.cpp.o - -.PHONY : az_coordinate.o - -# target to build an object file -az_coordinate.cpp.o: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_coordinate.cpp.o -.PHONY : az_coordinate.cpp.o - -az_coordinate.i: az_coordinate.cpp.i - -.PHONY : az_coordinate.i - -# target to preprocess a source file -az_coordinate.cpp.i: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_coordinate.cpp.i -.PHONY : az_coordinate.cpp.i - -az_coordinate.s: az_coordinate.cpp.s - -.PHONY : az_coordinate.s - -# target to generate assembly for a file -az_coordinate.cpp.s: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_coordinate.cpp.s -.PHONY : az_coordinate.cpp.s - -az_fly.o: az_fly.cpp.o - -.PHONY : az_fly.o - -# target to build an object file -az_fly.cpp.o: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_fly.cpp.o -.PHONY : az_fly.cpp.o - -az_fly.i: az_fly.cpp.i - -.PHONY : az_fly.i - -# target to preprocess a source file -az_fly.cpp.i: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_fly.cpp.i -.PHONY : az_fly.cpp.i - -az_fly.s: az_fly.cpp.s - -.PHONY : az_fly.s - -# target to generate assembly for a file -az_fly.cpp.s: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_fly.cpp.s -.PHONY : az_fly.cpp.s - -az_mission.o: az_mission.cpp.o - -.PHONY : az_mission.o - -# target to build an object file -az_mission.cpp.o: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_mission.cpp.o -.PHONY : az_mission.cpp.o - -az_mission.i: az_mission.cpp.i - -.PHONY : az_mission.i - -# target to preprocess a source file -az_mission.cpp.i: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_mission.cpp.i -.PHONY : az_mission.cpp.i - -az_mission.s: az_mission.cpp.s - -.PHONY : az_mission.s - -# target to generate assembly for a file -az_mission.cpp.s: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/az_mission.cpp.s -.PHONY : az_mission.cpp.s - -main.o: main.cpp.o - -.PHONY : main.o - -# target to build an object file -main.cpp.o: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/main.cpp.o -.PHONY : main.cpp.o - -main.i: main.cpp.i - -.PHONY : main.i - -# target to preprocess a source file -main.cpp.i: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/main.cpp.i -.PHONY : main.cpp.i - -main.s: main.cpp.s - -.PHONY : main.s - -# target to generate assembly for a file -main.cpp.s: - $(MAKE) -f CMakeFiles/azaion.dir/build.make CMakeFiles/azaion.dir/main.cpp.s -.PHONY : main.cpp.s - -# Help Target -help: - @echo "The following are some of the valid targets for this Makefile:" - @echo "... all (the default if no target is provided)" - @echo "... clean" - @echo "... depend" - @echo "... rebuild_cache" - @echo "... edit_cache" - @echo "... azaion" - @echo "... az_action_point.o" - @echo "... az_action_point.i" - @echo "... az_action_point.s" - @echo "... az_coordinate.o" - @echo "... az_coordinate.i" - @echo "... az_coordinate.s" - @echo "... az_fly.o" - @echo "... az_fly.i" - @echo "... az_fly.s" - @echo "... az_mission.o" - @echo "... az_mission.i" - @echo "... az_mission.s" - @echo "... main.o" - @echo "... main.i" - @echo "... main.s" -.PHONY : help - - - -#============================================================================= -# Special targets to cleanup operation of make. - -# Special rule to run CMake to check the build system integrity. -# No rule that depends on this can have commands that come from listfiles -# because they might be regenerated. -cmake_check_build_system: - $(CMAKE_COMMAND) -S$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 -.PHONY : cmake_check_build_system - diff --git a/src/autopilot.pro b/src/autopilot.pro new file mode 100644 index 0000000..e1ce58d --- /dev/null +++ b/src/autopilot.pro @@ -0,0 +1,26 @@ +QT = core +CONFIG += c++17 cmdline link_pkgconfig +PKGCONFIG += mavsdk + +QMAKE_CXXFLAGS += -Wno-address-of-packed-member + +SOURCES += \ + az_action_point.cpp \ + az_coordinate.cpp \ + az_drone_controller.cpp \ + az_mission.cpp \ + az_mission_controller.cpp \ + az_utils.cpp \ + main.cpp + +target.path = /opt/$${TARGET}/bin +!isEmpty(target.path): INSTALLS += target + +HEADERS += \ + az_action_point.h \ + az_config.h \ + az_coordinate.h \ + az_drone_controller.h \ + az_mission.h \ + az_mission_controller.h \ + az_utils.h diff --git a/src/az_config.h b/src/az_config.h new file mode 100644 index 0000000..737390b --- /dev/null +++ b/src/az_config.h @@ -0,0 +1,6 @@ +#pragma once + +#define FUNCTION_NAME(func) func() + +const char *AZ_CONNECTION = "udp://:14550"; +const int AZ_RELATIVE_FLY_ALTITUDE = 50; diff --git a/src/az_drone_controller.cpp b/src/az_drone_controller.cpp new file mode 100644 index 0000000..db3b1d2 --- /dev/null +++ b/src/az_drone_controller.cpp @@ -0,0 +1,232 @@ +#include +#include +#include +#include +#include +#include +#include +#include "az_config.h" +#include "az_drone_controller.h" + +AzDroneController::AzDroneController(AzMission &mission, QObject *parent) : + QObject(parent), + mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}, // TODO!! Autopilot or CompanionComputer? + mDroneState(AZ_DRONE_STATE_DISCONNECTED) +{ + qDebug() << "AzDroneController::AzDroneController() Thread ID: " << QThread::currentThreadId(); + mFirstPosition.relative_altitude_m = -10000; + + mMissionController = new AzMissionController(mission, this); + connect(mMissionController, &AzMissionController::missionProgressChanged, this, &AzDroneController::missionIndexChangedSlot); + connect(mMissionController, &AzMissionController::finished, this, &AzDroneController::missionFinishedSlot); +} + + +void AzDroneController::start() +{ + // Must wait that main event loop is launched in main() + delayedStateCallSlot(0); +} + + +void AzDroneController::delayedStateCallSlot(int ms) +{ + QTimer::singleShot(ms, this, &AzDroneController::droneStateMachineSlot); +} + + +bool AzDroneController::stateConnect(void) +{ + return mMavsdk.add_any_connection(AZ_CONNECTION) == ConnectionResult::Success; +} + + +bool AzDroneController::stateAutopilot(void) +{ + return (mSystem = mMavsdk.first_autopilot(3.0).value()) != nullptr; +} + + +bool AzDroneController::stateTelemetryModule(void) +{ + if ((mTelemetry = new Telemetry(mSystem)) == nullptr) { + return false; + } + + const auto setRateResult = mTelemetry->set_rate_position(1); // 1 Hz + if (setRateResult != Telemetry::Result::Success) { + qCritical() << "Setting rate failed: " << (int)setRateResult; + return false; + } + + // Subscripe to position updates. Updated comes from different MAVSDK thread and so + // we send it as event to this function so that it's handled in the main thread. + connect(this, &AzDroneController::newPosition, this, &AzDroneController::newPositionSlot, Qt::QueuedConnection); + mTelemetry->subscribe_position([this](Telemetry::Position position) { + emit newPosition(position); + }); + + Telemetry::Position home = mTelemetry->home(); + qDebug() << "Home: lat:" << home.latitude_deg + << "lon:" << home.longitude_deg + << "altitude abs:" << home.absolute_altitude_m + << "altitude rel:" << home.relative_altitude_m; + + return true; +} + + +bool AzDroneController::stateActionModule(void) +{ + mAction = new Action(mSystem); + if (mAction == nullptr) { + return false; + } + + mMissionController->setAction(mAction); + return true; +} + + +bool AzDroneController::stateReadyForArming(void) +{ + return mTelemetry->health_all_ok(); +} + + +bool AzDroneController::stateArm(void) +{ + return mAction->arm() == Action::Result::Success; +} + + +bool AzDroneController::stateTakeoff(void) +{ + mAction->set_takeoff_altitude(50); + qDebug() << "AzDroneController::stateTakeoff() taking off starts"; + Action::Result result = mAction->takeoff(); //return mAction->takeoff() == Action::Result::Success; + qDebug() << "AzDroneController::stateTakeoff() taking off ends"; + + return result == Action::Result::Success; +} + + +bool AzDroneController::stateFlyMission(void) +{ + float flight_altitude_abs = AZ_RELATIVE_FLY_ALTITUDE + mFirstPosition.absolute_altitude_m; + return mMissionController->startMissions(flight_altitude_abs); +} + + +void AzDroneController::missionFinishedSlot(void) +{ + qDebug() << "AzDroneController::missionFinishedSlot() Landing the drone."; + mAction->land(); + // TODO!! This must be improved a lot. + QTimer::singleShot(20000, this, &AzDroneController::disarmDroneSlot); +} + + +void AzDroneController::disarmDroneSlot(void) +{ + qDebug() << "AzDroneController::disarmDroneSlot() Disarming the drone and quit."; + // TODO!! This must be improved a lot. + mAction->disarm(); + QCoreApplication::instance()->quit(); +} + + +void AzDroneController::droneStateMachineSlot(void) +{ + static QString states[] = { + "DISCONNECTED", + "CONNECTED", + "AUTOPILOT", + "TELEMETRY_MODULE", + "ACTION_MODULE", + "READY_FOR_ARMING", + "ARMED", + "TAKE_OFF", + "FLY_MISSION", + "LAND", + }; + + qDebug() << ""; + qDebug() << "AzDroneController::droneStateMachineSlot() Current state:" << states[(int)mDroneState]; + + using MethodPtr = bool (AzDroneController::*)(); + + struct MethodInfo { + AzDroneState currentState; + MethodPtr nextStateMethodPtr; + QString description; + int nextStateDelay; + }; + + const MethodInfo stateMethods[] = { + { AZ_DRONE_STATE_DISCONNECTED, &AzDroneController::stateConnect, "stateConnect()", 1000 }, + { AZ_DRONE_STATE_CONNECTED, &AzDroneController::stateAutopilot, "stateAutopilot()", 1000 }, + { AZ_DRONE_STATE_AUTOPILOT, &AzDroneController::stateTelemetryModule, "stateTelemetryModule()", 1000 }, + { AZ_DRONE_STATE_TELEMETRY_MODULE, &AzDroneController::stateActionModule, "stateActionModule()", 1000 }, + { AZ_DRONE_STATE_ACTION_MODULE, &AzDroneController::stateReadyForArming, "stateReadyForArming()", 1000 }, + { AZ_DRONE_STATE_READY_FOR_ARMING, &AzDroneController::stateArm, "stateArm()", 1000 }, + { AZ_DRONE_STATE_ARMED, &AzDroneController::stateTakeoff, "stateTakeoff()", 10000 }, + { AZ_DRONE_STATE_TAKE_OFF, &AzDroneController::stateFlyMission, "stateFlyMission()", 1000 }, + { AZ_DRONE_STATE_FLY_MISSION, nullptr, "No function to call. AzMissionControl running", 0 } + }; + + for (uint i = 0; i < sizeof(stateMethods) / sizeof(MethodInfo); i++) { + if (stateMethods[i].currentState == mDroneState) { + if ((int)mDroneState == (int)AZ_DRONE_STATE_FLY_MISSION) { + qDebug() << "AzMissionContoroller running. Waiting signals from it."; + return; + } + + bool result = (this->*stateMethods[i].nextStateMethodPtr)(); + + if (result) { + qDebug() << "AzDroneController::droneStateMachineSlot()" << stateMethods[i].description << "succeeded"; + qDebug() << "AzDroneController::droneStateMachineSlot() calling next state function in" << stateMethods[i].nextStateDelay << "ms."; + mDroneState = stateMethods[i + 1].currentState; + qDebug() << "AzDroneController::droneStateMachineSlot() New state:" << states[(int)mDroneState]; + } + else { + qDebug() << "AzDroneController::droneStateMachineSlot() << stateMethods[i].description failed. Trying again."; + } + + //if (stateMethods[i].nextStateDelay > 0 && stateMethods[i].nextStateMethodPtr != nullptr) { + delayedStateCallSlot(stateMethods[i].nextStateDelay); + //} + + break; + } + } +} + + +void AzDroneController::newPositionSlot(Telemetry::Position position) +{ + // Save first position. It will be used later to set altitude for missions. + // TODO!! Probably we want to use rangefinder or at least barometer with altitude from the map later. + if (mFirstPosition.relative_altitude_m < -1000) { + qDebug() << "AzDroneController::newPositionSlot()" + << "First altitude" << mFirstPosition.relative_altitude_m + << "lat:" << position.latitude_deg + << "lon:" << position.longitude_deg; + mFirstPosition = position; + } + + if (mDroneState == AZ_DRONE_STATE_FLY_MISSION) { + mMissionController->newPosition(position); + } + else { + qDebug() << "AzDroneController::newPositionSlot() altitude rel:" << position.relative_altitude_m + << "altitude abs:" << position.absolute_altitude_m; + } +} + + +void AzDroneController::missionIndexChangedSlot(int currentIndex, int totalIndexes) +{ + qDebug() << "AzDroneController::missionIndexChanged()" << currentIndex << "/" << totalIndexes; +} diff --git a/src/az_drone_controller.h b/src/az_drone_controller.h new file mode 100644 index 0000000..8cdeffb --- /dev/null +++ b/src/az_drone_controller.h @@ -0,0 +1,64 @@ +#pragma once + +#include + +#include + +#include +#include +#include + +#include "az_mission.h" +#include "az_mission_controller.h" + +using namespace mavsdk; + +typedef enum { + AZ_DRONE_STATE_DISCONNECTED, + AZ_DRONE_STATE_CONNECTED, + AZ_DRONE_STATE_AUTOPILOT, + AZ_DRONE_STATE_TELEMETRY_MODULE, + AZ_DRONE_STATE_ACTION_MODULE, + AZ_DRONE_STATE_READY_FOR_ARMING, + AZ_DRONE_STATE_ARMED, + AZ_DRONE_STATE_TAKE_OFF, + AZ_DRONE_STATE_FLY_MISSION, + AZ_DRONE_STATE_LAND, + AZ_DRONE_STATE_END, +} AzDroneState; + +class AzDroneController : public QObject +{ + Q_OBJECT +public: + explicit AzDroneController(AzMission &mission, QObject *parent = nullptr); + void start(); + bool stateConnect(void); + bool stateAutopilot(void); + bool stateTelemetryModule(void); + bool stateActionModule(void); + bool stateReadyForArming(void); + bool stateArm(void); + bool stateTakeoff(void); + bool stateFlyMission(void); + +private slots: + void droneStateMachineSlot(void); + void delayedStateCallSlot(int ms); + void newPositionSlot(Telemetry::Position); + void missionIndexChangedSlot(int currentIndex, int totalIndexes); + void missionFinishedSlot(void); + void disarmDroneSlot(void); + +signals: + void newPosition(Telemetry::Position); + +private: + Mavsdk mMavsdk; + AzDroneState mDroneState; + std::shared_ptr mSystem; + Telemetry *mTelemetry; + Action *mAction; + Telemetry::Position mFirstPosition; + AzMissionController *mMissionController; +}; diff --git a/src/az_fly.cpp b/src/az_fly.cpp deleted file mode 100644 index bf6a9c6..0000000 --- a/src/az_fly.cpp +++ /dev/null @@ -1,213 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "az_action_point.h" -#include "az_fly.h" -#include "az_mission.h" - -using namespace std; -using chrono::seconds; -using this_thread::sleep_for; - -AzFly::AzFly(AzMission &mission) - : mAzMission(mission) - , mCurrentMissionIndex(0) -{} - -AzFly::~AzFly() -{ - delete mTelemetry; - delete mMission; - delete mAction; - delete mOffboard; -} - -vector AzFly::getMissionItems(void) -{ - vector missionItems; - const vector &actionPoints = mAzMission.getActionPoints(); - - for (int i = 0; i < actionPoints.size(); i++) { - Mission::MissionItem new_item{}; - new_item.latitude_deg = actionPoints[i].getPoint().latitude; - new_item.longitude_deg = actionPoints[i].getPoint().longitude; - new_item.relative_altitude_m = 10; - new_item.speed_m_s = 15; - new_item.is_fly_through = actionPoints[i].getType() == AZ_ACTION_POINT_TYPE_WAYPOINT ? true : false; - new_item.gimbal_pitch_deg = -45; - new_item.gimbal_yaw_deg = -30; - new_item.camera_action = Mission::MissionItem::CameraAction::TakePhoto; - missionItems.push_back(new_item); - } - - return missionItems; -} - -bool AzFly::initialize(void) -{ - ConnectionResult connection_result = mMavsdk.add_any_connection("udp://:14550"); - - if (connection_result != ConnectionResult::Success) { - cerr << "Connection failed: " << connection_result << '\n'; - return false; - } - - mSystem = mMavsdk.first_autopilot(3.0); - if (!mSystem) { - cerr << "Timed out waiting for system\n"; - return false; - } - else { - cout << "Connection OK\n"; - } - - mAction = new Action{mSystem.value()}; - mMission = new Mission{mSystem.value()}; - mTelemetry = new Telemetry{mSystem.value()}; - mOffboard = new Offboard{mSystem.value()}; - - while (!mTelemetry->health_all_ok()) { - cout << "Waiting for system to be ready\n"; - sleep_for(seconds(1)); - } - - cout << "System ready. Creating and uploading mission." << endl; - - return true; -} - -void AzFly::findAndDetroyThreadFunc(void) -{ - auto prom = std::make_shared>(); - auto future_result = prom->get_future(); - - mMission->pause_mission_async([prom](Mission::Result result) { prom->set_value(result); }); - const Mission::Result result = future_result.get(); - - if (result != Mission::Result::Success) { - cout << "Failed to pause mission" << endl; - return; - } - else { - cout << "Starting Offboard mode. Flying small circle and try to find the target." << endl; - - const Offboard::VelocityNedYaw stay{}; - mOffboard->set_velocity_ned(stay); - - Offboard::Result offboard_result = mOffboard->start(); - if (offboard_result != Offboard::Result::Success) { - std::cerr << "Offboard::start() failed: " << offboard_result << '\n'; - return; - } - - const float radius = 10.0f; // Radius of the circle in meters - const float velocity = 2.0f; // Velocity of the drone in m/s - const float angular_velocity = velocity / radius; // Angular velocity in rad/s - - for (float t = 0; t < 2 * M_PI; t += 0.1) { - Offboard::VelocityBodyYawspeed velocity_sp{velocity * cos(t), velocity * sin(t), 0.0f, angular_velocity}; - mOffboard->set_velocity_body(velocity_sp); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - - // Stop Offboard mode after completing the circle. - Offboard::VelocityBodyYawspeed stop_velocity{0.0f, 0.0f, 0.0f, 0.0f}; - mOffboard->set_velocity_body(stop_velocity); - std::this_thread::sleep_for(std::chrono::seconds(1)); - offboard_result = mOffboard->stop(); - if (offboard_result != Offboard::Result::Success) { - std::cerr << "Offboard::stop() failed: " << offboard_result << endl; - return; - } - - cout << "Stopping Offboard succeeded. Continuing to the next mission item." << endl; - - mMission->start_mission(); - } -} - -bool AzFly::start(void) -{ - vector mission_items = getMissionItems(); - - cout << "Uploading mission...\n"; - Mission::MissionPlan mission_plan{}; - mission_plan.mission_items = mission_items; - const Mission::Result upload_result = mMission->upload_mission(mission_plan); - - if (upload_result != Mission::Result::Success) { - cerr << "Mission upload failed: " << upload_result << ", exiting." << endl; - return 1; - } - - cout << "Arming...\n"; - const Action::Result arm_result = mAction->arm(); - if (arm_result != Action::Result::Success) { - cerr << "Arming failed: " << arm_result << '\n'; - return 1; - } - cout << "Armed.\n"; - - // Before starting the mission, we want to be sure to subscribe to the mission progress. - mMission->subscribe_mission_progress([this](Mission::MissionProgress mission_progress) { - mCurrentMissionIndex = mission_progress.current; - - if (mCurrentMissionIndex > 0) { - cout << "\nMission status update: " << mCurrentMissionIndex << "/" << mission_progress.total << "." << endl; - - const vector &actionPoints = mAzMission.getActionPoints(); - string actionTypeStr = actionPoints[mCurrentMissionIndex - 1].getTypeStr(); - string actionSpesificStr = actionPoints[mCurrentMissionIndex - 1].getActionSpecificStr(); - AzActionPointType actionType = actionPoints[mCurrentMissionIndex - 1].getType(); - cout << "Mission type: " << actionTypeStr << ". "; - if (actionType == AZ_ACTION_POINT_TYPE_WAYPOINT) { - cout << "Flying through waypoint." << endl; - } - else if (actionType == AZ_ACTION_POINT_TYPE_SEARCH) { - cout << "Pausing mission and searching " << actionSpesificStr << " target" << endl; - std::thread search_thread(&AzFly::findAndDetroyThreadFunc, this); - search_thread.detach(); - } - else if (actionType == AZ_ACTION_POINT_TYPE_RETURN) { - cout << "Arrived to the return point. Landing next." << endl; - } - } - }); - - Mission::Result start_mission_result = mMission->start_mission(); - if (start_mission_result != Mission::Result::Success) { - cerr << "Starting mission failed: " << start_mission_result << endl; - return 1; - } - - while (!mMission->is_mission_finished().second) { - sleep_for(seconds(1)); - } - - const Action::Result landingResults = mAction->land(); - if (landingResults != Action::Result::Success) { - cout << "Failed to land: " << landingResults << endl; - return 1; - } - - // We need to wait a bit, otherwise the armed state might not be correct yet. - sleep_for(seconds(2)); - - // Wait until we're done. - while (mTelemetry->armed()) { - sleep_for(seconds(1)); - } - - cout << "Disarmed, exiting." << endl; - - return true; -} diff --git a/src/az_fly.h b/src/az_fly.h deleted file mode 100644 index dad1737..0000000 --- a/src/az_fly.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include "az_mission.h" -#include -#include -#include -#include -#include - -using namespace mavsdk; -using namespace std; - -class AzFly -{ -public: - AzFly(AzMission &mission); - ~AzFly(); - bool initialize(void); - bool start(void); - -private: - Mavsdk mMavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - optional> mSystem; - Action *mAction; - Mission *mMission; - Telemetry *mTelemetry; - Offboard *mOffboard; - - AzMission &mAzMission; - vector getMissionItems(void); - void findAndDetroyThreadFunc(void); - int mCurrentMissionIndex; -}; diff --git a/src/az_mission.cpp b/src/az_mission.cpp index 9f1b566..088f7de 100644 --- a/src/az_mission.cpp +++ b/src/az_mission.cpp @@ -11,10 +11,10 @@ using namespace rapidjson; using namespace std; -AzMission::AzMission(string filename) - : mFilename(filename) - , mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE)) - , mOperationalHeight(INVALID_HEIGHT) +AzMission::AzMission(string filename) : + mFilename(filename), + mOperationalHeight(INVALID_HEIGHT), + mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE)) { if (filesystem::exists(mFilename) == false) { cerr << "JSON file " << mFilename << " doesn't exists." << endl; @@ -137,7 +137,7 @@ ostream &operator<<(ostream &os, const AzMission &obj) os << "Operational height: " << obj.mOperationalHeight << endl; os << "Return point: " << obj.mReturnPoint << endl; - for (int i = 0; i < obj.mActionPoints.size(); i++) { + for (size_t i = 0; i < obj.mActionPoints.size(); i++) { os << "\nAction point[" << i << "]:" << endl; os << obj.mActionPoints[i]; } diff --git a/src/az_mission_controller.cpp b/src/az_mission_controller.cpp new file mode 100644 index 0000000..2202dfe --- /dev/null +++ b/src/az_mission_controller.cpp @@ -0,0 +1,92 @@ +#include +#include +#include "az_mission_controller.h" +#include "az_mission.h" +#include "az_utils.h" + + +AzMissionController::AzMissionController(AzMission &mission, QObject *parent) : + QObject(parent), + mMission(mission), + mAction(nullptr), + mMissionStarted(false), + mAbsoluteAltitude(-10000) +{ +} + + +void AzMissionController::setAction(Action *action) +{ + mAction = action; +} + + +bool AzMissionController::startMissions(float absoluteAltitude) +{ + mCurrentMissionIndex = -1; + mMissionStarted = true; + mAbsoluteAltitude = absoluteAltitude; // TODO!! Use altitudes from the JSON file. + return flyToNextMissionItem(); +} + + +void AzMissionController::stopMissions(void) +{ + // TODO!! Needs to be improved. At least send signal to AzDroneController. + mMissionStarted = false; +} + + +bool AzMissionController::flyToNextMissionItem(void) +{ + mCurrentMissionIndex++; + + if (mCurrentMissionIndex >= (int)mMission.getActionPoints().size()) { + qDebug() << "AzMissionController::flyToNextMissionItem() All mission items are handled. Stopping mission handling."; + mMissionStarted = false; + emit finished(); + return true; + } + + qDebug() << "AzMissionController::flyToNextMissionItem() flying to the item" + << mCurrentMissionIndex + 1 << "/" << mMission.getActionPoints().size(); + + const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint(); + 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"; + + if (result == Action::Result::Success) { + emit missionProgressChanged(mCurrentMissionIndex + 1, mMission.getActionPoints().size()); + return true; + } + + return false; +} + + +void AzMissionController::newPosition(Telemetry::Position position) +{ + (void)position; + + if (mMissionStarted == false) { + return; + } + + const AzCoordinate &coordinate = mMission.getActionPoints().at(mCurrentMissionIndex).getPoint(); + float distanceToTarget = AzUtils::distance(position.latitude_deg, + position.longitude_deg, + coordinate.latitude, + coordinate.longitude); + + qDebug() << "AzMissionController::newPosition() distanceToTarget: " << distanceToTarget; + + // TODO!! In final application we need to + if (distanceToTarget <= 0.01) { + qDebug() << "AzMissionController::newPosition() target reached. Continuing to the next item."; + flyToNextMissionItem(); + } +} diff --git a/src/az_mission_controller.h b/src/az_mission_controller.h new file mode 100644 index 0000000..2435062 --- /dev/null +++ b/src/az_mission_controller.h @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include "az_mission.h" +#include "plugins/action/action.h" + +using namespace mavsdk; + +class AzMissionController : public QObject +{ + Q_OBJECT +public: + explicit AzMissionController(AzMission &mission, QObject *parent = nullptr); + void setAction(Action *action); + bool startMissions(float absoluteAltitude); + void stopMissions(void); + +public slots: + void newPosition(Telemetry::Position position); + +signals: + void missionProgressChanged(int currentIndex, int totalIndexes); + void finished(void); + +private: + bool flyToNextMissionItem(void); + + AzMission &mMission; + Action *mAction; + int mCurrentMissionIndex; + bool mMissionStarted; + float mAbsoluteAltitude; +}; diff --git a/src/az_utils.cpp b/src/az_utils.cpp new file mode 100644 index 0000000..ecfc2ab --- /dev/null +++ b/src/az_utils.cpp @@ -0,0 +1,18 @@ +#include +#include "az_utils.h" + +AzUtils::AzUtils() {} + +#define EARTH_RADIUS 6371.0 // Earth radius in kilometers + +double degrees_to_radians(double degrees) { + return degrees * M_PI / 180.0; +} + +double AzUtils::distance(double lat1, double lon1, double lat2, double lon2) { + double dlat = degrees_to_radians(lat2 - lat1); + double dlon = degrees_to_radians(lon2 - lon1); + double a = sin(dlat/2) * sin(dlat/2) + cos(degrees_to_radians(lat1)) * cos(degrees_to_radians(lat2)) * sin(dlon/2) * sin(dlon/2); + double c = 2 * atan2(sqrt(a), sqrt(1-a)); + return EARTH_RADIUS * c; +} diff --git a/src/az_utils.h b/src/az_utils.h new file mode 100644 index 0000000..bc4bd73 --- /dev/null +++ b/src/az_utils.h @@ -0,0 +1,9 @@ +#pragma once + +class AzUtils +{ +public: + AzUtils(); + + static double distance(double lat1, double lon1, double lat2, double lon2); +}; diff --git a/src/fly_mission.cpp b/src/fly_mission.cpp deleted file mode 100644 index cff7d15..0000000 --- a/src/fly_mission.cpp +++ /dev/null @@ -1,230 +0,0 @@ -// -// Demonstrates how to add and fly Waypoint missions using the MAVSDK. -// - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -using namespace mavsdk; -using std::chrono::seconds; -using std::this_thread::sleep_for; - -Mission::MissionItem make_mission_item( - double latitude_deg, - double longitude_deg, - float relative_altitude_m, - float speed_m_s, - bool is_fly_through, - float gimbal_pitch_deg, - float gimbal_yaw_deg, - Mission::MissionItem::CameraAction camera_action) -{ - Mission::MissionItem new_item{}; - new_item.latitude_deg = latitude_deg; - new_item.longitude_deg = longitude_deg; - new_item.relative_altitude_m = relative_altitude_m; - new_item.speed_m_s = speed_m_s; - new_item.is_fly_through = is_fly_through; - new_item.gimbal_pitch_deg = gimbal_pitch_deg; - new_item.gimbal_yaw_deg = gimbal_yaw_deg; - new_item.camera_action = camera_action; - return new_item; -} - -void usage(const std::string &bin_name) -{ - std::cerr << "Usage : " << bin_name << " \n" - << "Connection URL format should be :\n" - << " For TCP : tcp://[server_host][:server_port]\n" - << " For UDP : udp://[bind_host][:bind_port]\n" - << " For Serial : serial:///path/to/serial/dev[:baudrate]\n" - << "For example, to connect to the simulator use URL: udp://:14540\n"; -} - -int main(int argc, char **argv) -{ - if (argc != 2) { - usage(argv[0]); - return 1; - } - - Mavsdk mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}}; - ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]); - - if (connection_result != ConnectionResult::Success) { - std::cerr << "Connection failed: " << connection_result << '\n'; - return 1; - } - - auto system = mavsdk.first_autopilot(3.0); - if (!system) { - std::cerr << "Timed out waiting for system\n"; - return 1; - } - else { - std::cout << "Connection OK\n"; - } - - auto action = Action{system.value()}; - auto mission = Mission{system.value()}; - auto telemetry = Telemetry{system.value()}; - - while (!telemetry.health_all_ok()) { - std::cout << "Waiting for system to be ready\n"; - sleep_for(seconds(1)); - } - - std::cout << "System ready\n"; - std::cout << "Creating and uploading mission\n"; - - std::vector mission_items; - - mission_items.push_back(make_mission_item( - 47.398170327054473, - 8.5456490218639658, - 10.0f, - 5.0f, - false, - 20.0f, - 60.0f, - Mission::MissionItem::CameraAction::None)); - - mission_items.push_back(make_mission_item( - 47.398241338125118, - 8.5455360114574432, - 10.0f, - 2.0f, - true, - 0.0f, - -60.0f, - Mission::MissionItem::CameraAction::TakePhoto)); - - mission_items.push_back(make_mission_item( - 47.398139363821485, - 8.5453846156597137, - 10.0f, - 5.0f, - true, - -45.0f, - 0.0f, - Mission::MissionItem::CameraAction::StartVideo)); - - mission_items.push_back(make_mission_item( - 47.398058617228855, - 8.5454618036746979, - 10.0f, - 2.0f, - false, - -90.0f, - 30.0f, - Mission::MissionItem::CameraAction::StopVideo)); - - mission_items.push_back(make_mission_item( - 47.398100366082858, - 8.5456969141960144, - 10.0f, - 5.0f, - false, - -45.0f, - -30.0f, - Mission::MissionItem::CameraAction::StartPhotoInterval)); - - mission_items.push_back(make_mission_item( - 47.398001890458097, - 8.5455576181411743, - 10.0f, - 5.0f, - false, - 0.0f, - 0.0f, - Mission::MissionItem::CameraAction::StopPhotoInterval)); - - std::cout << "Uploading mission...\n"; - Mission::MissionPlan mission_plan{}; - mission_plan.mission_items = mission_items; - const Mission::Result upload_result = mission.upload_mission(mission_plan); - - if (upload_result != Mission::Result::Success) { - std::cerr << "Mission upload failed: " << upload_result << ", exiting.\n"; - return 1; - } - - std::cout << "Arming...\n"; - const Action::Result arm_result = action.arm(); - if (arm_result != Action::Result::Success) { - std::cerr << "Arming failed: " << arm_result << '\n'; - return 1; - } - std::cout << "Armed.\n"; - - std::atomic want_to_pause{false}; - // Before starting the mission, we want to be sure to subscribe to the - // mission progress. - mission.subscribe_mission_progress([&want_to_pause](Mission::MissionProgress mission_progress) { - std::cout << "Mission status update: " << mission_progress.current << " / " << mission_progress.total << '\n'; - - if (mission_progress.current >= 2) { - // We can only set a flag here. If we do more request inside the - // callback, we risk blocking the system. - want_to_pause = true; - } - }); - - Mission::Result start_mission_result = mission.start_mission(); - if (start_mission_result != Mission::Result::Success) { - std::cerr << "Starting mission failed: " << start_mission_result << '\n'; - return 1; - } - - while (!want_to_pause) { - sleep_for(seconds(1)); - } - - std::cout << "Pausing mission...\n"; - const Mission::Result pause_mission_result = mission.pause_mission(); - - if (pause_mission_result != Mission::Result::Success) { - std::cerr << "Failed to pause mission:" << pause_mission_result << '\n'; - } - std::cout << "Mission paused.\n"; - - // Pause for 5 seconds. - sleep_for(seconds(5)); - - // Then continue. - Mission::Result start_mission_again_result = mission.start_mission(); - if (start_mission_again_result != Mission::Result::Success) { - std::cerr << "Starting mission again failed: " << start_mission_again_result << '\n'; - return 1; - } - - while (!mission.is_mission_finished().second) { - sleep_for(seconds(1)); - } - - // We are done, and can do RTL to go home. - std::cout << "Commanding RTL...\n"; - const Action::Result rtl_result = action.return_to_launch(); - if (rtl_result != Action::Result::Success) { - std::cout << "Failed to command RTL: " << rtl_result << '\n'; - return 1; - } - std::cout << "Commanded RTL.\n"; - - // We need to wait a bit, otherwise the armed state might not be correct yet. - sleep_for(seconds(2)); - - while (telemetry.armed()) { - // Wait until we're done. - sleep_for(seconds(1)); - } - std::cout << "Disarmed, exiting.\n"; -} diff --git a/src/main.cpp b/src/main.cpp index 8dc3b1d..4534228 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,23 +1,22 @@ -#include - -#include "az_fly.h" +#include +#include #include "az_mission.h" - -using namespace std; +#include "az_drone_controller.h" int main(int argc, char *argv[]) { + QCoreApplication a(argc, argv); + if (argc != 2) { - cout << "Plese give mission file as an argument\n"; + qCritical() << "Please give mission file as an argument\n"; return 1; } AzMission mission(argv[1]); cout << mission; - AzFly fly(mission); - fly.initialize(); - fly.start(); + AzDroneController droneController(mission); + droneController.start(); - return 0; + return a.exec(); } diff --git a/src/mission.json b/src/mission.json index 737ffe4..5b48d66 100644 --- a/src/mission.json +++ b/src/mission.json @@ -7,12 +7,12 @@ ] }, "action_points": [ - {"point": [52.53175307, 13.386865865], "height": 500, "action_enum": "waypoint"}, - {"point": [52.53175307, 13.387710935], "height": 400, "action_enum": "search", "action_specific": {"targets": ["artillery"]}}, - {"point": [52.53121053, 13.386865865], "height": 300, "action_enum": "search", "action_specific": {"targets": ["artillery", "tank"]}}, - {"point": [52.53121053, 13.387710935], "height": 300, "action_enum": "search", "action_specific": {"targets": ["tank"]}}, - {"point": [52.53148180, 13.387288400], "height": 500, "action_enum": "return"} + {"point": [-35.35967231, 149.16146047], "height": 500, "action_enum": "waypoint"}, + {"point": [-35.35973951, 149.16801175], "height": 400, "action_enum": "search", "action_specific": {"targets": ["artillery"]}}, + {"point": [-35.36408532, 149.16816283], "height": 300, "action_enum": "search", "action_specific": {"targets": ["artillery", "tank"]}}, + {"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": [99.99, 11.11] + "return_point": [-35.36218126, 149.16505887] }