From f7acface7f9d82478cd9d1653639010007e06c66 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tuomas=20J=C3=A4rvinen?= Date: Mon, 18 Mar 2024 22:31:38 +0100 Subject: [PATCH] Initial version. Can fly simple mission defined in JSON file. Please check README.md how to install dependencies and run the application. --- .gitmodules | 9 ++ 3rd/mavsdk | 1 + 3rd/px4 | 1 + 3rd/rapidjson | 1 + README.md | 47 +++++++ src/.clang-format | 105 ++++++++++++++ src/.gitignore | 13 ++ src/CMakeLists.txt | 28 ++++ src/Makefile | 298 ++++++++++++++++++++++++++++++++++++++++ src/az_action_point.cpp | 68 +++++++++ src/az_action_point.h | 44 ++++++ src/az_coordinate.cpp | 12 ++ src/az_coordinate.h | 19 +++ src/az_fly.cpp | 213 ++++++++++++++++++++++++++++ src/az_fly.h | 33 +++++ src/az_mission.cpp | 145 +++++++++++++++++++ src/az_mission.h | 46 +++++++ src/fly_mission.cpp | 230 +++++++++++++++++++++++++++++++ src/main.cpp | 23 ++++ src/mission.json | 18 +++ 20 files changed, 1354 insertions(+) create mode 100644 .gitmodules create mode 160000 3rd/mavsdk create mode 160000 3rd/px4 create mode 160000 3rd/rapidjson create mode 100644 README.md create mode 100644 src/.clang-format create mode 100644 src/.gitignore create mode 100644 src/CMakeLists.txt create mode 100644 src/Makefile create mode 100644 src/az_action_point.cpp create mode 100644 src/az_action_point.h create mode 100644 src/az_coordinate.cpp create mode 100644 src/az_coordinate.h create mode 100644 src/az_fly.cpp create mode 100644 src/az_fly.h create mode 100644 src/az_mission.cpp create mode 100644 src/az_mission.h create mode 100644 src/fly_mission.cpp create mode 100644 src/main.cpp create mode 100644 src/mission.json diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..70b07a4 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,9 @@ +[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 new file mode 160000 index 0000000..e06de06 --- /dev/null +++ b/3rd/mavsdk @@ -0,0 +1 @@ +Subproject commit e06de06d9770b94d6d1ae0a2937ee5f2f2a0ebfb diff --git a/3rd/px4 b/3rd/px4 new file mode 160000 index 0000000..32aa326 --- /dev/null +++ b/3rd/px4 @@ -0,0 +1 @@ +Subproject commit 32aa3263a60d48a960eb8a2ccc50073815250889 diff --git a/3rd/rapidjson b/3rd/rapidjson new file mode 160000 index 0000000..5ec44fb --- /dev/null +++ b/3rd/rapidjson @@ -0,0 +1 @@ +Subproject commit 5ec44fb9206695e5293f610b0a46d21851d0c966 diff --git a/README.md b/README.md new file mode 100644 index 0000000..f9e503b --- /dev/null +++ b/README.md @@ -0,0 +1,47 @@ +# 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. + + +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 + +## Speed up the compilations +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 --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 + +sudo dpkg -i ibmavsdk-dev_2.4.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 + +sudo dpkg -i libmavsdk-dev_2.4.1_ubuntu22.04_amd64.deb + +## Build example application +cd src && cmake . && make + +## Launch similator +cd 3rd/px4 + +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 + diff --git a/src/.clang-format b/src/.clang-format new file mode 100644 index 0000000..93d8843 --- /dev/null +++ b/src/.clang-format @@ -0,0 +1,105 @@ +Language: Cpp +SeparateDefinitionBlocks: Always +EmptyLineBeforeAccessModifier: LogicalBlock +MaxEmptyLinesToKeep: 2 +AccessModifierOffset: -4 +AlignAfterOpenBracket: AlwaysBreak +AlignConsecutiveAssignments: true +AlignConsecutiveDeclarations: None +AlignEscapedNewlines: DontAlign +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: Inline +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: Yes +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: true + AfterControlStatement: false + AfterEnum: false + AfterFunction: true + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: true + AfterUnion: false + BeforeCatch: false + BeforeElse: true + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: false + SplitEmptyNamespace: false +BreakBeforeBinaryOperators: All +BreakBeforeBraces: Custom +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeComma +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 120 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - forever # avoids { wrapped to next line + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeCategories: + - Regex: '^ + +using namespace std; + +const double INVALID_LATITUDE = -100000; +const double INVALID_LONGITUDE = -100000; + +class AzCoordinate +{ +public: + double latitude; + double longitude; + + AzCoordinate(double lat, double lon); + + friend ostream &operator<<(ostream &os, const AzCoordinate &obj); +}; diff --git a/src/az_fly.cpp b/src/az_fly.cpp new file mode 100644 index 0000000..bf6a9c6 --- /dev/null +++ b/src/az_fly.cpp @@ -0,0 +1,213 @@ +#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 new file mode 100644 index 0000000..dad1737 --- /dev/null +++ b/src/az_fly.h @@ -0,0 +1,33 @@ +#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 new file mode 100644 index 0000000..9f1b566 --- /dev/null +++ b/src/az_mission.cpp @@ -0,0 +1,145 @@ +#include "az_mission.h" + +#include +#include +#include + +#include "../3rd/rapidjson/include/rapidjson/document.h" +#include "../3rd/rapidjson/include/rapidjson/error/en.h" +#include "../3rd/rapidjson/include/rapidjson/filereadstream.h" + +using namespace rapidjson; +using namespace std; + +AzMission::AzMission(string filename) + : mFilename(filename) + , mReturnPoint(AzCoordinate(INVALID_LATITUDE, INVALID_LONGITUDE)) + , mOperationalHeight(INVALID_HEIGHT) +{ + if (filesystem::exists(mFilename) == false) { + cerr << "JSON file " << mFilename << " doesn't exists." << endl; + exit(1); + } + + parse(); +} + +void AzMission::parse(void) +{ + FILE *fp = fopen(mFilename.c_str(), "r"); + char readBuffer[65536]; + FileReadStream is(fp, readBuffer, sizeof(readBuffer)); + + Document document; + document.ParseStream(is); + fclose(fp); + + if (document.HasParseError()) { + cerr << "JSON parse error: " << GetParseError_En(document.GetParseError()) << endl; + exit(1); + } + + // Parse operational height + int operational_height = document["operational_height"].GetInt(); + if (operational_height > 0) { + mOperationalHeight = operational_height; + } + + // Parse geofences + const Value &geofences = document["geofences"]; + const Value &polygons = geofences["polygons"]; + for (SizeType i = 0; i < polygons.Size(); i++) { + AzGeofence geofence; + const Value &polygon = polygons[i]; + const Value &points = polygon["points"]; + for (SizeType j = 0; j < points.Size(); j++) { + AzCoordinate coord(points[j][0].GetDouble(), points[j][1].GetDouble()); + geofence.coordinates.push_back(coord); + } + string fence_type = polygon["fence_type"].GetString(); + if (fence_type == "INCLUSION") { + geofence.type = AZ_GEOFENCE_TYPE_INCLUSION; + } + else if (fence_type == "INCLUSION") { + geofence.type = AZ_GEOFENCE_TYPE_EXCLUSION; + } + + mGeofences.push_back(geofence); + } + + // Parse action points + const Value &action_points = document["action_points"]; + for (SizeType i = 0; i < action_points.Size(); i++) { + double lat = action_points[i]["point"][0].GetDouble(); + double lon = action_points[i]["point"][1].GetDouble(); + AzCoordinate point(lat, lon); + + int height = action_points[i]["height"].GetInt(); + + string action_enum = action_points[i]["action_enum"].GetString(); + AzActionPointType action_point_type = AZ_ACTION_POINT_TYPE_NONE; + if (action_enum == "waypoint") { + action_point_type = AZ_ACTION_POINT_TYPE_WAYPOINT; + } + else if (action_enum == "search") { + action_point_type = AZ_ACTION_POINT_TYPE_SEARCH; + } + else if (action_enum == "return") { + action_point_type = AZ_ACTION_POINT_TYPE_RETURN; + } + + uint actionSpefific = 0; + if (action_points[i].HasMember("action_specific")) { + const Value &targets = action_points[i]["action_specific"]["targets"]; + for (SizeType j = 0; j < targets.Size(); j++) { + if (targets[j].GetString() == string("tank")) { + actionSpefific |= AZ_ACTION_SPECIFIC_TANK; + } + else if (targets[j].GetString() == string("artillery")) { + actionSpefific |= AZ_ACTION_SPECIFIC_ARTILLERY; + } + } + } + + AzActionPoint actionPoint(point, height, action_point_type, actionSpefific); + mActionPoints.push_back(actionPoint); + } + + // Parse return_point + mReturnPoint = AzCoordinate(document["return_point"][0].GetDouble(), document["return_point"][1].GetDouble()); +} + +const vector &AzMission::getGeofences(void) const +{ + return mGeofences; +} + +const vector &AzMission::getActionPoints(void) const +{ + return mActionPoints; +} + +int AzMission::getOperationalHeight(void) const +{ + return mOperationalHeight; +} + +AzCoordinate AzMission::getReturnPoint(void) const +{ + return mReturnPoint; +} + +ostream &operator<<(ostream &os, const AzMission &obj) +{ + os << "\nMission defination:" << endl; + os << "----------------------" << endl; + os << "Filename: " << obj.mFilename << endl; + os << "Operational height: " << obj.mOperationalHeight << endl; + os << "Return point: " << obj.mReturnPoint << endl; + + for (int i = 0; i < obj.mActionPoints.size(); i++) { + os << "\nAction point[" << i << "]:" << endl; + os << obj.mActionPoints[i]; + } + return os; +} diff --git a/src/az_mission.h b/src/az_mission.h new file mode 100644 index 0000000..696616b --- /dev/null +++ b/src/az_mission.h @@ -0,0 +1,46 @@ +#pragma once + +#include +#include +#include + +#include "az_action_point.h" +#include "az_coordinate.h" + +using namespace std; + +const double INVALID_HEIGHT = -100000; + +enum AzGeofenceType { AZ_GEOFENCE_TYPE_NONE = 0, AZ_GEOFENCE_TYPE_INCLUSION = 1, AZ_GEOFENCE_TYPE_EXCLUSION = 2 }; + +class AzGeofence +{ +public: + vector coordinates; + AzGeofenceType type; +}; + +class AzMission +{ +public: + AzMission(string filename); + + const vector &getGeofences(void) const; + + const vector &getActionPoints(void) const; + + int getOperationalHeight(void) const; + + AzCoordinate getReturnPoint(void) const; + + friend std::ostream &operator<<(std::ostream &os, const AzMission &obj); + +private: + void parse(void); + + string mFilename; + vector mGeofences; + vector mActionPoints; + int mOperationalHeight; + AzCoordinate mReturnPoint; +}; diff --git a/src/fly_mission.cpp b/src/fly_mission.cpp new file mode 100644 index 0000000..cff7d15 --- /dev/null +++ b/src/fly_mission.cpp @@ -0,0 +1,230 @@ +// +// 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 new file mode 100644 index 0000000..8dc3b1d --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,23 @@ +#include + +#include "az_fly.h" +#include "az_mission.h" + +using namespace std; + +int main(int argc, char *argv[]) +{ + if (argc != 2) { + cout << "Plese give mission file as an argument\n"; + return 1; + } + + AzMission mission(argv[1]); + cout << mission; + + AzFly fly(mission); + fly.initialize(); + fly.start(); + + return 0; +} diff --git a/src/mission.json b/src/mission.json new file mode 100644 index 0000000..737ffe4 --- /dev/null +++ b/src/mission.json @@ -0,0 +1,18 @@ +{ + "operational_height": 1100, + "geofences": { + "polygons": [ + {"points": [[12543.4213, 23476.324], [123.312, 984356.345]], "fence_type": "INCLUSION"}, + {"points": [[12543.4213, 23476.324], [123.312, 984356.345]], "fence_type": "EXCLUSION"} + ] + }, + "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"} + ], + "return_point": [99.99, 11.11] +} +