mirror of
https://github.com/azaion/autopilot.git
synced 2026-04-23 03:06:34 +00:00
Initial version. Can fly simple mission defined in JSON file.
Please check README.md how to install dependencies and run the application.
This commit is contained in:
Tuomas Järvinen
@@ -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
|
||||||
Submodule
+1
Submodule 3rd/mavsdk added at e06de06d97
Submodule
+1
Submodule 3rd/px4 added at 32aa3263a6
Submodule
+1
Submodule 3rd/rapidjson added at 5ec44fb920
@@ -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
|
||||||
|
|
||||||
@@ -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: '^<Q.*'
|
||||||
|
Priority: 200
|
||||||
|
IncludeIsMainRegex: '(Test)?$'
|
||||||
|
IndentCaseLabels: false
|
||||||
|
IndentWidth: 4
|
||||||
|
IndentWrappedFunctionNames: false
|
||||||
|
InsertBraces: false
|
||||||
|
JavaScriptQuotes: Leave
|
||||||
|
JavaScriptWrapImports: true
|
||||||
|
KeepEmptyLinesAtTheStartOfBlocks: false
|
||||||
|
# Do not add QT_BEGIN_NAMESPACE/QT_END_NAMESPACE as this will indent lines in between.
|
||||||
|
MacroBlockBegin: ""
|
||||||
|
MacroBlockEnd: ""
|
||||||
|
NamespaceIndentation: None
|
||||||
|
ObjCBlockIndentWidth: 4
|
||||||
|
ObjCSpaceAfterProperty: false
|
||||||
|
ObjCSpaceBeforeProtocolList: true
|
||||||
|
PenaltyBreakAssignment: 88
|
||||||
|
PenaltyBreakBeforeFirstCallParameter: 300
|
||||||
|
PenaltyBreakComment: 500
|
||||||
|
PenaltyBreakFirstLessLess: 400
|
||||||
|
PenaltyBreakString: 600
|
||||||
|
PenaltyExcessCharacter: 50
|
||||||
|
PenaltyReturnTypeOnItsOwnLine: 300
|
||||||
|
PointerAlignment: Right
|
||||||
|
ReflowComments: false
|
||||||
|
SortIncludes: CaseSensitive
|
||||||
|
SortUsingDeclarations: true
|
||||||
|
SpaceAfterCStyleCast: true
|
||||||
|
SpaceAfterTemplateKeyword: false
|
||||||
|
SpaceBeforeAssignmentOperators: true
|
||||||
|
SpaceBeforeParens: ControlStatements
|
||||||
|
SpaceInEmptyParentheses: false
|
||||||
|
SpacesBeforeTrailingComments: 1
|
||||||
|
SpacesInAngles: Never
|
||||||
|
SpacesInContainerLiterals: false
|
||||||
|
SpacesInCStyleCastParentheses: false
|
||||||
|
SpacesInParentheses: false
|
||||||
|
SpacesInSquareBrackets: false
|
||||||
|
Standard: c++17
|
||||||
|
TabWidth: 4
|
||||||
|
UseTab: Never
|
||||||
|
|
||||||
@@ -0,0 +1,13 @@
|
|||||||
|
.cmake/
|
||||||
|
.qtc/
|
||||||
|
.qtc_clangd/
|
||||||
|
CMakeCache.txt
|
||||||
|
CMakeCache.txt.prev
|
||||||
|
CMakeFiles/
|
||||||
|
CMakeLists.txt.user
|
||||||
|
Makefile
|
||||||
|
Testing/
|
||||||
|
azaion
|
||||||
|
cmake_install.cmake
|
||||||
|
qtcsettings.cmake
|
||||||
|
|
||||||
@@ -0,0 +1,28 @@
|
|||||||
|
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()
|
||||||
+298
@@ -0,0 +1,298 @@
|
|||||||
|
# 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
|
||||||
|
|
||||||
@@ -0,0 +1,68 @@
|
|||||||
|
#include "az_action_point.h"
|
||||||
|
|
||||||
|
AzActionPoint::AzActionPoint(
|
||||||
|
const AzCoordinate &point, int height, AzActionPointType actionPointType, uint actionSpesific)
|
||||||
|
: mPoint(point)
|
||||||
|
, mHeight(height)
|
||||||
|
, mType(actionPointType)
|
||||||
|
, mActionSpesific(actionSpesific)
|
||||||
|
{}
|
||||||
|
|
||||||
|
AzCoordinate AzActionPoint::getPoint(void) const
|
||||||
|
{
|
||||||
|
return mPoint;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AzActionPoint::getHeight(void) const
|
||||||
|
{
|
||||||
|
return mHeight;
|
||||||
|
}
|
||||||
|
|
||||||
|
AzActionPointType AzActionPoint::getType(void) const
|
||||||
|
{
|
||||||
|
return mType;
|
||||||
|
}
|
||||||
|
|
||||||
|
string AzActionPoint::getTypeStr(void) const
|
||||||
|
{
|
||||||
|
static const string typeStrs[] = {"None", "Waypoint", "Search", "Return"};
|
||||||
|
return typeStrs[mType];
|
||||||
|
}
|
||||||
|
|
||||||
|
string AzActionPoint::getActionSpecificStr(void) const
|
||||||
|
{
|
||||||
|
if (isTank() == true && isArtillery() == true) {
|
||||||
|
return "tank or artillery";
|
||||||
|
}
|
||||||
|
else if (isTank() == true) {
|
||||||
|
return "tank";
|
||||||
|
}
|
||||||
|
else if (isArtillery() == true) {
|
||||||
|
return "artillery";
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return "none";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AzActionPoint::isTank(void) const
|
||||||
|
{
|
||||||
|
return mActionSpesific & AZ_ACTION_SPECIFIC_TANK;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool AzActionPoint::isArtillery(void) const
|
||||||
|
{
|
||||||
|
return mActionSpesific & AZ_ACTION_SPECIFIC_ARTILLERY;
|
||||||
|
}
|
||||||
|
|
||||||
|
ostream &operator<<(ostream &os, const AzActionPoint &obj)
|
||||||
|
{
|
||||||
|
os << "Point: " << obj.mPoint << endl;
|
||||||
|
os << "Height: " << obj.mHeight << endl;
|
||||||
|
os << "Type: " << obj.getTypeStr() << endl;
|
||||||
|
|
||||||
|
// TODO!! This is braindead.
|
||||||
|
os << "Action specific type: " + obj.getActionSpecificStr() << endl;
|
||||||
|
|
||||||
|
return os;
|
||||||
|
}
|
||||||
@@ -0,0 +1,44 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "az_coordinate.h"
|
||||||
|
|
||||||
|
enum AzActionPointType {
|
||||||
|
AZ_ACTION_POINT_TYPE_NONE = 0,
|
||||||
|
AZ_ACTION_POINT_TYPE_WAYPOINT = 1,
|
||||||
|
AZ_ACTION_POINT_TYPE_SEARCH = 2,
|
||||||
|
AZ_ACTION_POINT_TYPE_RETURN = 3
|
||||||
|
};
|
||||||
|
|
||||||
|
enum AzActionSpecific {
|
||||||
|
AZ_ACTION_SPECIFIC_NONE = 1 << 0,
|
||||||
|
AZ_ACTION_SPECIFIC_TANK = 1 << 1,
|
||||||
|
AZ_ACTION_SPECIFIC_ARTILLERY = 1 << 2,
|
||||||
|
};
|
||||||
|
|
||||||
|
class AzActionPoint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
AzActionPoint(const AzCoordinate &point, int height, AzActionPointType actionPointType, uint actionSpesific);
|
||||||
|
|
||||||
|
AzCoordinate getPoint(void) const;
|
||||||
|
|
||||||
|
int getHeight(void) const;
|
||||||
|
|
||||||
|
AzActionPointType getType(void) const;
|
||||||
|
|
||||||
|
string getTypeStr(void) const;
|
||||||
|
|
||||||
|
bool isTank(void) const;
|
||||||
|
|
||||||
|
bool isArtillery(void) const;
|
||||||
|
|
||||||
|
string getActionSpecificStr(void) const;
|
||||||
|
|
||||||
|
friend ostream &operator<<(ostream &os, const AzActionPoint &obj);
|
||||||
|
|
||||||
|
private:
|
||||||
|
AzCoordinate mPoint;
|
||||||
|
int mHeight;
|
||||||
|
AzActionPointType mType;
|
||||||
|
uint mActionSpesific;
|
||||||
|
};
|
||||||
@@ -0,0 +1,12 @@
|
|||||||
|
#include "az_coordinate.h"
|
||||||
|
|
||||||
|
AzCoordinate::AzCoordinate(double lat, double lon)
|
||||||
|
: latitude(lat)
|
||||||
|
, longitude(lon)
|
||||||
|
{}
|
||||||
|
|
||||||
|
ostream &operator<<(ostream &os, const AzCoordinate &obj)
|
||||||
|
{
|
||||||
|
os << "(" << obj.latitude << ", " << obj.longitude << ")";
|
||||||
|
return os;
|
||||||
|
}
|
||||||
@@ -0,0 +1,19 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <ostream>
|
||||||
|
|
||||||
|
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);
|
||||||
|
};
|
||||||
+213
@@ -0,0 +1,213 @@
|
|||||||
|
#include <mavsdk/mavsdk.h>
|
||||||
|
#include <mavsdk/plugins/action/action.h>
|
||||||
|
#include <mavsdk/plugins/mission/mission.h>
|
||||||
|
#include <mavsdk/plugins/offboard/offboard.h>
|
||||||
|
#include <mavsdk/plugins/telemetry/telemetry.h>
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <future>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#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<Mission::MissionItem> AzFly::getMissionItems(void)
|
||||||
|
{
|
||||||
|
vector<Mission::MissionItem> missionItems;
|
||||||
|
const vector<AzActionPoint> &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<std::promise<Mission::Result>>();
|
||||||
|
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::MissionItem> 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<AzActionPoint> &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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,33 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "az_mission.h"
|
||||||
|
#include <mavsdk/mavsdk.h>
|
||||||
|
#include <mavsdk/plugins/action/action.h>
|
||||||
|
#include <mavsdk/plugins/mission/mission.h>
|
||||||
|
#include <mavsdk/plugins/offboard/offboard.h>
|
||||||
|
#include <mavsdk/plugins/telemetry/telemetry.h>
|
||||||
|
|
||||||
|
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<shared_ptr<System>> mSystem;
|
||||||
|
Action *mAction;
|
||||||
|
Mission *mMission;
|
||||||
|
Telemetry *mTelemetry;
|
||||||
|
Offboard *mOffboard;
|
||||||
|
|
||||||
|
AzMission &mAzMission;
|
||||||
|
vector<Mission::MissionItem> getMissionItems(void);
|
||||||
|
void findAndDetroyThreadFunc(void);
|
||||||
|
int mCurrentMissionIndex;
|
||||||
|
};
|
||||||
@@ -0,0 +1,145 @@
|
|||||||
|
#include "az_mission.h"
|
||||||
|
|
||||||
|
#include <filesystem>
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#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<AzGeofence> &AzMission::getGeofences(void) const
|
||||||
|
{
|
||||||
|
return mGeofences;
|
||||||
|
}
|
||||||
|
|
||||||
|
const vector<AzActionPoint> &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;
|
||||||
|
}
|
||||||
@@ -0,0 +1,46 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#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<AzCoordinate> coordinates;
|
||||||
|
AzGeofenceType type;
|
||||||
|
};
|
||||||
|
|
||||||
|
class AzMission
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
AzMission(string filename);
|
||||||
|
|
||||||
|
const vector<AzGeofence> &getGeofences(void) const;
|
||||||
|
|
||||||
|
const vector<AzActionPoint> &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<AzGeofence> mGeofences;
|
||||||
|
vector<AzActionPoint> mActionPoints;
|
||||||
|
int mOperationalHeight;
|
||||||
|
AzCoordinate mReturnPoint;
|
||||||
|
};
|
||||||
@@ -0,0 +1,230 @@
|
|||||||
|
//
|
||||||
|
// Demonstrates how to add and fly Waypoint missions using the MAVSDK.
|
||||||
|
//
|
||||||
|
|
||||||
|
#include <mavsdk/mavsdk.h>
|
||||||
|
#include <mavsdk/plugins/action/action.h>
|
||||||
|
#include <mavsdk/plugins/mission/mission.h>
|
||||||
|
#include <mavsdk/plugins/telemetry/telemetry.h>
|
||||||
|
|
||||||
|
#include <chrono>
|
||||||
|
#include <functional>
|
||||||
|
#include <future>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
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 << " <connection_url>\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::MissionItem> 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<bool> 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";
|
||||||
|
}
|
||||||
@@ -0,0 +1,23 @@
|
|||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
#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;
|
||||||
|
}
|
||||||
@@ -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]
|
||||||
|
}
|
||||||
|
|
||||||
Reference in New Issue
Block a user