Skip to content

Commit

Permalink
mavsdk_server: fix autopilot discovery (#2388)
Browse files Browse the repository at this point in the history
Instead of using the subscribe_on_new_systems callback, we just use a
while loop here. That's because the subscribe_on_new_system callback
won't tell us if an autopilot is discovered when another component of
the same system is discovered first, thus triggering the callback early
when no autopilot is around.

With a stupid old while loop we can just keep checking this until we
have an autopilot and then exit.
  • Loading branch information
julianoes authored Sep 2, 2024
1 parent 7943261 commit 71126ac
Show file tree
Hide file tree
Showing 4 changed files with 24 additions and 150 deletions.
59 changes: 23 additions & 36 deletions src/mavsdk_server/src/connection_initiator.h
Original file line number Diff line number Diff line change
@@ -1,7 +1,9 @@
#pragma once

#include <chrono>
#include <future>
#include <mutex>
#include <thread>
#include <string>

#include "connection_result.h"
Expand All @@ -15,29 +17,37 @@ template<typename Mavsdk> class ConnectionInitiator {
ConnectionInitiator() {}
~ConnectionInitiator() {}

bool start(Mavsdk& mavsdk, const std::string& connection_url)
bool connect(Mavsdk& mavsdk, const std::string& connection_url)
{
LogInfo() << "Waiting to discover system on " << connection_url << "...";
_discovery_future = wrapped_subscribe_on_new_system(mavsdk);

if (!add_any_connection(mavsdk, connection_url)) {
return false;
}

return true;
}

bool wait() { return _discovery_future.get(); }
// Instead of using the subscribe_on_new_system callback, we just use a
// while loop here. That's because the subscribe_on_new_system callback
// won't tell us if an autopilot is discovered when another component
// of the same system is discovered first, thus triggering the callback
// early when not autopilot is around.
// With a stupid old while loop we can just keep checking this until
// we have an autopilot and then exit.
while (!_should_exit) {
for (const auto& system : mavsdk.systems()) {
if (system->has_autopilot()) {
LogInfo() << "System discovered";
return true;
}
}

void cancel()
{
std::lock_guard<std::mutex> guard(_mutex);
if (!_is_discovery_finished) {
_is_discovery_finished = true;
_discovery_promise->set_value(false);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}

return false;
}

void cancel() { _should_exit = true; }

private:
bool add_any_connection(Mavsdk& mavsdk, const std::string& connection_url)
{
Expand All @@ -51,30 +61,7 @@ template<typename Mavsdk> class ConnectionInitiator {
return true;
}

std::future<bool> wrapped_subscribe_on_new_system(Mavsdk& mavsdk)
{
auto future = _discovery_promise->get_future();

mavsdk.subscribe_on_new_system([this, &mavsdk]() {
std::lock_guard<std::mutex> guard(_mutex);
for (auto system : mavsdk.systems()) {
if (!_is_discovery_finished && system->has_autopilot() && system->is_connected()) {
LogInfo() << "System discovered";

_is_discovery_finished = true;
_discovery_promise->set_value(true);
break;
}
}
});

return future;
}

std::mutex _mutex;
std::atomic<bool> _is_discovery_finished = false;
std::shared_ptr<std::promise<bool>> _discovery_promise = std::make_shared<std::promise<bool>>();
std::future<bool> _discovery_future{};
std::atomic<bool> _should_exit{false};
};

} // namespace mavsdk_server
Expand Down
3 changes: 1 addition & 2 deletions src/mavsdk_server/src/mavsdk_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,7 @@ class MavsdkServer::Impl {

bool connect(const std::string& connection_url)
{
_connection_initiator.start(_mavsdk, connection_url);
return _connection_initiator.wait();
return _connection_initiator.connect(_mavsdk, connection_url);
}

int startGrpcServer(const int port)
Expand Down
1 change: 0 additions & 1 deletion src/mavsdk_server/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,6 @@ add_executable(unit_tests_mavsdk_server
action_service_impl_test.cpp
mavsdk_server_main.cpp
camera_service_impl_test.cpp
connection_initiator_test.cpp
core_service_impl_test.cpp
mission_service_impl_test.cpp
offboard_service_impl_test.cpp
Expand Down
111 changes: 0 additions & 111 deletions src/mavsdk_server/test/connection_initiator_test.cpp

This file was deleted.

0 comments on commit 71126ac

Please sign in to comment.