From d9fc7d36a6948c291098e75ea706a61fd8ad16bb Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 5 Jun 2015 11:16:06 -0700 Subject: [PATCH 1/4] Added option to communicate with autopilot over UDP or Serial port Added command-line options to specify which protocol to use, with sane defaults. Signed-off-by: Mark Charlebois --- jMAVSim.ipr | 8 +- src/me/drton/jmavsim/MAVLinkPort.java | 2 + src/me/drton/jmavsim/SerialMAVLinkPort.java | 17 +++- src/me/drton/jmavsim/Simulator.java | 89 ++++++++++++++++++--- src/me/drton/jmavsim/UDPMavLinkPort.java | 25 +++++- 5 files changed, 121 insertions(+), 20 deletions(-) diff --git a/jMAVSim.ipr b/jMAVSim.ipr index f5b887ef..6713c951 100644 --- a/jMAVSim.ipr +++ b/jMAVSim.ipr @@ -41,7 +41,9 @@ - + + + @@ -175,7 +177,7 @@ - + @@ -194,4 +196,4 @@ - \ No newline at end of file + diff --git a/src/me/drton/jmavsim/MAVLinkPort.java b/src/me/drton/jmavsim/MAVLinkPort.java index 2978315c..52ce4995 100644 --- a/src/me/drton/jmavsim/MAVLinkPort.java +++ b/src/me/drton/jmavsim/MAVLinkPort.java @@ -12,6 +12,8 @@ protected MAVLinkPort(MAVLinkSchema schema) { super(schema); } + public abstract void open() throws IOException; + public abstract void close() throws IOException; public abstract boolean isOpened(); diff --git a/src/me/drton/jmavsim/SerialMAVLinkPort.java b/src/me/drton/jmavsim/SerialMAVLinkPort.java index 46fd6ae5..8e9ce1bf 100644 --- a/src/me/drton/jmavsim/SerialMAVLinkPort.java +++ b/src/me/drton/jmavsim/SerialMAVLinkPort.java @@ -20,16 +20,31 @@ public class SerialMAVLinkPort extends MAVLinkPort { private MAVLinkStream stream; private boolean debug = false; + // connection information + String portName; + int baudRate; + int dataBits; + int stopBits; + int parity; + public SerialMAVLinkPort(MAVLinkSchema schema) { super(schema); this.schema = schema; } + public void setup(String portName, int baudRate, int dataBits, int stopBits, int parity) { + this.portName = portName; + this.baudRate = baudRate; + this.dataBits = dataBits; + this.stopBits = stopBits; + this.parity = parity; + } + public void setDebug(boolean debug) { this.debug = debug; } - public void open(String portName, int baudRate, int dataBits, int stopBits, int parity) throws IOException { + public void open() throws IOException { serialPort = new SerialPort(portName); try { serialPort.openPort(); diff --git a/src/me/drton/jmavsim/Simulator.java b/src/me/drton/jmavsim/Simulator.java index f5314119..4b069d40 100644 --- a/src/me/drton/jmavsim/Simulator.java +++ b/src/me/drton/jmavsim/Simulator.java @@ -10,12 +10,23 @@ import javax.vecmath.Vector3d; import javax.xml.parsers.ParserConfigurationException; import java.io.IOException; -import java.net.InetSocketAddress; /** * User: ton Date: 26.11.13 Time: 12:33 */ public class Simulator { + + public static boolean USE_SERIAL_PORT = false; + public static final int DEFAULT_AUTOPILOT_PORT = 14550; + public static final int DEFAULT_QGC_PORT = 14555; + public static final String DEFAULT_SERIAL_PATH = "/dev/tty.usbmodem1"; + public static final int DEFAULT_SERIAL_BAUD_RATE = 230400; + + private static int autopilotPort = DEFAULT_AUTOPILOT_PORT; + private static int qgcPort = DEFAULT_QGC_PORT; + private static String serialPath = DEFAULT_SERIAL_PATH; + private static int serialBaudRate = DEFAULT_SERIAL_BAUD_RATE; + private World world; private int sleepInterval = 5; // Main loop interval, in ms @@ -38,13 +49,25 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration world.addObject(connCommon); // Create ports - // Serial port: connection to autopilot - SerialMAVLinkPort serialMAVLinkPort = new SerialMAVLinkPort(schema); - connCommon.addNode(serialMAVLinkPort); - connHIL.addNode(serialMAVLinkPort); + MAVLinkPort autopilotMavLinkPort; + if (USE_SERIAL_PORT) { + //Serial port: connection to autopilot over serial. + SerialMAVLinkPort port = new SerialMAVLinkPort(schema); + port.setup(serialPath, serialBaudRate, 8, 1, 0); + autopilotMavLinkPort = port; + } else { + UDPMavLinkPort port = new UDPMavLinkPort(schema); + port.setup(0, autopilotPort, true); // default source port 0 for autopilot, which is a client of JMAVSim + autopilotMavLinkPort = port; + } + + // allow HIL and GCS to talk to this port + connHIL.addNode(autopilotMavLinkPort); + connCommon.addNode(autopilotMavLinkPort); // UDP port: connection to ground station - UDPMavLinkPort udpMavLinkPort = new UDPMavLinkPort(schema); - connCommon.addNode(udpMavLinkPort); + UDPMavLinkPort udpGCMavLinkPort = new UDPMavLinkPort(schema); + udpGCMavLinkPort.setup(qgcPort, autopilotPort, false); + connCommon.addNode(udpGCMavLinkPort); // Create environment SimpleEnvironment simpleEnvironment = new SimpleEnvironment(world); @@ -106,10 +129,11 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration // Open ports //serialMAVLinkPort.setDebug(true); - serialMAVLinkPort.open("/dev/tty.usbmodem1", 230400, 8, 1, 0); - serialMAVLinkPort.sendRaw("\nsh /etc/init.d/rc.usb\n".getBytes()); + autopilotMavLinkPort.open(); + //serialMAVLinkPort.sendRaw("\nsh /etc/init.d/rc.usb\n".getBytes()); //udpMavLinkPort.setDebug(true); - udpMavLinkPort.open(new InetSocketAddress(14555), new InetSocketAddress(14550)); + udpGCMavLinkPort.open(); + //udpMavLinkPort.open(new InetSocketAddress(14555), new InetSocketAddress(14550)); // Run try { @@ -119,8 +143,8 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration } // Close ports - serialMAVLinkPort.close(); - udpMavLinkPort.close(); + autopilotMavLinkPort.close(); + udpGCMavLinkPort.close(); } public void run() throws IOException, InterruptedException { @@ -136,6 +160,47 @@ public void run() throws IOException, InterruptedException { public static void main(String[] args) throws InterruptedException, IOException, ParserConfigurationException, SAXException { + + String usageString = "java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator " + + "[-udp | -serial ]"; + // default is to use UDP. + if (args.length == 0) { + USE_SERIAL_PORT = false; + } + if (args.length > 0 && args.length < 3 || args.length > 3) { + System.err.println("Incorrect number of arguments. \n Usage: " + usageString); + return; + } + + int i = 0; + while (i < args.length) { + String arg = args[i++]; + if (arg.equalsIgnoreCase("-udp")) { + USE_SERIAL_PORT = false; + try { + autopilotPort = Integer.parseInt(args[i++]); + qgcPort = Integer.parseInt(args[i++]); + } catch (NumberFormatException e) { + System.out.println("Expected: " + usageString + ", got: " + e.toString()); + return; + } + } else if (arg.equals("-serial")) { + USE_SERIAL_PORT = true; + try { + serialPath = args[i++]; + serialBaudRate = Integer.parseInt(args[i++]); + } catch (NumberFormatException e) { + System.out.println("Expected: " + usageString + ", got: " + e.toString()); + return; + } + } + } + + if (i != args.length) { + System.err.println("Usage: " + usageString); + return; + } else { System.out.println("Success!"); } + new Simulator(); } } diff --git a/src/me/drton/jmavsim/UDPMavLinkPort.java b/src/me/drton/jmavsim/UDPMavLinkPort.java index a48d9a8b..35b300dc 100644 --- a/src/me/drton/jmavsim/UDPMavLinkPort.java +++ b/src/me/drton/jmavsim/UDPMavLinkPort.java @@ -5,6 +5,7 @@ import me.drton.jmavlib.mavlink.MAVLinkMessage; import java.io.IOException; +import java.net.InetSocketAddress; import java.net.SocketAddress; import java.nio.ByteBuffer; import java.nio.channels.DatagramChannel; @@ -16,6 +17,11 @@ public class UDPMavLinkPort extends MAVLinkPort { private MAVLinkSchema schema; private DatagramChannel channel = null; private ByteBuffer rxBuffer = ByteBuffer.allocate(8192); + private SocketAddress sendAddress; + private SocketAddress bindPort; + private SocketAddress peerPort; + private int portAddress; + private boolean isClient; private MAVLinkStream stream; private boolean debug = false; @@ -29,13 +35,24 @@ public void setDebug(boolean debug) { this.debug = debug; } - public void open(SocketAddress bindAddress, SocketAddress peerAddress) throws IOException { + public void setup(int bindPort, int peerPort, boolean client) { + this.bindPort = new InetSocketAddress("127.0.0.1", bindPort); + this.peerPort = new InetSocketAddress("127.0.0.1", peerPort); + this.isClient = client; + } + + public void open() throws IOException { channel = DatagramChannel.open(); - channel.socket().bind(bindAddress); channel.configureBlocking(false); - channel.connect(peerAddress); + channel.socket().bind(bindPort); + channel.connect(peerPort); stream = new MAVLinkStream(schema, channel); - stream.setDebug(debug); + if (isClient) { + // Client initiates communication. + MAVLinkMessage dummy = new MAVLinkMessage(schema, 0, 1, 51); + System.out.println("Sending dummy message"); + stream.write(dummy); + } } @Override From 6d304bd5c6affae6b5d0084a44248ad0a9a00270 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Mon, 8 Jun 2015 09:56:36 -0700 Subject: [PATCH 2/4] UDP support fixes Fixed issue with opening port, and adjusted command-line parsing. UDP support should now be functional and selectable as a command line option. Signed-off-by: Mark Charlebois --- src/me/drton/jmavsim/Simulator.java | 19 +++++++++++++++---- src/me/drton/jmavsim/UDPMavLinkPort.java | 13 +++---------- 2 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/me/drton/jmavsim/Simulator.java b/src/me/drton/jmavsim/Simulator.java index 4b069d40..5f2915e0 100644 --- a/src/me/drton/jmavsim/Simulator.java +++ b/src/me/drton/jmavsim/Simulator.java @@ -57,7 +57,7 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration autopilotMavLinkPort = port; } else { UDPMavLinkPort port = new UDPMavLinkPort(schema); - port.setup(0, autopilotPort, true); // default source port 0 for autopilot, which is a client of JMAVSim + port.setup(0, autopilotPort); // default source port 0 for autopilot, which is a client of JMAVSim autopilotMavLinkPort = port; } @@ -66,7 +66,7 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration connCommon.addNode(autopilotMavLinkPort); // UDP port: connection to ground station UDPMavLinkPort udpGCMavLinkPort = new UDPMavLinkPort(schema); - udpGCMavLinkPort.setup(qgcPort, autopilotPort, false); + udpGCMavLinkPort.setup(qgcPort, autopilotPort); connCommon.addNode(udpGCMavLinkPort); // Create environment @@ -162,12 +162,12 @@ public static void main(String[] args) throws InterruptedException, IOException, ParserConfigurationException, SAXException { String usageString = "java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator " + - "[-udp | -serial ]"; + "[-udp | -serial ]"; // default is to use UDP. if (args.length == 0) { USE_SERIAL_PORT = false; } - if (args.length > 0 && args.length < 3 || args.length > 3) { + if (args.length == 2 || args.length > 3) { System.err.println("Incorrect number of arguments. \n Usage: " + usageString); return; } @@ -177,7 +177,12 @@ public static void main(String[] args) String arg = args[i++]; if (arg.equalsIgnoreCase("-udp")) { USE_SERIAL_PORT = false; + if (args.length == 1) { + // only arg is -udp, so use default port values. + break; + } try { + // try to parse passed-in ports. autopilotPort = Integer.parseInt(args[i++]); qgcPort = Integer.parseInt(args[i++]); } catch (NumberFormatException e) { @@ -186,6 +191,10 @@ public static void main(String[] args) } } else if (arg.equals("-serial")) { USE_SERIAL_PORT = true; + if (args.length == 1) { + // only arg is -serial, so use default values + break; + } try { serialPath = args[i++]; serialBaudRate = Integer.parseInt(args[i++]); @@ -193,6 +202,8 @@ public static void main(String[] args) System.out.println("Expected: " + usageString + ", got: " + e.toString()); return; } + } else { + System.err.println("Unknown flag: " + arg + ", usage: " + usageString); } } diff --git a/src/me/drton/jmavsim/UDPMavLinkPort.java b/src/me/drton/jmavsim/UDPMavLinkPort.java index 35b300dc..44340989 100644 --- a/src/me/drton/jmavsim/UDPMavLinkPort.java +++ b/src/me/drton/jmavsim/UDPMavLinkPort.java @@ -21,7 +21,6 @@ public class UDPMavLinkPort extends MAVLinkPort { private SocketAddress bindPort; private SocketAddress peerPort; private int portAddress; - private boolean isClient; private MAVLinkStream stream; private boolean debug = false; @@ -35,24 +34,17 @@ public void setDebug(boolean debug) { this.debug = debug; } - public void setup(int bindPort, int peerPort, boolean client) { + public void setup(int bindPort, int peerPort) { this.bindPort = new InetSocketAddress("127.0.0.1", bindPort); this.peerPort = new InetSocketAddress("127.0.0.1", peerPort); - this.isClient = client; } public void open() throws IOException { channel = DatagramChannel.open(); - channel.configureBlocking(false); channel.socket().bind(bindPort); + channel.configureBlocking(false); channel.connect(peerPort); stream = new MAVLinkStream(schema, channel); - if (isClient) { - // Client initiates communication. - MAVLinkMessage dummy = new MAVLinkMessage(schema, 0, 1, 51); - System.out.println("Sending dummy message"); - stream.write(dummy); - } } @Override @@ -86,6 +78,7 @@ public void update(long t) { if (msg == null) { break; } + System.out.println("msg.name: " + msg.getMsgName() + ", type: " + msg.getMsgType()); sendMessage(msg); } catch (IOException e) { // Silently ignore this exception, we likely just have nobody on this port yet/already From 6bfc6b1bf213a2642f4c2fa2a63d059ad871f2b2 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 11 Jun 2015 09:13:44 -0700 Subject: [PATCH 3/4] Specify IP address on the command-line Added ability to specify IP address on the command-line, and turn off messages being sent to QGC Signed-off-by: Mark Charlebois --- src/me/drton/jmavsim/MavlinkTest.java | 3 +- src/me/drton/jmavsim/Simulator.java | 98 +++++++++++++++++++----- src/me/drton/jmavsim/UDPMavLinkPort.java | 10 ++- 3 files changed, 87 insertions(+), 24 deletions(-) diff --git a/src/me/drton/jmavsim/MavlinkTest.java b/src/me/drton/jmavsim/MavlinkTest.java index 0d0b23de..b1544cb0 100644 --- a/src/me/drton/jmavsim/MavlinkTest.java +++ b/src/me/drton/jmavsim/MavlinkTest.java @@ -29,7 +29,8 @@ public void update(long t) { } }; connection.addNode(node); - port.open("/dev/tty.usbmodem1", 57600, 8, 1, 0); + port.setup("/dev/tty.usbmodem1", 57600, 8, 1, 0); + port.open(); port.sendRaw("\nsh /etc/init.d/rc.usb\n".getBytes()); while (true) { port.update(System.currentTimeMillis()); diff --git a/src/me/drton/jmavsim/Simulator.java b/src/me/drton/jmavsim/Simulator.java index 5f2915e0..92f47cc5 100644 --- a/src/me/drton/jmavsim/Simulator.java +++ b/src/me/drton/jmavsim/Simulator.java @@ -10,6 +10,8 @@ import javax.vecmath.Vector3d; import javax.xml.parsers.ParserConfigurationException; import java.io.IOException; +import java.lang.reflect.Array; +import java.util.Arrays; /** * User: ton Date: 26.11.13 Time: 12:33 @@ -17,11 +19,14 @@ public class Simulator { public static boolean USE_SERIAL_PORT = false; + public static boolean COMMUNICATE_WITH_QGC = true; public static final int DEFAULT_AUTOPILOT_PORT = 14550; public static final int DEFAULT_QGC_PORT = 14555; public static final String DEFAULT_SERIAL_PATH = "/dev/tty.usbmodem1"; public static final int DEFAULT_SERIAL_BAUD_RATE = 230400; + public static final String LOCAL_HOST = "127.0.0.1"; + private static String autopilotIpAddress = LOCAL_HOST; private static int autopilotPort = DEFAULT_AUTOPILOT_PORT; private static int qgcPort = DEFAULT_QGC_PORT; private static String serialPath = DEFAULT_SERIAL_PATH; @@ -57,7 +62,7 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration autopilotMavLinkPort = port; } else { UDPMavLinkPort port = new UDPMavLinkPort(schema); - port.setup(0, autopilotPort); // default source port 0 for autopilot, which is a client of JMAVSim + port.setup(0, autopilotIpAddress, autopilotPort); // default source port 0 for autopilot, which is a client of JMAVSim autopilotMavLinkPort = port; } @@ -66,8 +71,10 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration connCommon.addNode(autopilotMavLinkPort); // UDP port: connection to ground station UDPMavLinkPort udpGCMavLinkPort = new UDPMavLinkPort(schema); - udpGCMavLinkPort.setup(qgcPort, autopilotPort); - connCommon.addNode(udpGCMavLinkPort); + if (COMMUNICATE_WITH_QGC) { + udpGCMavLinkPort.setup(qgcPort, LOCAL_HOST, autopilotPort); + connCommon.addNode(udpGCMavLinkPort); + } // Create environment SimpleEnvironment simpleEnvironment = new SimpleEnvironment(world); @@ -130,10 +137,10 @@ public Simulator() throws IOException, InterruptedException, ParserConfiguration // Open ports //serialMAVLinkPort.setDebug(true); autopilotMavLinkPort.open(); - //serialMAVLinkPort.sendRaw("\nsh /etc/init.d/rc.usb\n".getBytes()); //udpMavLinkPort.setDebug(true); - udpGCMavLinkPort.open(); - //udpMavLinkPort.open(new InetSocketAddress(14555), new InetSocketAddress(14550)); + if (COMMUNICATE_WITH_QGC) { + udpGCMavLinkPort.open(); + } // Run try { @@ -161,13 +168,16 @@ public void run() throws IOException, InterruptedException { public static void main(String[] args) throws InterruptedException, IOException, ParserConfigurationException, SAXException { + String udpString = "-udp :"; + String qgcString = " -qgc "; + String serialString = "-serial "; String usageString = "java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator " + - "[-udp | -serial ]"; + "[" + udpString + " | " + serialString + "] "+ qgcString; // default is to use UDP. if (args.length == 0) { USE_SERIAL_PORT = false; } - if (args.length == 2 || args.length > 3) { + if (args.length > 7) { System.err.println("Incorrect number of arguments. \n Usage: " + usageString); return; } @@ -175,18 +185,39 @@ public static void main(String[] args) int i = 0; while (i < args.length) { String arg = args[i++]; + if (arg.equalsIgnoreCase("--help")) { + System.out.println("Usage: " + usageString); + System.out.println("\n Note: if is set to -1, JMavSim won't generate Mavlink messages for GroundControl."); + return; + } if (arg.equalsIgnoreCase("-udp")) { USE_SERIAL_PORT = false; if (args.length == 1) { - // only arg is -udp, so use default port values. + // only arg is -udp, so use default values. break; } - try { - // try to parse passed-in ports. - autopilotPort = Integer.parseInt(args[i++]); - qgcPort = Integer.parseInt(args[i++]); - } catch (NumberFormatException e) { - System.out.println("Expected: " + usageString + ", got: " + e.toString()); + if (i < args.length) { + String nextArg = args[i++]; + if (nextArg.startsWith("-")) { + // only turning on udp, but want to use default ports + i--; + continue; + } + try { + // try to parse passed-in ports. + String[] list = nextArg.split(":"); + if (list.length != 2) { + System.err.println("Expected: " + udpString + ", got: " + Arrays.toString(list)); + return; + } + autopilotIpAddress = list[0]; + autopilotPort = Integer.parseInt(list[1]); + } catch (NumberFormatException e) { + System.err.println("Expected: " + usageString + ", got: " + e.toString()); + return; + } + } else { + System.err.println("-udp needs an argument: " + udpString); return; } } else if (arg.equals("-serial")) { @@ -195,15 +226,42 @@ public static void main(String[] args) // only arg is -serial, so use default values break; } - try { - serialPath = args[i++]; - serialBaudRate = Integer.parseInt(args[i++]); - } catch (NumberFormatException e) { - System.out.println("Expected: " + usageString + ", got: " + e.toString()); + if ( (i+2) <= args.length) { + try { + serialPath = args[i++]; + serialBaudRate = Integer.parseInt(args[i++]); + } catch (NumberFormatException e) { + System.err.println("Expected: " + usageString + ", got: " + e.toString()); + return; + } + } else { + System.err.println("-serial needs two arguments. Expected: " + serialString + ", got: " + Arrays.toString(args)); + return; + } + } else if (arg.equals("-qgc")) { + if (i < args.length) { + try { + qgcPort = Integer.parseInt(args[i++]); + if (qgcPort < 0) { + COMMUNICATE_WITH_QGC = false; + } else { + COMMUNICATE_WITH_QGC = true; + } + if (args.length == 1) { + // only arg is -qgc, so use default values + break; + } + } catch (NumberFormatException e) { + System.err.println("Expected: " + usageString + ", got: " + e.toString()); + return; + } + } else { + System.err.println("-qgc needs an argument: " + qgcString); return; } } else { System.err.println("Unknown flag: " + arg + ", usage: " + usageString); + return; } } diff --git a/src/me/drton/jmavsim/UDPMavLinkPort.java b/src/me/drton/jmavsim/UDPMavLinkPort.java index 44340989..fbd51713 100644 --- a/src/me/drton/jmavsim/UDPMavLinkPort.java +++ b/src/me/drton/jmavsim/UDPMavLinkPort.java @@ -5,8 +5,10 @@ import me.drton.jmavlib.mavlink.MAVLinkMessage; import java.io.IOException; +import java.net.Inet4Address; import java.net.InetSocketAddress; import java.net.SocketAddress; +import java.net.UnknownHostException; import java.nio.ByteBuffer; import java.nio.channels.DatagramChannel; @@ -34,9 +36,11 @@ public void setDebug(boolean debug) { this.debug = debug; } - public void setup(int bindPort, int peerPort) { - this.bindPort = new InetSocketAddress("127.0.0.1", bindPort); - this.peerPort = new InetSocketAddress("127.0.0.1", peerPort); + public void setup(int bindPort, String peerAddress, int peerPort) { + this.bindPort = new InetSocketAddress(Simulator.LOCAL_HOST, bindPort); + //this.peerPort = new InetSocketAddress("129.46.67.209", peerPort); + //this.bindPort = new InetSocketAddress("127.0.0.1", bindPort); + this.peerPort = new InetSocketAddress(peerAddress, peerPort); } public void open() throws IOException { From bc022d54d56fe8cb30c53a6d303193b9f239069a Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Fri, 12 Jun 2015 14:48:32 -0700 Subject: [PATCH 4/4] Use LAN interface instead of loopback Use the external network interface not loopback for sending. Passing in an IP address as a command argument now works. Signed-off-by: Mark Charlebois --- src/me/drton/jmavsim/UDPMavLinkPort.java | 66 ++++++++++++++++++++---- 1 file changed, 57 insertions(+), 9 deletions(-) diff --git a/src/me/drton/jmavsim/UDPMavLinkPort.java b/src/me/drton/jmavsim/UDPMavLinkPort.java index fbd51713..06f9ddce 100644 --- a/src/me/drton/jmavsim/UDPMavLinkPort.java +++ b/src/me/drton/jmavsim/UDPMavLinkPort.java @@ -5,12 +5,10 @@ import me.drton.jmavlib.mavlink.MAVLinkMessage; import java.io.IOException; -import java.net.Inet4Address; -import java.net.InetSocketAddress; -import java.net.SocketAddress; -import java.net.UnknownHostException; +import java.net.*; import java.nio.ByteBuffer; import java.nio.channels.DatagramChannel; +import java.util.Enumeration; /** * User: ton Date: 02.12.13 Time: 20:56 @@ -20,12 +18,14 @@ public class UDPMavLinkPort extends MAVLinkPort { private DatagramChannel channel = null; private ByteBuffer rxBuffer = ByteBuffer.allocate(8192); private SocketAddress sendAddress; - private SocketAddress bindPort; + private SocketAddress bindPort = null; private SocketAddress peerPort; private int portAddress; private MAVLinkStream stream; private boolean debug = false; + private String[] LOCAL_HOST_TERMS = { "localhost", "127.0.0.1" }; + public UDPMavLinkPort(MAVLinkSchema schema) { super(schema); this.schema = schema; @@ -36,11 +36,59 @@ public void setDebug(boolean debug) { this.debug = debug; } - public void setup(int bindPort, String peerAddress, int peerPort) { - this.bindPort = new InetSocketAddress(Simulator.LOCAL_HOST, bindPort); - //this.peerPort = new InetSocketAddress("129.46.67.209", peerPort); - //this.bindPort = new InetSocketAddress("127.0.0.1", bindPort); + public void setup(int bindPort, String peerAddress, int peerPort) throws UnknownHostException, IOException { this.peerPort = new InetSocketAddress(peerAddress, peerPort); + // If PX4 is running on localhost, we should connect on local host as well. + for (String term : LOCAL_HOST_TERMS) { + if (peerAddress.equalsIgnoreCase(term)) { + this.bindPort = new InetSocketAddress(term, bindPort); + } + } + // Otherwise, we should attempt to find the external IP address and connect over that. + if (this.bindPort == null) { + InetAddress localHostExternalIPAddress = getMyHostIPAddress(); + this.bindPort = new InetSocketAddress(localHostExternalIPAddress, bindPort); + } + if (debug) { + System.out.println("peerAddress: " + peerAddress + ", bindAddress: " + this.bindPort.toString()); + } + } + + /** + * Searches for the externally-reachable IP address for this machine. Note that if PX4 is running on + * a private network, this method may or may not work to setup communication. + * + * @return the best possible address found. + * @throws UnknownHostException + * @throws SocketException + * @throws IOException + */ + private static InetAddress getMyHostIPAddress() throws UnknownHostException, SocketException, IOException { + InetAddress possibleAddress = null; + // Look over all the network interfaces for the appropriate interface. + Enumeration networkInterfaces = NetworkInterface.getNetworkInterfaces(); + while (networkInterfaces.hasMoreElements()) { + NetworkInterface iface = networkInterfaces.nextElement(); + Enumeration addresses = iface.getInetAddresses(); + while (addresses.hasMoreElements()) { + InetAddress address = addresses.nextElement(); + if (!address.isLoopbackAddress()) { + if (address.isSiteLocalAddress()) { + // probably a good one! + return address; + } else { + // Found a non-loopback address that isn't site local. + // Might be link local (private network), but probably better than nothing. + possibleAddress = address; + } + } + } + } + // At this point, if we haven't found a better option, we better just take whatever Java thinks is best. + if (possibleAddress == null) { + possibleAddress = InetAddress.getLoopbackAddress(); + } + return possibleAddress; } public void open() throws IOException {