Added command line option for UDP connection to autopilot

jMAVSim.ipr

@@ -41,7 +41,9 @@
       </profile>
     </annotationProcessing>
   </component>
-  <component name="CopyrightManager" default="" />
+  <component name="CopyrightManager" default="">
+    <module2copyright />
+  </component>
   <component name="DependencyValidationManager">
     <option name="SKIP_IMPORT_STATEMENTS" value="false" />
   </component>
@@ -175,7 +177,7 @@
       <module fileurl="file://$PROJECT_DIR$/jMAVSim.iml" filepath="$PROJECT_DIR$/jMAVSim.iml" />
     </modules>
   </component>
-  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_6" default="false" assert-keyword="true" jdk-15="true" project-jdk-name="1.7" project-jdk-type="JavaSDK">
+  <component name="ProjectRootManager" version="2" languageLevel="JDK_1_6" assert-keyword="true" jdk-15="true" project-jdk-name="1.7" project-jdk-type="JavaSDK">
     <output url="file://$PROJECT_DIR$/out" />
   </component>
   <component name="VcsDirectoryMappings">
@@ -194,4 +196,4 @@
       <jarDirectory url="file://$PROJECT_DIR$/lib" recursive="false" />
     </library>
   </component>
-</project>
+</project>

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();

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());

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();

src/me/drton/jmavsim/Simulator.java

@@ -10,12 +10,28 @@
 import javax.vecmath.Vector3d;
 import javax.xml.parsers.ParserConfigurationException;
 import java.io.IOException;
-import java.net.InetSocketAddress;
+import java.lang.reflect.Array;
+import java.util.Arrays;
 
 /**
  * User: ton Date: 26.11.13 Time: 12:33
  */
 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;
+    private static int serialBaudRate = DEFAULT_SERIAL_BAUD_RATE;
+
     private World world;
     private int sleepInterval = 5;  // Main loop interval, in ms
 
@@ -38,13 +54,27 @@ 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, autopilotIpAddress, autopilotPort); // 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);
+        if (COMMUNICATE_WITH_QGC) {
+            udpGCMavLinkPort.setup(qgcPort, LOCAL_HOST, autopilotPort);
+            connCommon.addNode(udpGCMavLinkPort);
+        }
 
         // Create environment
         SimpleEnvironment simpleEnvironment = new SimpleEnvironment(world);
@@ -106,10 +136,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();
         //udpMavLinkPort.setDebug(true);
-        udpMavLinkPort.open(new InetSocketAddress(14555), new InetSocketAddress(14550));
+        if (COMMUNICATE_WITH_QGC) {
+            udpGCMavLinkPort.open();
+        }
 
         // Run
         try {
@@ -119,8 +150,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 +167,109 @@ public void run() throws IOException, InterruptedException {
 
     public static void main(String[] args)
             throws InterruptedException, IOException, ParserConfigurationException, SAXException {
+
+        String udpString = "-udp <autopilot ip address>:<autopilot port>";
+        String qgcString = " -qgc <qgc port>";
+        String serialString = "-serial <path> <baudRate>";
+        String usageString = "java -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator " +
+                "[" + udpString + " | " + serialString + "] "+ qgcString;
+        // default is to use UDP.
+        if (args.length == 0) {
+            USE_SERIAL_PORT = false;
+        }
+        if (args.length > 7) {
+            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("--help")) {
+                System.out.println("Usage: " + usageString);
+                System.out.println("\n Note: if <qgc <port> 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 values.
+                    break;
+                }
+                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")) {
+                USE_SERIAL_PORT = true;
+                if (args.length == 1) {
+                    // only arg is -serial, so use default values
+                    break;
+                }
+                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;
+            }
+         }
+
+        if (i != args.length) {
+            System.err.println("Usage: " + usageString);
+            return;
+        } else { System.out.println("Success!"); }
+
         new Simulator();
     }
 }

src/me/drton/jmavsim/UDPMavLinkPort.java

@@ -5,9 +5,10 @@
 import me.drton.jmavlib.mavlink.MAVLinkMessage;
 
 import java.io.IOException;
-import java.net.SocketAddress;
+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
@@ -16,9 +17,15 @@ public class UDPMavLinkPort extends MAVLinkPort {
     private MAVLinkSchema schema;
     private DatagramChannel channel = null;
     private ByteBuffer rxBuffer = ByteBuffer.allocate(8192);
+    private SocketAddress sendAddress;
+    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;
@@ -29,13 +36,67 @@ public void setDebug(boolean debug) {
         this.debug = debug;
     }
 
-    public void open(SocketAddress bindAddress, SocketAddress peerAddress) throws IOException {
+    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<NetworkInterface> networkInterfaces = NetworkInterface.getNetworkInterfaces();
+        while (networkInterfaces.hasMoreElements()) {
+           NetworkInterface iface = networkInterfaces.nextElement();
+           Enumeration<InetAddress> 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 {
         channel = DatagramChannel.open();
-        channel.socket().bind(bindAddress);
+        channel.socket().bind(bindPort);
         channel.configureBlocking(false);
-        channel.connect(peerAddress);
+        channel.connect(peerPort);
         stream = new MAVLinkStream(schema, channel);
-        stream.setDebug(debug);
     }
 
     @Override
@@ -69,6 +130,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