From 5b58b916269e70849b8be1f2cf31e46815ed530b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 29 May 2020 14:16:09 +1000
Subject: [PATCH] SITL: allow connect back to originating port for matlab

---
 .../examples/JSON/MATLAB/SITL_connector.m     |  2 ++
 .../JSON/MATLAB/tcp_udp_ip_2.0.6/pnet.c       | 21 +++++++++++++------
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m b/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m
index bd0568d82f..5c1a4f0a39 100644
--- a/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m
+++ b/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m
@@ -87,6 +87,8 @@ while true
     physics_time_s = physics_time_s + state.delta_t;
 
     if ~connected
+        % use port -1 to indicate connection to address of last recv pkt
+        pnet(u,'udpconnect',"",-1);
         connected = true;
         [ip, port] = pnet(u,'gethost');
         fprintf('Connected to %i.%i.%i.%i:%i\n',ip,port)
diff --git a/libraries/SITL/examples/JSON/MATLAB/tcp_udp_ip_2.0.6/pnet.c b/libraries/SITL/examples/JSON/MATLAB/tcp_udp_ip_2.0.6/pnet.c
index 41d59899af..4f5d0cf1c7 100644
--- a/libraries/SITL/examples/JSON/MATLAB/tcp_udp_ip_2.0.6/pnet.c
+++ b/libraries/SITL/examples/JSON/MATLAB/tcp_udp_ip_2.0.6/pnet.c
@@ -844,7 +844,7 @@ int writedata()
           ) {
             con[con_index].status=STATUS_NOCONNECT;
             perror( "sendto() / send()" );
-            mexPrintf("\nREMOTE HOST DISCONNECTED\n");
+            mexPrintf("\nREMOTE HOST DISCONNECTED %d\n", s_errno);
             break;
         }
         if( !IS_STATUS_TCP_CONNECTED(con[con_index].status) && sentlen>0 )
@@ -927,8 +927,9 @@ int read2buff(const int len,int newline,int noblock)
         int readlen=len-con[con_index].read.pos;
 //	mexPrintf("DEBUG: READLINE: readlen:%d\n",readlen);
         if(readlen>0) {
-            if(IS_STATUS_CONNECTED(con[con_index].status))
+            if(IS_STATUS_CONNECTED(con[con_index].status)) {
                 retval=recv(con[con_index].fid,&con[con_index].read.ptr[con[con_index].read.pos],readlen,MSG_NOSIGNAL);
+            }
             else {
                 struct sockaddr_in my_addr;
                 int fromlen=sizeof(my_addr);
@@ -1068,14 +1069,22 @@ int udp_connect(const char *hostname,int port)
 {
     if(con[con_index].status < STATUS_UDP_CLIENT)
         mexErrMsgTxt("Must pass UDP client or server handler!");
-    if(ipv4_lookup(hostname,port)==-1)
+    if (port == -1) {
+        // use address of last received packet
+        con[con_index].remote_addr.sin_family=AF_INET;
+    } else if (ipv4_lookup(hostname,port)==-1) {
         return -1;
-    if(connect(con[con_index].fid,(struct sockaddr *)&con[con_index].remote_addr,sizeof(struct sockaddr)) == -1)
+    }
+    mexPrintf("connecting to port %d\n", con[con_index].remote_addr.sin_port);
+    if(connect(con[con_index].fid,(struct sockaddr *)&con[con_index].remote_addr,sizeof(struct sockaddr)) == -1) {
+        mexErrMsgTxt("connect failed");
         return -1; /*Can't connect to remote host. */
-    if(con[con_index].status == STATUS_UDP_CLIENT)
+    }
+    if(con[con_index].status == STATUS_UDP_CLIENT) {
         con[con_index].status = STATUS_UDP_CLIENT_CONNECT;
-    else
+    } else {
         con[con_index].status = STATUS_UDP_SERVER_CONNECT;
+    }
     nonblockingsocket(con[con_index].fid); /* Non blocking read! */
     return con[con_index].status;
 }