/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/MKTCPServer.java |
---|
0,0 → 1,148 |
package dongfang.mkt.main; |
import java.io.IOException; |
import java.io.InputStream; |
import java.io.OutputStream; |
import java.net.ServerSocket; |
import java.net.Socket; |
import java.net.SocketAddress; |
import java.util.HashMap; |
import java.util.HashSet; |
import java.util.Map; |
import java.util.Set; |
import dongfang.mkt.comm.MKConnection; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
public class MKTCPServer { |
private MKConnection mkConnection; |
private ServerSocket serverSocket; |
private Map<SocketAddress, Socket> connections = new HashMap<SocketAddress, Socket>(); |
private int port; |
public MKTCPServer(MKConnection mkConnection, int port) throws IOException { |
this.mkConnection = mkConnection; |
serverSocket = new ServerSocket(port); |
this.port = port; |
} |
public void start() { |
ClientConnectionServer svr = new ClientConnectionServer(); |
svr.setDaemon(true); |
svr.start(); |
new Manifold().start(); |
System.out.println("Waiting for connections at port " + port); |
} |
class ClientConnectionServer extends Thread { |
public void run() { |
while (true) { |
try { |
// accept() will block, preventing performance loss bc of inf looping. |
Socket clientSocket = serverSocket.accept(); |
System.out.println("Accepted a connection from " + clientSocket.getRemoteSocketAddress()); |
connections.put(clientSocket.getRemoteSocketAddress(), clientSocket); |
new Mux(clientSocket.getInputStream()).start(); |
} catch (IOException ex) { |
// ignore. |
System.err.println(ex); |
} |
} |
} |
} |
/* |
* This thread accepts new client connections but does not do anything for |
* transferring data. |
*/ |
void send(byte[] buf, int len) throws IOException { |
synchronized (mkConnection.getOutputStream()) { |
String snak = new String(buf, 0, len); |
//System.out.println("Sending on behalf of client: " + snak); |
mkConnection.getOutputStream().write(buf, 0, len); |
} |
} |
/* |
* For each client, this takes the client's output and sends frames on |
* (without mixing them up) to the common connection. An instance of this |
* must be made and started for each client. |
*/ |
class Mux extends Thread { |
InputStream input; |
byte[] buffer = new byte[1024]; |
Mux(InputStream input) { |
this.input = input; |
} |
public void run() { |
while (true) { |
try { |
int r; |
while ((r = input.read()) != '#') { |
// can't be! There should be only legal frames coming. |
} |
buffer[0] = '#'; |
int c = 1; |
while ((r = input.read()) != '\r') { |
buffer[c++] = (byte) r; |
} |
buffer[c++] = '\r'; |
send(buffer, c); |
} catch (IOException ex) { |
} |
} |
} |
} |
class Manifold extends Thread { |
public void run() { |
byte[] buf = new byte[1024]; |
int read; |
InputStream is = null; |
try { |
is = mkConnection.getInputStream(); |
} catch (IOException ex) { |
throw new RuntimeException(ex); |
} |
while(true) { |
try { |
read = is.read(buf, 0, buf.length); |
String snak = new String(buf, 0, read); |
Set<SocketAddress> keys = new HashSet<SocketAddress>(connections.keySet()); |
// Will read() block or return zero when no data is available? |
if (read > 0) { |
//System.out.println("Received: " + snak); |
for(SocketAddress a: keys) { |
try { |
Socket connection = connections.get(a); |
OutputStream os = connection.getOutputStream(); |
os.write(buf, 0, read); |
} catch (IOException ex) { |
// ignore errors writing to each client. |
} |
} |
} else { |
try { |
Thread.sleep(10); |
} catch (InterruptedException ex) {} |
} |
} catch (IOException ex) { |
// ignore. |
} |
} |
} |
} |
public static void main(String[] args) throws IOException { |
int port = Integer.parseInt(args[0]); |
MKConnection c = new RXTXSerialPort(); |
c.init(null); |
MKTCPServer server = new MKTCPServer(c, port); |
server.start(); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/OSDLogger.java |
---|
15,6 → 15,7 |
import dongfang.mkt.comm.FrameQueue; |
import dongfang.mkt.comm.MKConnection; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
import dongfang.mkt.comm.tcp.MKTCPConnection; |
import dongfang.mkt.frames.OSDDataRequestFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
64,11 → 65,13 |
w.write(" <currentPosition " + f.getCurrentPosition().toXML() + "/>\n"); |
w.write(" <targetPosition " + f.getTargetPosition().toXML() + " " + f.getCurrentToTarget().toXML() + "/>\n"); |
w.write(" <homePosition " + f.getHomePosition().toXML() + " " + f.getCurrentToHome().toXML() + "/>\n"); |
w.write(" <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + ((double)f.getGroundSpeed())/100 + "\"/>\n"); |
w.write(" <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + f.getGroundSpeed() + "\"/>\n"); |
w.write(" <waypoints index=\"" + f.getWaypointIndex() + "\" count=\"" + f.getWaypointCount() + "\"/>\n"); |
w.write(" <height barometric=\"" + f.getHeightByPressure() + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n"); |
double gpsHeight = f.getCurrentPosition().getAltitude() - f.getHomePosition().getAltitude(); |
w.write(" <height barometric=\"" + f.getHeightByPressure() + "\" gps=\"" + gpsHeight + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n"); |
w.write(" <status rcQuality=\"" + f.getRcQuality() + "\" fcflags=\"" + f.getFcFlags() + "\" fcflags2=\"" + f.getFcFlags2() + "\" ncflags=\"" + f.getNcFlags() + "\" errorCode=\"" + f.getErrorCode() + "\" numberOfSatellites=\"" + f.getNumberOfSatellites() + "\"/>\n"); |
w.write(" <flight time=\"" + f.getFlightTime() + "\" batteryVoltage=\"" + f.getBatteryVoltage() + "\" throttle=\"" + f.getThrottle() + "\" current=\"" + f.getCurrent() + "\" capacityUsed=\"" + f.getCapacityUsed() + "\"/>\n"); |
160,8 → 163,8 |
public static void main(String[] args) throws IOException { |
OSDLogger test = new OSDLogger(); |
MKConnection _port = new RXTXSerialPort(); |
_port.init(null); |
MKConnection _port = new MKTCPConnection(); //RXTXSerialPort(); |
_port.init("8080"); |
FrameQueue q = new FrameQueue(_port); |
String encoding = "iso-8859-1"; |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/SimpleOSDTool.java |
---|
0,0 → 1,64 |
package dongfang.mkt.main; |
import java.io.BufferedReader; |
import java.io.IOException; |
import java.io.InputStreamReader; |
import java.io.PrintStream; |
import javax.swing.JFrame; |
import dongfang.mkt.comm.FrameQueue; |
import dongfang.mkt.comm.MKConnection; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
import dongfang.mkt.comm.tcp.MKTCPConnection; |
import dongfang.mkt.frames.OSDDataRequestFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
import dongfang.mkt.ui.offscreendisplay.MapImageView; |
import dongfang.mkt.ui.offscreendisplay.OSDDataConsumer; |
import dongfang.mkt.ui.offscreendisplay.SimpleOSDView; |
public class SimpleOSDTool { |
private static final PrintStream STDERR = System.out; |
private void osd(final FrameQueue q, final OSDDataConsumer consumer, final int interval) throws IOException { |
long timer = 0; |
OSDDataRequestFrame f = new OSDDataRequestFrame(); |
f.setAutoSendInterval(interval); |
BufferedReader in = new BufferedReader(new InputStreamReader(System.in)); |
Long firsttimestamp = null; |
while (!in.ready()) |
try { |
long now = System.currentTimeMillis(); |
if (now - timer > 3000) { |
timer = now; |
q.sendRequest(f); |
} |
OSDDataResponseFrame rf = (OSDDataResponseFrame) q.getResponseFor(f, 5000); |
long timestamp = System.currentTimeMillis(); |
if (firsttimestamp == null) { |
firsttimestamp = timestamp; |
timestamp = 0; |
} else { |
timestamp -= firsttimestamp; |
} |
consumer.update(rf, timestamp); |
} catch (IOException ex) { |
STDERR.println(ex); |
} |
} |
public static void main(String[] args) throws IOException { |
SimpleOSDTool test = new SimpleOSDTool(); |
MKConnection _port = new MKTCPConnection(); // new RXTXSerialPort(); |
_port.init("8080"); |
FrameQueue q = new FrameQueue(_port); |
SimpleOSDView v = new SimpleOSDView (); |
v.init(); |
JFrame f = new JFrame(); |
f.getContentPane().add(v); |
f.setSize(v.getSize()); |
f.setVisible(true); |
v.setThatWidth(); |
test.osd(q, v, 20); |
} |
} |