Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1567 → Rev 1568

/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,10 → 65,12
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);
}
}