diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 11ddc7112..edfeb74d7 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -17,6 +17,7 @@ add_subdirectory(camera_settings) add_subdirectory(camera_zoom) add_subdirectory(component_metadata) add_subdirectory(disconnect) +add_subdirectory(reconnect) add_subdirectory(fly_mission) add_subdirectory(fly_multiple_drones) add_subdirectory(fly_qgc_mission) diff --git a/examples/reconnect/CMakeLists.txt b/examples/reconnect/CMakeLists.txt new file mode 100644 index 000000000..5fc43cc9a --- /dev/null +++ b/examples/reconnect/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.10.2) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +project(reconnect) + +add_executable(reconnect + reconnect.cpp +) + +find_package(MAVSDK REQUIRED) + +target_link_libraries(reconnect + MAVSDK::mavsdk +) + +if(NOT MSVC) + add_compile_options(reconnect PRIVATE -Wall -Wextra) +else() + add_compile_options(reconnect PRIVATE -W2) +endif() diff --git a/examples/reconnect/reconnect.cpp b/examples/reconnect/reconnect.cpp new file mode 100644 index 000000000..2e057c3c0 --- /dev/null +++ b/examples/reconnect/reconnect.cpp @@ -0,0 +1,97 @@ +// +// This is an example how to reconnect a connection on error. +// + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace mavsdk; + +void usage(const std::string& bin_name) +{ + std::cerr << "Usage : " << bin_name << " \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: udpin://0.0.0.0:14540\n"; +} + +int main(int argc, char** argv) +{ + if (argc < 2) { + usage(argv[0]); + return 1; + } + + // Keep track of our connections + struct ConnectionEntry { + std::string url; + Mavsdk::ConnectionHandle handle; + bool connected; + }; + std::mutex connections_mutex; + std::vector connections; + + Mavsdk mavsdk{Mavsdk::Configuration{ComponentType::GroundStation}}; + + // Add all connections. + for (int i = 1; i < argc; ++i) { + ConnectionEntry entry{}; + entry.url = argv[i]; + entry.connected = false; + connections.push_back(entry); + } + + // Subscribe to connection errors. + // When an error happens, remove the connection and mark it as disconnected. + mavsdk.subscribe_connection_errors([&](Mavsdk::ConnectionError connection_error) { + std::cout << "Connection error: " << connection_error.error_description << std::endl; + + mavsdk.remove_connection(connection_error.connection_handle); + + std::lock_guard lock(connections_mutex); + auto it = std::find_if(connections.begin(), connections.end(), [&](ConnectionEntry& entry) { + return entry.handle == connection_error.connection_handle; + }); + + assert(it != connections.end()); + + std::cout << "Removed connection: '" << it->url << "'" << std::endl; + it->connected = false; + }); + + while (true) { + { + // Got through connections and try to add them. + std::lock_guard lock(connections_mutex); + for (auto& entry : connections) { + if (!entry.connected) { + std::cout << "Try adding connection '" << entry.url << "'" << std::endl; + auto result = mavsdk.add_any_connection_with_handle(entry.url); + + if (result.first != ConnectionResult::Success) { + std::cout << "Adding connection '" << entry.url + << "'failed: " << result.first << std::endl; + } else { + entry.handle = result.second; + entry.connected = true; + } + } + } + } + + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + return 0; +} diff --git a/src/mavsdk/core/connection.h b/src/mavsdk/core/connection.h index d7c5bc447..e38901e3c 100644 --- a/src/mavsdk/core/connection.h +++ b/src/mavsdk/core/connection.h @@ -4,7 +4,9 @@ #include "mavlink_receiver.h" #include #include +#include #include +#include namespace mavsdk { @@ -21,7 +23,7 @@ class Connection { virtual ConnectionResult start() = 0; virtual ConnectionResult stop() = 0; - virtual bool send_message(const mavlink_message_t& message) = 0; + virtual std::pair send_message(const mavlink_message_t& message) = 0; bool has_system_id(uint8_t system_id); bool should_forward_messages() const; diff --git a/src/mavsdk/core/include/mavsdk/mavsdk.h b/src/mavsdk/core/include/mavsdk/mavsdk.h index 3baf9218a..5f4e1d147 100644 --- a/src/mavsdk/core/include/mavsdk/mavsdk.h +++ b/src/mavsdk/core/include/mavsdk/mavsdk.h @@ -125,6 +125,44 @@ class Mavsdk { */ void remove_connection(ConnectionHandle handle); + /** + * ConnectionError type + */ + struct ConnectionError { + std::string error_description; + ConnectionHandle connection_handle; + }; + + /** + * Connection Error callback type + */ + using ConnectionErrorCallback = std::function; + + /** + * @brief Handle type to remove a connection error subscription. + */ + using ConnectionErrorHandle = Handle; + + /** + * Subscribe to connection errors. + * + * This will trigger when messages fail to be sent which can help + * diagnosing network interfaces or serial devices disappearing. + * + * Usually, an error will require to remove a connection and add it fresh. + * + * @param callback Callback to subscribe. + * @return Handle to unsubscribe again. + */ + ConnectionErrorHandle subscribe_connection_errors(ConnectionErrorCallback callback); + + /** + * Unsubscribe from connection errors. + * + * @param handle Handle to unsubscribe. + */ + void unsubscribe_connection_errors(ConnectionErrorHandle handle); + /** * @brief Get a vector of systems which have been discovered or set-up. * diff --git a/src/mavsdk/core/mavsdk.cpp b/src/mavsdk/core/mavsdk.cpp index d665c8ed5..a2673e0da 100644 --- a/src/mavsdk/core/mavsdk.cpp +++ b/src/mavsdk/core/mavsdk.cpp @@ -200,4 +200,15 @@ void Mavsdk::intercept_outgoing_messages_async(std::functionintercept_outgoing_messages_async(callback); } +Mavsdk::ConnectionErrorHandle +Mavsdk::subscribe_connection_errors(Mavsdk::ConnectionErrorCallback callback) +{ + return _impl->subscribe_connection_errors(callback); +} + +void Mavsdk::unsubscribe_connection_errors(ConnectionErrorHandle handle) +{ + return _impl->unsubscribe_connection_errors(handle); +} + } // namespace mavsdk diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index 5b5e16639..c9525c635 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -5,6 +5,7 @@ #include #include "connection.h" +#include "log.h" #include "tcp_client_connection.h" #include "tcp_server_connection.h" #include "udp_connection.h" @@ -305,8 +306,13 @@ void MavsdkImpl::forward_message(mavlink_message_t& message, Connection* connect !entry.connection->should_forward_messages()) { continue; } - if ((*entry.connection).send_message(message)) { + auto result = (*entry.connection).send_message(message); + if (result.first) { successful_emissions++; + } else { + _connections_errors_subscriptions.queue( + Mavsdk::ConnectionError{result.second, entry.handle}, + [this](const auto& func) { call_user_callback(func); }); } } if (successful_emissions == 0) { @@ -473,9 +479,13 @@ bool MavsdkImpl::send_message(mavlink_message_t& message) if (target_system_id != 0 && !(*_connection.connection).has_system_id(target_system_id)) { continue; } - - if ((*_connection.connection).send_message(message)) { + const auto result = (*_connection.connection).send_message(message); + if (result.first) { successful_emissions++; + } else { + _connections_errors_subscriptions.queue( + Mavsdk::ConnectionError{result.second, _connection.handle}, + [this](const auto& func) { call_user_callback(func); }); } } @@ -915,6 +925,22 @@ void MavsdkImpl::intercept_outgoing_messages_async(std::function callback); void intercept_outgoing_messages_async(std::function callback); + Mavsdk::ConnectionErrorHandle + subscribe_connection_errors(Mavsdk::ConnectionErrorCallback callback); + void unsubscribe_connection_errors(Mavsdk::ConnectionErrorHandle handle); + std::shared_ptr server_component(unsigned instance = 0); std::shared_ptr @@ -128,6 +132,7 @@ class MavsdkImpl { Handle<> handle; }; std::vector _connections{}; + CallbackList _connections_errors_subscriptions{}; mutable std::recursive_mutex _systems_mutex{}; std::vector>> _systems{}; diff --git a/src/mavsdk/core/serial_connection.cpp b/src/mavsdk/core/serial_connection.cpp index eac14227d..df92bf61d 100644 --- a/src/mavsdk/core/serial_connection.cpp +++ b/src/mavsdk/core/serial_connection.cpp @@ -10,6 +10,8 @@ #include #endif +#include + namespace mavsdk { #ifndef WINDOWS @@ -249,16 +251,21 @@ ConnectionResult SerialConnection::stop() return ConnectionResult::Success; } -bool SerialConnection::send_message(const mavlink_message_t& message) +std::pair SerialConnection::send_message(const mavlink_message_t& message) { + std::pair result; + if (_serial_node.empty()) { LogErr() << "Dev Path unknown"; - return false; + result.first = false; + result.second = "Dev Path unknown"; + return result; } if (_baudrate == 0) { - LogErr() << "Baudrate unknown"; - return false; + result.first = false; + result.second = "Baudrate unknown"; + return result; } uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; @@ -275,11 +282,19 @@ bool SerialConnection::send_message(const mavlink_message_t& message) #endif if (send_len != buffer_len) { - LogErr() << "write failure: " << GET_ERROR(); - return false; + result.first = false; + result.second = "Baudrate unknown"; + + std::stringstream ss; + ss << "write failure: " << GET_ERROR(); + LogErr() << ss.str(); + result.first = false; + result.second = ss.str(); + return result; } - return true; + result.first = true; + return result; } void SerialConnection::receive() diff --git a/src/mavsdk/core/serial_connection.h b/src/mavsdk/core/serial_connection.h index 0582d3548..d4743cbe5 100644 --- a/src/mavsdk/core/serial_connection.h +++ b/src/mavsdk/core/serial_connection.h @@ -24,7 +24,7 @@ class SerialConnection : public Connection { ConnectionResult stop() override; ~SerialConnection() override; - bool send_message(const mavlink_message_t& message) override; + std::pair send_message(const mavlink_message_t& message) override; // Non-copyable SerialConnection(const SerialConnection&) = delete; diff --git a/src/mavsdk/core/tcp_client_connection.cpp b/src/mavsdk/core/tcp_client_connection.cpp index 1b2c958a1..42c36eb95 100644 --- a/src/mavsdk/core/tcp_client_connection.cpp +++ b/src/mavsdk/core/tcp_client_connection.cpp @@ -2,8 +2,10 @@ #include "log.h" #include +#include #include #include +#include #ifdef WINDOWS #ifndef MINGW @@ -128,20 +130,29 @@ ConnectionResult TcpClientConnection::stop() return ConnectionResult::Success; } -bool TcpClientConnection::send_message(const mavlink_message_t& message) +std::pair TcpClientConnection::send_message(const mavlink_message_t& message) { + std::pair result; + if (!_is_ok) { - return false; + // TODO: not entirely sure whether this makes sense here. + result.first = false; + result.second = "not ok"; + return result; } if (_remote_ip.empty()) { - LogErr() << "Remote IP unknown"; - return false; + result.first = false; + result.second = "Remote IP unknown"; + LogErr() << result.second; + return result; } if (_remote_port_number == 0) { - LogErr() << "Remote port unknown"; - return false; + result.first = false; + result.second = "Remote port unknown"; + LogErr() << result.second; + return result; } struct sockaddr_in dest_addr {}; @@ -167,11 +178,17 @@ bool TcpClientConnection::send_message(const mavlink_message_t& message) send(_socket_fd.get(), reinterpret_cast(buffer), buffer_len, flags); if (send_len != buffer_len) { - LogErr() << "send failure: " << GET_ERROR(errno); + std::stringstream ss; + ss << "Send failure: " << GET_ERROR(errno); + LogErr() << ss.str(); _is_ok = false; - return false; + result.first = false; + result.second = ss.str(); + return result; } - return true; + + result.first = true; + return result; } void TcpClientConnection::receive() diff --git a/src/mavsdk/core/tcp_client_connection.h b/src/mavsdk/core/tcp_client_connection.h index 5c64a3d72..6b9d2e9b6 100644 --- a/src/mavsdk/core/tcp_client_connection.h +++ b/src/mavsdk/core/tcp_client_connection.h @@ -30,7 +30,7 @@ class TcpClientConnection : public Connection { ConnectionResult start() override; ConnectionResult stop() override; - bool send_message(const mavlink_message_t& message) override; + std::pair send_message(const mavlink_message_t& message) override; // Non-copyable TcpClientConnection(const TcpClientConnection&) = delete; diff --git a/src/mavsdk/core/tcp_server_connection.cpp b/src/mavsdk/core/tcp_server_connection.cpp index 02970e40e..7219c5786 100644 --- a/src/mavsdk/core/tcp_server_connection.cpp +++ b/src/mavsdk/core/tcp_server_connection.cpp @@ -3,6 +3,7 @@ #include #include +#include #ifdef WINDOWS #ifndef MINGW @@ -105,8 +106,10 @@ ConnectionResult TcpServerConnection::stop() return ConnectionResult::Success; } -bool TcpServerConnection::send_message(const mavlink_message_t& message) +std::pair TcpServerConnection::send_message(const mavlink_message_t& message) { + std::pair result; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t buffer_len = mavlink_msg_to_send_buffer(buffer, &message); @@ -122,10 +125,16 @@ bool TcpServerConnection::send_message(const mavlink_message_t& message) send(_client_socket_fd.get(), reinterpret_cast(buffer), buffer_len, flags); if (send_len != buffer_len) { - LogErr() << "send failure: " << GET_ERROR(errno); - return false; + std::stringstream ss; + ss << "Send failure: " << GET_ERROR(errno); + LogErr() << ss.str(); + result.first = false; + result.second = ss.str(); + return result; } - return true; + + result.first = true; + return result; } void TcpServerConnection::accept_client() diff --git a/src/mavsdk/core/tcp_server_connection.h b/src/mavsdk/core/tcp_server_connection.h index 863050f85..3eb26bf8f 100644 --- a/src/mavsdk/core/tcp_server_connection.h +++ b/src/mavsdk/core/tcp_server_connection.h @@ -20,7 +20,7 @@ class TcpServerConnection : public Connection { ConnectionResult start() override; ConnectionResult stop() override; - bool send_message(const mavlink_message_t& message) override; + std::pair send_message(const mavlink_message_t& message) override; private: void accept_client(); diff --git a/src/mavsdk/core/udp_connection.cpp b/src/mavsdk/core/udp_connection.cpp index f9fd68ff2..0ca6c65d1 100644 --- a/src/mavsdk/core/udp_connection.cpp +++ b/src/mavsdk/core/udp_connection.cpp @@ -17,6 +17,7 @@ #include #include +#include #ifdef WINDOWS #define GET_ERROR(_x) WSAGetLastError() @@ -114,12 +115,16 @@ ConnectionResult UdpConnection::stop() return ConnectionResult::Success; } -bool UdpConnection::send_message(const mavlink_message_t& message) +std::pair UdpConnection::send_message(const mavlink_message_t& message) { + std::pair result; + std::lock_guard lock(_remote_mutex); if (_remotes.size() == 0) { - return false; + result.first = false; + result.second = "no remotes"; + return result; } // Send the message to all the remotes. A remote is a UDP endpoint @@ -127,14 +132,23 @@ bool UdpConnection::send_message(const mavlink_message_t& message) // systems on two different endpoints, then messages directed towards // only one system will be sent to both remotes. The systems are // then expected to ignore messages that are not directed to them. - bool send_successful = true; + + // For multiple remotes, we ignore errors, for just one, we bubble it up. + result.first = true; + for (auto& remote : _remotes) { struct sockaddr_in dest_addr {}; dest_addr.sin_family = AF_INET; if (inet_pton(AF_INET, remote.ip.c_str(), &dest_addr.sin_addr.s_addr) != 1) { - LogErr() << "inet_pton failure for address: " << remote.ip; - send_successful = false; + std::stringstream ss; + ss << "inet_pton failure for: " << remote.ip << ":" << remote.port_number; + LogErr() << ss.str(); + result.first = false; + if (!result.second.empty()) { + result.second += ", "; + } + result.second += ss.str(); continue; } dest_addr.sin_port = htons(remote.port_number); @@ -151,13 +165,20 @@ bool UdpConnection::send_message(const mavlink_message_t& message) sizeof(dest_addr)); if (send_len != buffer_len) { - LogErr() << "sendto failure: " << GET_ERROR(errno); - send_successful = false; + std::stringstream ss; + ss << "sendto failure: " << GET_ERROR(errno) << " for: " << remote.ip << ":" + << remote.port_number; + LogErr() << ss.str(); + result.first = false; + if (!result.second.empty()) { + result.second += ", "; + } + result.second += ss.str(); continue; } } - return send_successful; + return result; } void UdpConnection::add_remote(const std::string& remote_ip, const int remote_port) diff --git a/src/mavsdk/core/udp_connection.h b/src/mavsdk/core/udp_connection.h index ea5f56865..f08e3f61d 100644 --- a/src/mavsdk/core/udp_connection.h +++ b/src/mavsdk/core/udp_connection.h @@ -24,7 +24,7 @@ class UdpConnection : public Connection { ConnectionResult start() override; ConnectionResult stop() override; - bool send_message(const mavlink_message_t& message) override; + std::pair send_message(const mavlink_message_t& message) override; void add_remote(const std::string& remote_ip, int remote_port);