diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index 34fddfbd9..caae02800 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -372,19 +372,6 @@ void MavsdkImpl::receive_message(mavlink_message_t& message, Connection* connect std::lock_guard lock(_systems_mutex); - // The only situation where we create a system with sysid 0 is when we initialize the connection - // to the remote. - if (_systems.size() == 1 && _systems[0].first == 0) { - LogDebug() << "New: System ID: " << static_cast(message.sysid) - << " Comp ID: " << static_cast(message.compid); - _systems[0].first = message.sysid; - _systems[0].second->system_impl()->set_system_id(message.sysid); - - // Even though the fake system was already discovered, we can now - // send a notification, now that it seems to really actually exist. - notify_on_discover(); - } - bool found_system = false; for (auto& system : _systems) { if (system.first == message.sysid) { @@ -559,12 +546,8 @@ std::pair MavsdkImpl::setup_udp_remo new_conn->add_remote(remote_ip, remote_port); auto handle = add_connection(new_conn); std::lock_guard lock(_systems_mutex); - if (_systems.empty()) { - make_system_with_component(0, 0); - } - // With a UDP remote, we need to initiate the connection by sending - // heartbeats. + // With a UDP remote, we need to initiate the connection by sending heartbeats. auto new_configuration = get_configuration(); new_configuration.set_always_send_heartbeats(true); set_configuration(new_configuration); diff --git a/src/mavsdk/core/system_impl.cpp b/src/mavsdk/core/system_impl.cpp index e923d164a..bfa1980c8 100644 --- a/src/mavsdk/core/system_impl.cpp +++ b/src/mavsdk/core/system_impl.cpp @@ -504,8 +504,8 @@ void SystemImpl::send_autopilot_version_request() if (fut.get() == MavlinkCommandSender::Result::Unsupported) { _old_message_520_supported = false; - LogWarn() << "Trying alternative command (512)."; - send_autopilot_version_request(); + LogWarn() + << "Trying alternative command REQUEST_MESSAGE instead of REQUEST_AUTOPILOT_CAPABILITIES next."; } } @@ -541,12 +541,6 @@ void SystemImpl::set_connected() _connected = true; - // System with sysid 0 is a bit special: it is a placeholder for a connection initiated - // by MAVSDK. As such, it should not be advertised as a newly discovered system. - if (static_cast(get_system_id()) != 0) { - _mavsdk_impl.notify_on_discover(); - } - // We call this later to avoid deadlocks on creating the server components. _mavsdk_impl.call_user_callback([this]() { // Send a heartbeat back immediately. @@ -568,6 +562,9 @@ void SystemImpl::set_connected() } // If not yet connected there is nothing to do/ } + + _mavsdk_impl.notify_on_discover(); + if (enable_needed) { if (has_autopilot()) { send_autopilot_version_request_async(nullptr); @@ -591,10 +588,10 @@ void SystemImpl::set_disconnected() //_heartbeat_timeout_cookie = nullptr; _connected = false; - _mavsdk_impl.notify_on_timeout(); _is_connected_callbacks.queue( false, [this](const auto& func) { _mavsdk_impl.call_user_callback(func); }); } + _mavsdk_impl.notify_on_timeout(); _mavsdk_impl.stop_sending_heartbeats(); diff --git a/src/mavsdk/plugins/info/info_impl.cpp b/src/mavsdk/plugins/info/info_impl.cpp index 03aee905d..2e99a6e33 100644 --- a/src/mavsdk/plugins/info/info_impl.cpp +++ b/src/mavsdk/plugins/info/info_impl.cpp @@ -52,7 +52,7 @@ void InfoImpl::enable() { // We're going to retry until we have the version. _system_impl->add_call_every( - [this]() { _system_impl->send_autopilot_version_request(); }, 1.0f, &_call_every_cookie); + [this]() { _system_impl->send_autopilot_version_request(); }, 2.0f, &_call_every_cookie); if (!_flight_info_subscriptions.empty()) { // We're hoping to get flight information regularly to update flight time.