Skip to content

Commit

Permalink
examples: pass through fly_mission example
Browse files Browse the repository at this point in the history
  • Loading branch information
@julianoes
julianoes committed May 29, 2021
1 parent 160b269 commit d16dbe5
Showing 1 changed file with 93 additions and 164 deletions.
257 changes: 93 additions & 164 deletions examples/fly_mission/fly_mission.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,17 +1,6 @@
/**
* @file fly_mission.cpp
*
* @brief Demonstrates how to Add & Fly Waypoint missions using the MAVSDK.
* The example is summarised below:
* 1. Adds mission items.
* 2. Starts mission from first mission item.
* 3. Illustrates Pause/Resume mission item.
* 4. Exits after the mission is accomplished.
*
* @author Julian Oes <julian@oes.ch>,
* Shakthi Prashanth M <shakthi.prashanth.m@intel.com>
* @date 2017-09-06
*/
//
// Demonstrates how to add and fly Waypoint missions using the MAVSDK.
//

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
Expand All @@ -24,22 +13,10 @@
#include <iostream>
#include <thread>

#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue
#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour

using namespace mavsdk;
using std::chrono::seconds;
using std::chrono::milliseconds;
using std::this_thread::sleep_for;

// Handles Action's result
inline void handle_action_err_exit(Action::Result result, const std::string& message);
// Handles Mission's result
inline void handle_mission_err_exit(Mission::Result result, const std::string& message);
// Handles Connection result
inline void handle_connection_err_exit(ConnectionResult result, const std::string& message);

static Mission::MissionItem make_mission_item(
double latitude_deg,
double longitude_deg,
Expand All @@ -50,70 +27,78 @@ static Mission::MissionItem make_mission_item(
float gimbal_yaw_deg,
Mission::MissionItem::CameraAction camera_action);

void usage(std::string bin_name)
void usage(const std::string& bin_name)
{
std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << bin_name << " <connection_url>" << std::endl
<< "Connection URL format should be :" << std::endl
<< " For TCP : tcp://[server_host][:server_port]" << std::endl
<< " For UDP : udp://[bind_host][:bind_port]" << std::endl
<< " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl
<< "For example, to connect to the simulator use URL: udp://:14540" << std::endl;
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)
std::shared_ptr<System> get_system(Mavsdk& mavsdk)
{
Mavsdk mavsdk;
std::cout << "Waiting to discover system...\n";
auto prom = std::promise<std::shared_ptr<System>>{};
auto fut = prom.get_future();

{
auto prom = std::promise<void>{};
auto future_result = prom.get_future();

std::cout << "Waiting to discover system..." << std::endl;
mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
const auto system = mavsdk.systems().at(0);

if (system->is_connected()) {
std::cout << "Discovered system" << std::endl;
prom.set_value();
} else {
std::cout << "System timed out" << std::endl;
std::cout << "Exiting." << std::endl;
exit(0);
}
});

std::string connection_url;
ConnectionResult connection_result;

if (argc == 2) {
connection_url = argv[1];
connection_result = mavsdk.add_any_connection(connection_url);
} else {
usage(argv[0]);
return 1;
}
// We wait for new systems to be discovered, once we find one that has an
// autopilot, we decide to use it.
mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
auto system = mavsdk.systems().back();

if (connection_result != ConnectionResult::Success) {
std::cout << ERROR_CONSOLE_TEXT << "Connection failed: " << connection_result
<< NORMAL_CONSOLE_TEXT << std::endl;
return 1;
if (system->has_autopilot()) {
std::cout << "Discovered autopilot\n";

// Unsubscribe again as we only want to find one system.
mavsdk.subscribe_on_new_system(nullptr);
prom.set_value(system);
}
});

// We usually receive heartbeats at 1Hz, therefore we should find a
// system after around 3 seconds max, surely.
if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
std::cerr << "No autopilot found.\n";
return {};
}

// Get discovered system now.
return fut.get();
}

int main(int argc, char** argv)
{
if (argc != 2) {
usage(argv[0]);
return 1;
}

Mavsdk mavsdk;
ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]);

if (connection_result != ConnectionResult::Success) {
std::cerr << "Connection failed: " << connection_result << '\n';
return 1;
}

future_result.get();
auto system = get_system(mavsdk);
if (!system) {
return 1;
}

auto system = mavsdk.systems().at(0);
auto action = Action{system};
auto mission = Mission{system};
auto telemetry = Telemetry{system};

while (!telemetry.health_all_ok()) {
std::cout << "Waiting for system to be ready" << std::endl;
std::cout << "Waiting for system to be ready\n";
sleep_for(seconds(1));
}

std::cout << "System ready" << std::endl;
std::cout << "Creating and uploading mission" << std::endl;
std::cout << "System ready\n";
std::cout << "Creating and uploading mission\n";

std::vector<Mission::MissionItem> mission_items;

Expand Down Expand Up @@ -177,35 +162,29 @@ int main(int argc, char** argv)
0.0f,
Mission::MissionItem::CameraAction::StopPhotoInterval));

{
std::cout << "Uploading mission..." << std::endl;
// We only have the upload_mission function asynchronous for now, so we wrap it using
// std::future.
auto prom = std::promise<Mission::Result>{};
auto future_result = prom.get_future();
Mission::MissionPlan mission_plan{};
mission_plan.mission_items = mission_items;
mission.upload_mission_async(
mission_plan, [&prom](Mission::Result result) { prom.set_value(result); });

const Mission::Result result = future_result.get();
if (result != Mission::Result::Success) {
std::cout << "Mission upload failed (" << result << "), exiting." << std::endl;
return 1;
}
std::cout << "Mission uploaded." << std::endl;
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..." << std::endl;
std::cout << "Arming...\n";
const Action::Result arm_result = action.arm();
handle_action_err_exit(arm_result, "Arm failed: ");
std::cout << "Armed." << std::endl;
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 << std::endl;
<< mission_progress.total << '\n';

if (mission_progress.current >= 2) {
// We can only set a flag here. If we do more request inside the callback,
Expand All @@ -214,71 +193,46 @@ int main(int argc, char** argv)
}
});

{
std::cout << "Starting mission." << std::endl;
auto prom = std::promise<Mission::Result>{};
auto future_result = prom.get_future();
mission.start_mission_async([&prom](Mission::Result result) {
prom.set_value(result);
std::cout << "Started mission." << std::endl;
});

const Mission::Result result = future_result.get();
handle_mission_err_exit(result, "Mission start failed: ");
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));
}

{
auto prom = std::promise<Mission::Result>{};
auto future_result = prom.get_future();
std::cout << "Pausing mission...\n";
const Mission::Result pause_mission_result = mission.pause_mission();

std::cout << "Pausing mission..." << std::endl;
mission.pause_mission_async([&prom](Mission::Result result) { prom.set_value(result); });

const Mission::Result result = future_result.get();
if (result != Mission::Result::Success) {
std::cout << "Failed to pause mission (" << result << ")" << std::endl;
} else {
std::cout << "Mission paused." << std::endl;
}
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.
{
auto prom = std::promise<Mission::Result>{};
auto future_result = prom.get_future();

std::cout << "Resuming mission..." << std::endl;
mission.start_mission_async([&prom](Mission::Result result) { prom.set_value(result); });

const Mission::Result result = future_result.get();
if (result != Mission::Result::Success) {
std::cout << "Failed to resume mission (" << result << ")" << std::endl;
} else {
std::cout << "Resumed mission." << std::endl;
}
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..." << std::endl;
const Action::Result result = action.return_to_launch();
if (result != Action::Result::Success) {
std::cout << "Failed to command RTL (" << result << ")" << std::endl;
} else {
std::cout << "Commanded RTL." << std::endl;
}
// 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));
Expand All @@ -287,7 +241,7 @@ int main(int argc, char** argv)
// Wait until we're done.
sleep_for(seconds(1));
}
std::cout << "Disarmed, exiting." << std::endl;
std::cout << "Disarmed, exiting.\n";
}

Mission::MissionItem make_mission_item(
Expand All @@ -311,28 +265,3 @@ Mission::MissionItem make_mission_item(
new_item.camera_action = camera_action;
return new_item;
}

inline void handle_action_err_exit(Action::Result result, const std::string& message)
{
if (result != Action::Result::Success) {
std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl;
exit(EXIT_FAILURE);
}
}

inline void handle_mission_err_exit(Mission::Result result, const std::string& message)
{
if (result != Mission::Result::Success) {
std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl;
exit(EXIT_FAILURE);
}
}

// Handles connection result
inline void handle_connection_err_exit(ConnectionResult result, const std::string& message)
{
if (result != ConnectionResult::Success) {
std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl;
exit(EXIT_FAILURE);
}
}

0 comments on commit d16dbe5

Please sign in to comment.