azaion/autopilot
1
0
Fork 0
mirror of https://github.com/azaion/autopilot.git synced 2026-04-22 14:06:35 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial version of ArduPilot compatible autopilot

Browse Source 
- removed PX4 and MAVSDK git submodules
This commit is contained in:
Tuomas Järvinen
2024-05-12 22:19:10 +02:00
parent f7acface7f
commit f832b9fc92
20 changed files with 514 additions and 856 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CMakeLists.txt
-28
View File
@@ -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()
src/Makefile
-298
View File
@@ -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
src/autopilot.pro
+26
View File
@@ -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
src/az_config.h
+6
View File
@@ -0,0 +1,6 @@
#pragma once
#define FUNCTION_NAME(func) func()
const char *AZ_CONNECTION = "udp://:14550";
const int AZ_RELATIVE_FLY_ALTITUDE = 50;
src/az_drone_controller.cpp
+232
View File
@@ -0,0 +1,232 @@
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <QCoreApplication>
#include <QDebug>
#include <QThread>
#include <QTimer>
#include <unistd.h>
#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;
}
src/az_drone_controller.h
+64
View File
@@ -0,0 +1,64 @@
#pragma once
#include <QObject>
#include <memory.h>
#include <mavsdk.h>
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#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<System> mSystem;
Telemetry *mTelemetry;
Action *mAction;
Telemetry::Position mFirstPosition;
AzMissionController *mMissionController;
};
src/az_fly.cpp
-213
View File
@@ -1,213 +0,0 @@
#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;
}
src/az_fly.h
-33
View File
@@ -1,33 +0,0 @@
#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;
};
src/az_mission.cpp
+5 -5
View File
@@ -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];
}
src/az_mission_controller.cpp
+92
View File
@@ -0,0 +1,92 @@
#include <QDebug>
#include <mavsdk/plugins/telemetry/telemetry.h>
#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();
}
}
src/az_mission_controller.h
+34
View File
@@ -0,0 +1,34 @@
#pragma once
#include <QObject>
#include <mavsdk/plugins/telemetry/telemetry.h>
#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;
};
src/az_utils.cpp
+18
View File
@@ -0,0 +1,18 @@
#include <math.h>
#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;
}
src/az_utils.h
+9
View File
@@ -0,0 +1,9 @@
#pragma once
class AzUtils
{
public:
AzUtils();
static double distance(double lat1, double lon1, double lat2, double lon2);
};
src/fly_mission.cpp
-230
View File
@@ -1,230 +0,0 @@
//
// 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";
}
src/main.cpp
+9 -10
View File
@@ -1,23 +1,22 @@
#include <iostream>
#include "az_fly.h"
#include <QCoreApplication>
#include <QDebug>
#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();
}
src/mission.json
+6 -6
View File
@@ -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]
}