Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

examples: rewrite discovery for main example #1328

Merged
merged 1 commit into from
Feb 23, 2021
Merged

examples: rewrite discovery for main example #1328

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 20 additions & 25 deletions examples/takeoff_land/takeoff_and_land.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,13 @@
#include <mavsdk/plugins/action/action.h>
#include <mavsdk/plugins/telemetry/telemetry.h>
#include <iostream>
#include <future>
#include <memory>
#include <thread>

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

#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red
#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue
Expand All @@ -29,19 +31,12 @@ void usage(std::string bin_name)
<< "For example, to connect to the simulator use URL: udp://:14540" << std::endl;
}

void component_discovered(ComponentType component_type)
{
std::cout << NORMAL_CONSOLE_TEXT << "Discovered a component with type "
<< unsigned(component_type) << std::endl;
}

int main(int argc, char** argv)
{
Mavsdk mavsdk;
std::string connection_url;
ConnectionResult connection_result;

bool discovered_system = false;
if (argc == 2) {
connection_url = argv[1];
connection_result = mavsdk.add_any_connection(connection_url);
Expand All @@ -57,30 +52,30 @@ int main(int argc, char** argv)
}

std::cout << "Waiting to discover system..." << std::endl;
mavsdk.subscribe_on_new_system([&mavsdk, &discovered_system]() {
const auto system = mavsdk.systems().at(0);
auto prom = std::promise<std::shared_ptr<System>>{};
auto fut = prom.get_future();

mavsdk.subscribe_on_new_system([&mavsdk, &prom]() {
auto system = mavsdk.systems().back();

if (system->is_connected()) {
std::cout << "Discovered system" << std::endl;
discovered_system = true;
if (system->has_autopilot()) {
std::cout << "Discovered autopilot" << std::endl;

// 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 2
// seconds.
sleep_for(seconds(2));

if (!discovered_system) {
std::cout << ERROR_CONSOLE_TEXT << "No system found, exiting." << NORMAL_CONSOLE_TEXT
// We usually receive heartbeats at 1Hz, therefore we should find a
// system after around 3 seconds.
if (fut.wait_for(seconds(3)) == std::future_status::timeout) {
std::cout << ERROR_CONSOLE_TEXT << "No autopilot found, exiting." << NORMAL_CONSOLE_TEXT
<< std::endl;
return 1;
}

const auto system = mavsdk.systems().at(0);

// Register a callback so we get told when components (camera, gimbal) etc
// are found.
system->register_component_discovered_callback(component_discovered);
auto system = fut.get();

auto telemetry = Telemetry{system};
auto action = Action{system};
Expand Down