Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1562 → Rev 1563

/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/FrameQueue.java
0,0 → 1,87
package dongfang.mkt.comm;
 
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
 
import dongfang.mkt.frames.RequestFrame;
import dongfang.mkt.frames.ResponseFrame;
 
/**
* Thread safe!
*
* @author dongfang
*/
public class FrameQueue {
private final MKInputStream input;
private final MKOutputStream output;
// private List responseQueue;
private ResponseFrame lastResponseFrame;
private boolean doQueue = true;
 
class Receiver extends Thread {
public void run() {
while (doQueue) {
try {
ResponseFrame f = input.getNextFrame();
synchronized (FrameQueue.this.input) {
lastResponseFrame = f;
FrameQueue.this.input.notifyAll();
}
} catch (IOException ex) {
System.err.println(ex);
}
}
System.out.println("Receiver terminated.");
}
}
 
public FrameQueue(MKConnection port) throws IOException {
super();
this.input = new MKInputStream (port.getInputStream());
this.output = new MKOutputStream(port.getOutputStream());
new Receiver().start();
}
 
public FrameQueue(InputStream in, OutputStream out) throws IOException {
super();
this.input = new MKInputStream (in);
this.output = new MKOutputStream(out);
new Receiver().start();
}
 
public void sendRequest(RequestFrame f) throws IOException {
synchronized (this.output) {
output.write(f);
}
}
 
public ResponseFrame getResponseFor(RequestFrame f, int maxwait) throws IOException {
ResponseFrame response;
long timeout = System.currentTimeMillis() + maxwait;
synchronized (input) {
while ((response = responseTo(f)) == null && System.currentTimeMillis() < timeout) {
try {
input.wait(100);
} catch (InterruptedException ex) {
}
}
}
return response;
}
 
public void kill() {
doQueue = false;
}
 
private ResponseFrame responseTo(RequestFrame f) {
synchronized (this.input) {
if (lastResponseFrame != null && lastResponseFrame.isResponseTo(f)) {
ResponseFrame result = lastResponseFrame;
lastResponseFrame = null;
return result;
}
return null;
}
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKConnection.java
0,0 → 1,11
package dongfang.mkt.comm;
 
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
 
public interface MKConnection {
void init(String id) throws IOException;
InputStream getInputStream() throws IOException ;
OutputStream getOutputStream() throws IOException ;
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKInputStream.java
0,0 → 1,401
package dongfang.mkt.comm;
 
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
 
import dongfang.mkt.datatype.GPSBearingAndRange;
import dongfang.mkt.datatype.GPSPosition;
import dongfang.mkt.frames.AllDisplaysResponseFrame;
import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;
import dongfang.mkt.frames.AttitudeDataResponseFrame;
import dongfang.mkt.frames.ChangeParameterSetResponseFrame;
import dongfang.mkt.frames.CompassHeadingResponseFrame;
import dongfang.mkt.frames.ConfirmFrame;
import dongfang.mkt.frames.DebugResponseFrame;
import dongfang.mkt.frames.MotorTestResponseFrame;
import dongfang.mkt.frames.OSDDataResponseFrame;
import dongfang.mkt.frames.ReadExternalControlResponseFrame;
import dongfang.mkt.frames.ResponseFrame;
import dongfang.mkt.frames.SetCompassHeadingResponseFrame;
import dongfang.mkt.frames.UniversalReadParamSetResponseFrame;
import dongfang.mkt.frames.UniversalWriteParamSetResponseFrame;
import dongfang.mkt.frames.VariablesResponseFrame;
import dongfang.mkt.frames.VersionResponseFrame;
 
public class MKInputStream extends InputStream {
int readByteCnt;
class MKDataInputStream {
int[] inbuf = new int[4];
int[] outbuf = new int[3];
int outbufptr = outbuf.length; // reset to "buffer empty"
 
private boolean decode() throws IOException {
for (int i = 0; i < 4; i++) {
int raw = MKInputStream.this.readByte();
int in = raw - '=';
if (in < 0 || in > 63)
return false;
// throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!");
inbuf[i] = in;
readByteCnt++;
}
outbuf[0] = (inbuf[0] << 2) | (inbuf[1] >>> 4);
outbuf[1] = ((inbuf[1] & 0x0f) << 4) | (inbuf[2] >>> 2);
outbuf[2] = ((inbuf[2] & 0x03) << 6) | inbuf[3];
outbufptr = 0;
return true;
}
 
public void reset() {
outbufptr = outbuf.length; // reset to "buffer empty"
}
 
public int readByte() throws IOException {
if (outbufptr > 2 && !decode())
throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!");
return outbuf[outbufptr++];
}
 
public int readSignedByte() throws IOException {
byte result = (byte)readByte();
return result;
}
 
public int readWord() throws IOException {
int byte0 = readByte();
int byte1 = readByte();
return (byte1 << 8) | byte0;
}
 
public int readSignedWord() throws IOException {
int word = readWord();
if (word > 32767)
word = word - 65536;
return word;
}
public int readSignedDWord() throws IOException {
int byte0 = readByte();
int byte1 = readByte();
int byte2 = readByte();
int byte3 = readByte();
return (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0;
}
 
public int[] readBytes(int length) throws IOException {
int[] result = new int[length];
for (int i = 0; i < length; i++) {
result[i] = readByte();
}
return result;
}
 
public int[] readWords(int length) throws IOException {
int[] result = new int[length];
for (int i = 0; i < length; i++) {
result[i] = readWord();
}
return result;
}
 
public int[] readSignedWords(int length) throws IOException {
int[] result = new int[length];
for (int i = 0; i < length; i++) {
result[i] = readSignedWord();
}
return result;
}
 
public char[] readChars(int length) throws IOException {
char[] result = new char[length];
for (int i = 0; i < length; i++) {
// Here, a 1:1 mapping between byte values and char codes is assumed.
// That means we're assuming ISO-8859-1 (= the first 256 code points
// of Unicode, which Java uses for chars)
result[i] = (char) readByte();
}
return result;
}
}
 
MKDataInputStream base64InputStream = new MKDataInputStream();
OutputStream nonPacketSpillway = null; //System.err;
final InputStream is;
int crc;
 
public MKInputStream(InputStream is) {
this.is = is;
}
@Override
public int read() throws IOException {
int i;
while ((i=is.read()) == -1);
// System.out.print("Received: " + i + " (as char: " + (char)i + ")\n");
return i;
}
 
public int readByte() throws IOException {
int _byte = read();
if (_byte < 0)
throw new IOException("End of Stream!");
crc += _byte;
return _byte;
}
 
public MKDataInputStream getBase64InputStream() {
return base64InputStream;
}
 
public ResponseFrame getNextFrame() throws IOException {
int c;
while ((c = read()) != '#') {
// throw it on some scrap-text buffer.
if (nonPacketSpillway != null)
nonPacketSpillway.write(c);
}
crc = '#';
base64InputStream.reset();
int address = readByte() - 'a';
int iid = readByte();
readByteCnt = 0;
//RESPONSE_IDS id = getResponseType(iid);
ResponseFrame result;
// System.out.println("Received a: " + (char)iid + " from " + address);
switch (iid) {
case 'A': {
AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address);
f.setChannel(base64InputStream.readByte());
f.setLabel(base64InputStream.readChars(16));
result = f;
break;
}
case 'B': {
ConfirmFrame f = new ConfirmFrame(address);
f.setFrameNum(base64InputStream.readByte());
result = f;
break;
}
case 'C': {
AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address);
f.setPitch(base64InputStream.readSignedWord());
f.setRoll(base64InputStream.readSignedWord());
f.setHeading(base64InputStream.readSignedWord());
f.setExpansion(base64InputStream.readBytes(8));
result = f;
break;
}
case 'D': {
DebugResponseFrame f = new DebugResponseFrame(address);
for (int i=0; i<2; i++)
f.setDigital(i, base64InputStream.readByte());
for (int i=0; i<32; i++)
f.setAnalog(i, base64InputStream.readSignedWord());
result = f;
break;
}
case 'F': {
ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);
f.setParameterSetNumber(base64InputStream.readByte());
result = f;
break;
}
case 'G': {
ReadExternalControlResponseFrame f = new ReadExternalControlResponseFrame(address);
f.setDigital(base64InputStream.readBytes(2));
f.setRemoteButtons(base64InputStream.readByte());
f.setPitch(base64InputStream.readByte());
f.setRoll(base64InputStream.readByte());
f.setYaw(base64InputStream.readByte());
f.setThrottle(base64InputStream.readByte());
f.setHeight(base64InputStream.readByte());
f.setCommand(base64InputStream.readByte());
f.setFrameNum(base64InputStream.readByte());
f.setArgument(base64InputStream.readByte());
result = f;
break;
}
case 'H': {
AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
f.setLine(base64InputStream.readByte());
//f.setMaxItem(getDataInputStream().readByte());
f.setText(base64InputStream.readChars(20));
result = f;
break;
}
case 'k' : {
CompassHeadingResponseFrame f = new CompassHeadingResponseFrame(address);
base64InputStream.readSignedWords(2);
base64InputStream.readBytes(2);
base64InputStream.readByte();
base64InputStream.readByte();
result = f;
break;
}
case 'L': {
AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
f.setItem(base64InputStream.readByte());
// f.setMaxItem(getDataInputStream().readByte());
f.setText(base64InputStream.readChars(80));
result = f;
break;
}
case 'O': {
OSDDataResponseFrame f = new OSDDataResponseFrame(address);
f.setVersion(base64InputStream.readByte());
GPSPosition pos = new GPSPosition();
pos.setLongitude(base64InputStream.readSignedDWord());
pos.setLatitude(base64InputStream.readSignedDWord());
pos.setAltitude(base64InputStream.readSignedDWord());
pos.setStatus(base64InputStream.readByte());
f.setCurrentPosition(pos);
pos = new GPSPosition();
pos.setLongitude(base64InputStream.readSignedDWord());
pos.setLatitude(base64InputStream.readSignedDWord());
pos.setAltitude(base64InputStream.readSignedDWord());
pos.setStatus(base64InputStream.readByte());
f.setTargetPosition(pos);
GPSBearingAndRange rnb = new GPSBearingAndRange();
rnb.setDistance(base64InputStream.readWord());
rnb.setBearing(base64InputStream.readSignedWord());
f.setCurrentToTarget(rnb);
 
pos = new GPSPosition();
pos.setLongitude(base64InputStream.readSignedDWord());
pos.setLatitude(base64InputStream.readSignedDWord());
pos.setAltitude(base64InputStream.readSignedDWord());
pos.setStatus(base64InputStream.readByte());
f.setHomePosition(pos);
rnb = new GPSBearingAndRange();
rnb.setDistance(base64InputStream.readWord());
rnb.setBearing(base64InputStream.readSignedWord());
f.setCurrentToHome(rnb);
f.setWaypointIndex(base64InputStream.readByte());
f.setWaypointCount(base64InputStream.readByte());
f.setNumberOfSatellites(base64InputStream.readByte());
f.setHeightByPressure(base64InputStream.readSignedWord());
f.setVerticalVelocityByPressure(base64InputStream.readSignedWord());
f.setFlightTime(base64InputStream.readWord());
f.setBatteryVoltage(base64InputStream.readByte());
f.setGroundSpeed(base64InputStream.readWord());
f.setDirectionOfFlight(base64InputStream.readSignedWord());
f.setCompassHeading(base64InputStream.readSignedWord());
f.setPitchAngle(base64InputStream.readSignedByte());
f.setRollAngle(base64InputStream.readSignedByte());
f.setRcQuality(base64InputStream.readByte());
f.setFcFlags(base64InputStream.readByte());
f.setNcFlags(base64InputStream.readByte());
f.setErrorCode(base64InputStream.readByte());
f.setOperatingRadius(base64InputStream.readByte());
f.setVerticalVelocityByGPS(base64InputStream.readSignedWord());
f.setTargetLoiterTime(base64InputStream.readByte());
f.setFcFlags2(base64InputStream.readByte());
f.setSetpointForAltitude(base64InputStream.readSignedWord());
f.setThrottle(base64InputStream.readByte());
f.setCurrent(base64InputStream.readWord());
f.setCapacityUsed(base64InputStream.readWord());
result = f;
break;
}
case 'S': {
UniversalWriteParamSetResponseFrame f = new UniversalWriteParamSetResponseFrame(address);
f.setParameterSetNumber(base64InputStream.readByte());
result = f;
break;
}
case 'T': {
MotorTestResponseFrame f = new MotorTestResponseFrame(address);
result = f;
break;
}
/*
* We have a collision with the 'x' token: Also used for VariablesRequest.
case 'x': {
LoopbackTestResponseFrame f = new LoopbackTestResponseFrame(address);
f.setByte(getDataInputStream().readByte());
f.setWord(getDataInputStream().readWord());
f.setChararray(getDataInputStream().readChars(8));
result = f;
break;
}
*/
case 'V': {
VersionResponseFrame f = new VersionResponseFrame(address);
f.setSWMajor(base64InputStream.readByte());
f.setSWMinor(base64InputStream.readByte());
f.setProtoMajor(base64InputStream.readByte());
f.setProtoMinor(base64InputStream.readByte());
f.setSWPatch(base64InputStream.readByte());
f.setHardwareErrors(base64InputStream.readBytes(5));
result = f;
break;
}
// This is my own creation. The ID collides with the waypoint one of FC.
case 'X': {
VariablesResponseFrame f = new VariablesResponseFrame(address);
f.setVariables(base64InputStream.readWords(8));
result = f;
break;
}
case 'w': {
SetCompassHeadingResponseFrame f = new SetCompassHeadingResponseFrame(address);
// do stuff.
/*
ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg
ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
ToMk3Mag.CalState = compassCalState;
*/
// Waste 8 bytes to make CRC match.
base64InputStream.readBytes(8);
result = f;
break;
}
case 'Q':
UniversalReadParamSetResponseFrame f = new UniversalReadParamSetResponseFrame(address);
f.setConfigurationSetNumber(base64InputStream.readByte());
f.setConfigurationVersion(base64InputStream.readByte());
int length = base64InputStream.readByte();
f.setData(base64InputStream.readBytes(length));
result = f;
break;
default:
int b;
int count = 0;
while((b=read()) != '\r') {
count++;
}
System.err.println("Unknown frame " + (char)iid + " received from " + address);
System.err.println("It appears to have " + (count-2) + " data bytes (encoded)");
System.err.println("(" + (count-2) * 6/8 + " data bytes decoded)");
result = null;
}
int receivedCRC = (read() - '=') << 6;
receivedCRC += (read() - '=');
crc %= 4096;
if (receivedCRC != crc) {
/// System.err.println("Expected CRC: " + crc + ", got CRC: " + receivedCRC);
throw new IOException("CRC mismatch! Calculated crc: " + (int)crc + "; received check crc: " + receivedCRC + ", difference: " + Math.abs(crc - receivedCRC));
}
if (read() != '\r') {
throw new IOException("CR at end of frame missing");
}
 
return result;
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKOutputStream.java
0,0 → 1,218
package dongfang.mkt.comm;
 
import java.io.IOException;
import java.io.OutputStream;
 
import dongfang.mkt.RequestFrameVisitor;
import dongfang.mkt.frames.AllDisplaysRequestFrame;
import dongfang.mkt.frames.AnalogDebugLabelRequestFrame;
import dongfang.mkt.frames.AttitudeDataRequestFrame;
import dongfang.mkt.frames.ChangeParameterSetRequestFrame;
import dongfang.mkt.frames.CompassHeadingRequestFrame;
import dongfang.mkt.frames.DebugRequestFrame;
import dongfang.mkt.frames.ExternalControlRequestFrame;
import dongfang.mkt.frames.LoopbackTestRequestFrame;
import dongfang.mkt.frames.MotorTestRequestFrame;
import dongfang.mkt.frames.OSDDataRequestFrame;
import dongfang.mkt.frames.ReadExternalControlRequestFrame;
import dongfang.mkt.frames.RequestFrame;
import dongfang.mkt.frames.ResetRequestFrame;
import dongfang.mkt.frames.SetCompassHeadingRequestFrame;
import dongfang.mkt.frames.SingleDisplayRequestFrame;
import dongfang.mkt.frames.UniversalReadParamSetRequestFrame;
import dongfang.mkt.frames.UniversalWriteParamSetRequestFrame;
import dongfang.mkt.frames.VariablesRequestFrame;
import dongfang.mkt.frames.VersionRequestFrame;
 
public class MKOutputStream extends OutputStream implements RequestFrameVisitor {
public class MKDataOutputStream {
int[] inbuf = new int[3];
int[] outbuf = new int[4];
int inbufptr = 0;
void writeByte(int b) throws IOException {
if(inbufptr == inbuf.length) flush();
inbuf[inbufptr++] = b;
}
 
public void writeBytes(int[] bs) throws IOException {
for (int i=0; i<bs.length; i++)
writeByte(bs[i]);
}
 
void writeWord(int w) throws IOException {
writeByte(w & 0xff);
writeByte(w >>> 8);
}
 
void writeChars(char[] s) throws IOException {
for (int i=0; i<s.length; i++) {
// Here, a 1:1 mapping between byte values and char codes is assumed.
// That means we're assuming ISO-8859-1 (= the first 256 code points
// of Unicode, which Java uses for chars)
writeByte(s[i]);
}
}
 
void flush() throws IOException {
if (inbufptr == 0)
return;
while(inbufptr < inbuf.length) {
// add padding .. well just clear it, for tidyness.
inbuf[inbufptr++] = 0;
}
 
MKOutputStream.this.writeByte((inbuf[0] >>> 2) + '=');
MKOutputStream.this.writeByte(( ((inbuf[0] & 0x03) << 4) | (inbuf[1] >>> 4) ) + '=');
MKOutputStream.this.writeByte(( ((inbuf[1] & 0x0f) << 2) | (inbuf[2] >>> 6)) + '=');
MKOutputStream.this.writeByte(((inbuf[2] & 0x3f) + '='));
 
inbufptr = 0;
}
}
 
final OutputStream os;
MKDataOutputStream base64OutputStream = new MKDataOutputStream();
int crc;
 
public MKOutputStream(OutputStream os) {
this.os = os;
}
@Override
public void write(int b) throws IOException {
os.write(b);
// System.out.println("Writing: " + b);
}
 
public void writeByte(int b) throws IOException {
crc += b;
write(b);
}
 
public void write(RequestFrame f) throws IOException {
write(crc = '#');
 
int address = f.getAddress() + 'a';
writeByte(address);
 
// Will cause one of the "visit" methods below
// to be called, depending on the type of f.
f.accept(this);
base64OutputStream.flush();
 
crc %= 4096;
write((crc >>> 6) + '=');
write((crc & 0x3f) + '=');
write('\r');
}
 
/*
public void visit(RequestFrame f) {
throw new RuntimeException("Unbound RequestFrame type: "
+ f.getClass().getSimpleName()
+ ". Don't know how to output.");
}
*/
 
public void visit(AnalogDebugLabelRequestFrame f) throws IOException {
writeByte('a');
base64OutputStream.writeByte(f.getChannel());
}
 
public void visit(AttitudeDataRequestFrame f) throws IOException {
writeByte('c');
base64OutputStream.writeByte(f.getAutoSendInterval());
}
 
public void visit(DebugRequestFrame f) throws IOException {
writeByte('d');
base64OutputStream.writeByte(f.getAutoSendInterval());
}
 
public void visit(ChangeParameterSetRequestFrame f) throws IOException {
writeByte('f');
base64OutputStream.writeByte(f.getParameterSetNumber());
}
 
public void visit(VersionRequestFrame f) throws IOException {
writeByte('v');
}
 
public void visit(ResetRequestFrame f) throws IOException {
writeByte('R');
}
 
public void visit(MotorTestRequestFrame f) throws IOException {
writeByte('t');
base64OutputStream.writeBytes(f.getMotorValues());
}
 
public void visit(SingleDisplayRequestFrame f) throws IOException {
writeByte('l');
base64OutputStream.writeByte(f.getMenuItemCode()); // In 0.74 there is not the all in one mode.
}
 
public void visit(AllDisplaysRequestFrame f) throws IOException {
writeByte('h');
base64OutputStream.writeByte(f.getPageOrder().getRemoteKeys());
// mdo.writeByte(f.getAutoSendInterval());
}
 
public void visit(VariablesRequestFrame f) throws IOException {
writeByte('x');
}
 
public void visit(ExternalControlRequestFrame f) throws IOException {
writeByte('b');
base64OutputStream.writeByte(f.getDigital()[0]);
base64OutputStream.writeByte(f.getDigital()[1]);
base64OutputStream.writeByte(f.getRemoteButtons());
base64OutputStream.writeByte(f.getPitch());
base64OutputStream.writeByte(f.getRoll());
base64OutputStream.writeByte(f.getYaw());
base64OutputStream.writeByte(f.getThrottle());
base64OutputStream.writeByte(f.getHeight());
base64OutputStream.writeByte(f.getCommand());
base64OutputStream.writeByte(f.getFrameNum());
base64OutputStream.writeByte(f.getArgument());
}
public void visit(ReadExternalControlRequestFrame f) throws IOException {
writeByte('g');
}
 
public void visit(LoopbackTestRequestFrame f) throws IOException {
writeByte('0');
base64OutputStream.writeByte(f.getByte());
base64OutputStream.writeWord(f.getWord());
base64OutputStream.writeChars(f.getChararray());
}
public void visit(UniversalReadParamSetRequestFrame f) throws IOException {
writeByte('q');
base64OutputStream.writeByte(f.getConfigurationSetNumber());
}
 
public void visit(UniversalWriteParamSetRequestFrame f) throws IOException {
writeByte('s');
base64OutputStream.writeByte(f.getConfigurationSetNumber());
base64OutputStream.writeByte(f.getConfigurationVersionNumber());
base64OutputStream.writeBytes(f.getData());
}
public void visit(SetCompassHeadingRequestFrame f) throws IOException {
writeByte('K');
}
 
public void visit(CompassHeadingRequestFrame f) throws IOException {
writeByte('w');
}
 
public void visit(OSDDataRequestFrame f) throws IOException {
writeByte('o');
base64OutputStream.writeByte(f.getAutoSendInterval());
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/serial/RXTXSerialPort.java
0,0 → 1,65
package dongfang.mkt.comm.serial;
 
import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.NoSuchPortException;
import gnu.io.SerialPort;
 
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
 
import dongfang.mkt.comm.MKConnection;
 
 
public class RXTXSerialPort implements MKConnection {
private SerialPort serialPort;
 
public void init(String id) throws IOException {
// CommPortIdentifier.getPortIdentifier("COM1");
// Just take the 1st available port.
CommPortIdentifier portIdentifier = null;
 
try {
if (id == null) {
portIdentifier = (CommPortIdentifier) CommPortIdentifier
.getPortIdentifiers().nextElement();
if (portIdentifier == null) {
throw new IOException(
"No serial port found (in search for any serial port).");
}
} else
portIdentifier = CommPortIdentifier.getPortIdentifier(id);
} catch (NoSuchPortException ex) {
throw new IOException(ex.getMessage());
}
 
System.out.println("Using " + portIdentifier.getName());
 
if (portIdentifier.isCurrentlyOwned()) {
System.out.println("Error: Port is currently in use");
return;
} else {
try {
CommPort commPort = portIdentifier.open(this.getClass()
.getName(), 2001);
if (commPort instanceof SerialPort) {
serialPort = (SerialPort) commPort;
serialPort.setSerialPortParams(57600,
SerialPort.DATABITS_8, SerialPort.STOPBITS_1,
SerialPort.PARITY_NONE);
}
} catch (Exception ex) {
throw new IOException(ex.getMessage());
}
}
}
 
public InputStream getInputStream() throws IOException {
return serialPort.getInputStream();
}
 
public OutputStream getOutputStream() throws IOException {
return serialPort.getOutputStream();
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/tcp/MKTCPConnection.java
0,0 → 1,31
package dongfang.mkt.comm.tcp;
 
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import java.net.Socket;
 
import dongfang.mkt.comm.MKConnection;
 
public class MKTCPConnection implements MKConnection {
private InputStream inputStream;
private OutputStream outputStream;
 
@Override
public void init(String id) throws IOException {
//URL url = new URL(id);
Socket socket = new Socket("localhost", Integer.parseInt(id));
inputStream = socket.getInputStream();
outputStream = socket.getOutputStream();
}
 
@Override
public InputStream getInputStream() throws IOException {
return inputStream;
}
 
@Override
public OutputStream getOutputStream() throws IOException {
return outputStream;
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/datatype/GPSBearingAndRange.java
0,0 → 1,32
package dongfang.mkt.datatype;
 
public class GPSBearingAndRange {
int distance; // in m/10. 16 bit unsigned.
int bearing; // in degrees. 16 bit signed.
public int getDistance() {
return distance;
}
public int getBearing() {
return bearing;
}
public void setDistance(int distance) {
this.distance = distance;
}
public void setBearing(int bearing) {
this.bearing = bearing;
}
public String toXML() {
String result = "distance=\"" + ((double)this.distance)/10;
result += "\" bearing=\"";
result += this.bearing;
result += "\"";
return result;
}
public String toString() {
String result = "" + ((double)this.distance)/10;
result += "m @";
result += this.bearing;
result += "deg";
return result;
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/datatype/GPSPosition.java
0,0 → 1,75
package dongfang.mkt.datatype;
 
public class GPSPosition {
int longitude; // in 1E-7 degrees. 32 bit signed.
int latitude; // in 1E-7 degrees. 32 bit signed.
long altitude; // in mm. 32 bit signed.
int status;
 
public int getLongitude() {
return longitude;
}
 
public int getLatitude() {
return latitude;
}
 
public long getAltitude() {
return altitude;
}
 
public int getStatus() {
return status;
}
 
public void setLongitude(int longitude) {
this.longitude = longitude;
}
 
public void setLatitude(int latitude) {
this.latitude = latitude;
}
 
public void setAltitude(long altitude) {
this.altitude = altitude;
}
 
public void setStatus(int status) {
this.status = status;
}
 
public String toXML() {
double latitude = Math.abs(this.latitude);
String result = "latitude=\"" + latitude / 1E7;
if (this.latitude < 0)
result += "S";
else
result += "N";
result += "\"";
double longitude = Math.abs(this.longitude);
result += " longitude=\"" + longitude / 1E7;
if (this.longitude < 0)
result += "W";
else
result += "E";
result += "\"";
return result;
}
 
public String toString() {
double latitude = Math.abs(this.latitude);
String result = "" + latitude / 1E7;
if (this.latitude < 0)
result += "S";
else
result += "N";
result += " ";
double longitude = Math.abs(this.longitude);
result += "" + longitude / 1E7;
if (this.longitude < 0)
result += "W";
else
result += "E";
return result;
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/OSDDataResponseFrame.java
1,98 → 1,16
package dongfang.mkt.frames;
 
import dongfang.mkt.datatype.GPSBearingAndRange;
import dongfang.mkt.datatype.GPSPosition;
 
public class OSDDataResponseFrame extends ResponseFrame {
 
public static class GPSPosition {
int longitude; // in 1E-7 degrees. 32 bit signed.
int latitude; // in 1E-7 degrees. 32 bit signed.
long altitude; // in mm. 32 bit signed.
int status;
public int getLongitude() {
return longitude;
}
public int getLatitude() {
return latitude;
}
public long getAltitude() {
return altitude;
}
public int getStatus() {
return status;
}
public void setLongitude(int longitude) {
this.longitude = longitude;
}
public void setLatitude(int latitude) {
this.latitude = latitude;
}
public void setAltitude(long altitude) {
this.altitude = altitude;
}
public void setStatus(int status) {
this.status = status;
}
public String toXML() {
double latitude = Math.abs(this.latitude);
String result = "latitude=\"" + latitude/1E7;
if (this.latitude < 0) result+= "S"; else result+="N";
result += "\"";
double longitude= Math.abs(this.longitude);
result += " longitude=\"" + longitude/1E7;
if (this.longitude < 0) result+= "W"; else result+="E";
result += "\"";
return result;
}
 
public String toString() {
double latitude = Math.abs(this.latitude);
String result = "" + latitude/1E7;
if (this.latitude < 0) result+= "S"; else result+="N";
result += " ";
double longitude= Math.abs(this.longitude);
result += "" + longitude/1E7;
if (this.longitude < 0) result+= "W"; else result+="E";
return result;
}
}
public static class GPSDistanceAndBearing {
int distance; // in m/10. 16 bit unsigned.
int bearing; // in degrees. 16 bit signed.
public int getDistance() {
return distance;
}
public int getBearing() {
return bearing;
}
public void setDistance(int distance) {
this.distance = distance;
}
public void setBearing(int bearing) {
this.bearing = bearing;
}
public String toXML() {
String result = "distance=\"" + ((double)this.distance)/10;
result += "\" bearing=\"";
result += this.bearing;
result += "\"";
return result;
}
public String toString() {
String result = "" + ((double)this.distance)/10;
result += "m @";
result += this.bearing;
result += "deg";
return result;
}
}
 
private int version;
private GPSPosition currentPosition;
private GPSPosition targetPosition;
private GPSDistanceAndBearing currentToTarget;
private GPSBearingAndRange currentToTarget;
private GPSPosition homePosition;
private GPSDistanceAndBearing currentToHome;
private GPSBearingAndRange currentToHome;
private int waypointIndex;
private int waypointCount;
140,13 → 58,13
public GPSPosition getTargetPosition() {
return targetPosition;
}
public GPSDistanceAndBearing getCurrentToTarget() {
public GPSBearingAndRange getCurrentToTarget() {
return currentToTarget;
}
public GPSPosition getHomePosition() {
return homePosition;
}
public GPSDistanceAndBearing getCurrentToHome() {
public GPSBearingAndRange getCurrentToHome() {
return currentToHome;
}
public int getWaypointIndex() {
230,13 → 148,13
public void setTargetPosition(GPSPosition targetPosition) {
this.targetPosition = targetPosition;
}
public void setCurrentToTarget(GPSDistanceAndBearing currentToTarget) {
public void setCurrentToTarget(GPSBearingAndRange currentToTarget) {
this.currentToTarget = currentToTarget;
}
public void setHomePosition(GPSPosition homePosition) {
this.homePosition = homePosition;
}
public void setCurrentToHome(GPSDistanceAndBearing currentToHome) {
public void setCurrentToHome(GPSBearingAndRange currentToHome) {
this.currentToHome = currentToHome;
}
public void setWaypointIndex(int waypointIndex) {
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/MKDebugLogger.java
13,14 → 13,15
import java.util.LinkedList;
import java.util.List;
 
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.AnalogDebugLabelRequestFrame;
import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;
import dongfang.mkt.frames.DebugRequestFrame;
import dongfang.mkt.frames.DebugResponseFrame;
import dongfang.mkt.frames.Frame;
import dongfang.mkt.io.MKCommPort;
import dongfang.mkt.io.RXTXSerialPort;
import dongfang.mkt.serial.FrameQueue;
 
public class MKDebugLogger {
private static final PrintStream STDERR = System.out;
201,8 → 202,8
public static void main(String[] args) throws IOException {
MKDebugLogger test = new MKDebugLogger();
 
MKCommPort 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/OSDLogger.java
12,13 → 12,13
import java.util.Date;
import java.util.List;
 
import dongfang.mkt.comm.FrameQueue;
import dongfang.mkt.comm.MKConnection;
import dongfang.mkt.comm.serial.RXTXSerialPort;
import dongfang.mkt.frames.OSDDataRequestFrame;
import dongfang.mkt.frames.OSDDataResponseFrame;
import dongfang.mkt.frames.OSDDataResponseFrame.GPSDistanceAndBearing;
import dongfang.mkt.frames.OSDDataResponseFrame.GPSPosition;
import dongfang.mkt.io.MKCommPort;
import dongfang.mkt.io.RXTXSerialPort;
import dongfang.mkt.serial.FrameQueue;
 
public class OSDLogger {
private static final PrintStream STDERR = System.out;
163,10 → 163,10
public static void main(String[] args) throws IOException {
OSDLogger test = new OSDLogger();
 
MKCommPort port = new RXTXSerialPort();
port.init(null);
MKConnection _port = new RXTXSerialPort();
_port.init(null);
 
FrameQueue q = new FrameQueue(port);
FrameQueue q = new FrameQueue(_port);
String encoding = "iso-8859-1";
 
OutputStream fout = new FileOutputStream("osd.xml");
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/UniversalConfigurator.java
18,19 → 18,20
import org.w3c.dom.NodeList;
import org.xml.sax.SAXException;
 
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.configuration.ConfigSet;
import dongfang.mkt.frames.UniversalReadParamSetRequestFrame;
import dongfang.mkt.frames.UniversalReadParamSetResponseFrame;
import dongfang.mkt.frames.UniversalWriteParamSetRequestFrame;
import dongfang.mkt.frames.UniversalWriteParamSetResponseFrame;
import dongfang.mkt.io.MKCommPort;
import dongfang.mkt.io.RXTXSerialPort;
import dongfang.mkt.serial.FrameQueue;
 
public class UniversalConfigurator {
private static void configure(String portIdentifier, UniversalWriteParamSetRequestFrame frame,
int parameterSetNumber) throws IOException {
MKCommPort port = new RXTXSerialPort();
MKConnection port = new RXTXSerialPort();
port.init(portIdentifier);
FrameQueue q = new FrameQueue(port);
 
62,7 → 63,7
}
 
private static ConfigSet readConfiguration(String portIdentifier, int parameterSetNumber) throws IOException {
MKCommPort port = new RXTXSerialPort();
MKConnection port = new MKTCPConnection(); //new RXTXSerialPort();//
port.init(portIdentifier);
FrameQueue q = new FrameQueue(port);
ConfigSet cs = null;
244,7 → 245,7
if ("w".equals(args[0]) && (args.length!=3 && args.length!=4)) help();
if ("r".equals(args[0]) && (args.length!=3 && args.length!=4)) help();
 
String portIdentifier = null;
String portIdentifier = "8080";
if ("r".equals(args[0])) {
readConfiguration(portIdentifier, Integer.parseInt(args[1]));
/dongfang_FC_rewrite_tool/src/dongfang/mkt/ui/offscreendisplay/MapImageView.java
0,0 → 1,88
package dongfang.mkt.ui.offscreendisplay;
 
/*
* u/l: geotagged geo:lat=47.327450 geo:lon=8.521657
* l/r: geotagged geo:lat=47.321749 geo:lon=8.534403
* house: geotagged geo:lat=47.324614 geo:lon=8.528202
*/
 
import java.awt.Color;
import java.awt.Graphics;
import java.awt.Image;
import java.awt.Toolkit;
 
import javax.swing.JFrame;
import javax.swing.JPanel;
 
import dongfang.mkt.datatype.GPSPosition;
 
public class MapImageView extends JPanel {
private Image mapImage;
private Image copterImage;
 
private static final int MAPWIDTH = 600;
private static final int MAPHEIGHT = 400;
 
// It is assumed the map is north/south oriented!
private GPSPosition upperLeft;
private GPSPosition lowerRight;
 
private GPSPosition hightlightPosition;
public MapImageView(GPSPosition upperLeft, GPSPosition lowerRight, String imageFileName) {
setSize(MAPWIDTH, MAPHEIGHT);
this.upperLeft = upperLeft;
this.lowerRight = lowerRight;
mapImage = Toolkit.getDefaultToolkit().getImage(imageFileName);
}
 
private double relativeX(GPSPosition pos) {
double lon = pos.getLongitude();
// The sign should be OK here (positive) both east and west.
double span = lowerRight.getLongitude() - upperLeft.getLongitude();
double offset = lon - upperLeft.getLongitude();
return offset / span;
}
 
private double relativeY(GPSPosition pos) {
double lat = pos.getLatitude();
// The sign should be OK here (positive) both east and west.
double span = lowerRight.getLatitude() - upperLeft.getLatitude();
double offset = lat - upperLeft.getLatitude();
return offset / span;
}
 
public void paintComponent(Graphics g) {
// Draw our Image object.
g.drawImage(mapImage, 0, 0, MAPWIDTH, MAPHEIGHT, this); // at location
g.setColor(Color.red);
int x = (int)(MAPWIDTH * relativeX(hightlightPosition) + 0.5);
int y = (int)(MAPHEIGHT * relativeY(hightlightPosition) + 0.5);
g.drawArc(x, y, 20, 20, 0, 360);
}
public static void main(String[] args) {
GPSPosition upperLeft = new GPSPosition();
GPSPosition lowerRight = new GPSPosition();
GPSPosition highlight = new GPSPosition();
upperLeft.setLatitude((int)(47.327450 * 1E7));
upperLeft.setLongitude((int)(8.521657 * 1E7));
 
// * l/r: geotagged geo:lat=47.321749 geo:lon=8.534403
lowerRight.setLatitude((int)(47.321749 * 1E7));
lowerRight.setLongitude((int)(8.534403 * 1E7));
// geotagged geo:lat=47.324614 geo:lon=8.528202
highlight.setLatitude((int)(47.324714 * 1E7));
highlight.setLongitude((int)(8.528102 * 1E7));
MapImageView v = new MapImageView(upperLeft, lowerRight, "flugplatz_small.png");
v.hightlightPosition = highlight;
JFrame f = new JFrame();
f.getContentPane().add(v);
f.setSize(v.getSize());
f.setVisible(true);
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/ui/offscreendisplay/OffScreenDisplay.java
0,0 → 1,9
package dongfang.mkt.ui.offscreendisplay;
 
import javax.swing.JFrame;
 
// A display of OSD data that is not OSD; that is, it is just an UI view without an underlying video display.
public class OffScreenDisplay extends JFrame {
// The compass direction of the "up" direction of the display screen.
private int displayHeading;
}