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/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/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..92f47cc5 100644
--- a/src/me/drton/jmavsim/Simulator.java
+++ b/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 :";
+ String qgcString = " -qgc ";
+ String serialString = "-serial ";
+ 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 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();
}
}
diff --git a/src/me/drton/jmavsim/UDPMavLinkPort.java b/src/me/drton/jmavsim/UDPMavLinkPort.java
index a48d9a8b..06f9ddce 100644
--- a/src/me/drton/jmavsim/UDPMavLinkPort.java
+++ b/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 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 {
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