Merge pull request #130 from PX4/pr-udp-connection

src: enable UDP connection in client and host mode
PX4 · Sep 2, 2021 · 0a5a8c6 · 0a5a8c6
2 parents 0a5375a + 2590b24
commit 0a5a8c6
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 15 deletions.
diff --git a/src/me/drton/jmavsim/Simulator.java b/src/me/drton/jmavsim/Simulator.java
@@ -234,7 +234,7 @@ public Simulator() throws IOException, InterruptedException {
         } else {
             UDPMavLinkPort port = new UDPMavLinkPort(schema);
             port.setDebug(DEBUG_MODE);
-            port.setup(autopilotIpAddress, autopilotPort);
+            port.setupHost(autopilotPort);
             if (monitorMessage) {
                 port.setMonitorMessageID(monitorMessageIds);
             }
@@ -264,7 +264,7 @@ public Simulator() throws IOException, InterruptedException {
 
             udpGCMavLinkPort = new UDPMavLinkPort(schema);
             udpGCMavLinkPort.setDebug(DEBUG_MODE);
-            udpGCMavLinkPort.setup(qgcIpAddress, qgcPeerPort);
+            udpGCMavLinkPort.setupClient(qgcIpAddress, qgcPeerPort);
             if (monitorMessage && PORT == Port.SERIAL) {
                 udpGCMavLinkPort.setMonitorMessageID(monitorMessageIds);
             }
@@ -284,7 +284,7 @@ public Simulator() throws IOException, InterruptedException {
 
             udpSDKMavLinkPort = new UDPMavLinkPort(schema);
             udpSDKMavLinkPort.setDebug(DEBUG_MODE);
-            udpSDKMavLinkPort.setup(sdkIpAddress, sdkPeerPort);
+            udpSDKMavLinkPort.setupClient(sdkIpAddress, sdkPeerPort);
             if (monitorMessage && PORT == Port.SERIAL) {
                 udpSDKMavLinkPort.setMonitorMessageID(monitorMessageIds);
             }
@@ -625,7 +625,7 @@ public void advanceTime() {
     }
 
     public final static String PRINT_INDICATION_STRING = "-m [<MsgID[, MsgID]...>]";
-    public final static String UDP_STRING = "-udp <mav ip>:<mav port>";
+    public final static String UDP_STRING = "-udp <mav port>";
     public final static String TCP_STRING = "-tcp <mav ip>:<mav port>";
     public final static String QGC_STRING = "-qgc <qgc ip address>:<qgc peer port>";
     public final static String SDK_STRING = "-sdk <sdk ip address>:<sdk peer port>";
@@ -714,14 +714,7 @@ public static void main(String[] args)
                         continue;
                     }
                     try {
-                        // try to parse passed-in ports.
-                        String[] list = nextArg.split(":");
-                        if (list.length != 2) {
-                            System.err.println("Expected: " + UDP_STRING + ", got: " + Arrays.toString(list));
-                            return;
-                        }
-                        autopilotIpAddress = list[0];
-                        autopilotPort = Integer.parseInt(list[1]);
+                        autopilotPort = Integer.parseInt(nextArg);
                     } catch (NumberFormatException e) {
                         System.err.println("Expected: " + USAGE_STRING + ", got: " + e.toString());
                         return;

diff --git a/src/me/drton/jmavsim/UDPMavLinkPort.java b/src/me/drton/jmavsim/UDPMavLinkPort.java
@@ -49,20 +49,33 @@ public void setDebug(boolean debug) {
         this.debug = debug;
     }
 
-    public void setup(String peerAddress, int peerPort) throws UnknownHostException, IOException {
+    public void setupClient(String peerAddress, int peerPort) throws UnknownHostException, IOException {
         this.peerPort = new InetSocketAddress(peerAddress, peerPort);
         this.bindPort = new InetSocketAddress("0.0.0.0", 0);
         if (debug) {
             System.out.println("peerAddress: " + this.peerPort.toString() + ", bindAddress: " +
                                this.bindPort.toString());
         }
+
+        channel = DatagramChannel.open();
+        channel.socket().bind(bindPort);
+        channel.configureBlocking(false);
+        channel.connect(this.peerPort);
     }
 
-    public void open() throws IOException {
+    public void setupHost(int peerPort) throws UnknownHostException, IOException {
+        this.bindPort = new InetSocketAddress("0.0.0.0", peerPort);
         channel = DatagramChannel.open();
         channel.socket().bind(bindPort);
+        channel.configureBlocking(true);
+        System.out.println("waiting for first message from: " + this.bindPort.toString());
+        this.peerPort = channel.receive(this.rxBuffer);
+        System.out.println("received first message from: " + this.peerPort.toString());
         channel.configureBlocking(false);
-        channel.connect(peerPort);
+        channel.connect(this.peerPort);
+    }
+
+    public void open() throws IOException {
         stream = new MAVLinkStream(schema, channel);
         stream.setDebug(debug);
     }