5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00

SITL: allow connect back to originating port for matlab

This commit is contained in:
Andrew Tridgell 2020-05-29 14:16:09 +10:00
parent f5e98a6d69
commit 5b58b91626
2 changed files with 17 additions and 6 deletions
libraries/SITL/examples/JSON/MATLAB
SITL_connector.m
tcp_udp_ip_2.0.6

View File

@ -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)

View File

@ -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;
}