Mavsdk_server UDP connection requires PX4 restart

akontsevich · May 19, 2023, 8:08pm

Yep, as usual build process changed significantly, so need to alter our scripts as well to test - all build targets from new or old documentation fail:

$ make px4_sitl gz_x500
gmake[1]: *** No rule to make target 'gz_x500'.  Stop.
make: *** [Makefile:232: px4_sitl] Error 2

$ make px4_sitl gazebo-classic
gmake[1]: *** No rule to make target 'gazebo-classic'.  Stop.
make: *** [Makefile:232: px4_sitl] Error 2

make does not list any useful targets to build as well. Any ideas?

JulianOes · May 20, 2023, 3:45am

I would just try to backport (use git cherry-pick) my PX4 change to your PX4 version.

Alternatively, if you want to hard-code the local MAVSDK port, just add a number here:

github.com

mavlink/MAVSDK/blob/efa68deaf56fc1286a6e6bb3e2076053b71fdc25/src/mavsdk/core/mavsdk_impl.cpp#L524


      
          }
          
          
ConnectionResult MavsdkImpl::setup_udp_remote(
              const std::string& remote_ip, int remote_port, ForwardingOption forwarding_option)
          {
              auto new_conn = std::make_shared<UdpConnection>(
                  [this](mavlink_message_t& message, Connection* connection) {
                      receive_message(message, connection);
                  },
                  "0.0.0.0",
                  0,
                  forwarding_option);
              if (!new_conn) {
                  return ConnectionResult::ConnectionError;
              }
              ConnectionResult ret = new_conn->start();
              if (ret == ConnectionResult::Success) {
                  new_conn->add_remote(remote_ip, remote_port);
                  add_connection(new_conn);
                  std::lock_guard<std::recursive_mutex> lock(_systems_mutex);
                  make_system_with_component(0, 0, true);

or here (should be the same):

github.com

mavlink/MAVSDK/blob/efa68deaf56fc1286a6e6bb3e2076053b71fdc25/src/mavsdk/core/udp_connection.cpp#L82


      
              _socket_fd = socket(AF_INET, SOCK_DGRAM, 0);
          
          
    if (_socket_fd < 0) {
                  LogErr() << "socket error" << GET_ERROR(errno);
                  return ConnectionResult::SocketError;
              }
          
          
    struct sockaddr_in addr {};
              addr.sin_family = AF_INET;
              inet_pton(AF_INET, _local_ip.c_str(), &(addr.sin_addr));
              addr.sin_port = htons(_local_port_number);
          
          
    if (bind(_socket_fd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) != 0) {
                  LogErr() << "bind error: " << GET_ERROR(errno);
                  return ConnectionResult::BindError;
              }
          
          
    return ConnectionResult::Success;
          }
          
          
void UdpConnection::start_recv_thread()

akontsevich · May 20, 2023, 2:39pm

Yes this works, thank You! And Now I can reconnect!

JulianOes · May 20, 2023, 9:34pm

Awesome, thanks for the note!

akontsevich · May 29, 2023, 10:02am

So now if I restart mavsdk_server connection is reestablished. However if I restart PX4 - it does not: simply because mavsdk_server does not have any re-connection mechanism. So think good to add it there as well.

JulianOes · June 4, 2023, 9:13pm

Hm, UDP shouldn’t require a “re-connection mechanism” as such but it should respond to the new remote port.

If you look at the implementation it basically adds a new remote (IP and port) when new traffic arrives on a port such as 14550 or 14540, and then should respond to that new remote.

github.com

mavlink/MAVSDK/blob/ea0b720538cff8e51947557234cb9c4300edadc3/src/mavsdk/core/udp_connection.cpp#L175-L197


      
          void UdpConnection::add_remote_with_remote_sysid(
              const std::string& remote_ip, const int remote_port, const uint8_t remote_sysid)
          {
              std::lock_guard<std::mutex> lock(_remote_mutex);
              Remote new_remote;
              new_remote.ip = remote_ip;
              new_remote.port_number = remote_port;
          
          
    auto existing_remote =
                  std::find_if(_remotes.begin(), _remotes.end(), [&new_remote](Remote& remote) {
                      return remote == new_remote;
                  });
          
          
    if (existing_remote == _remotes.end()) {
                  // 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<int>(remote_sysid) != 0) {
                      LogInfo() << "New system on: " << new_remote.ip << ":" << new_remote.port_number
                                << " (with system ID: " << static_cast<int>(remote_sysid) << ")";
                  }

This file has been truncated. show original

Can you share some output what happens exactly in your case where it doesn’t work?

akontsevich · June 6, 2023, 2:06pm

Hmm, I was wrong it reconnects automatically. Never mind then - problem on our end.