/dongfang_FC_rewrite_tool/flugplatz.jpg |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
Property changes: |
Added: svn:mime-type |
+application/octet-stream |
\ No newline at end of property |
/dongfang_FC_rewrite_tool/flugplatz_small.png |
---|
Cannot display: file marked as a binary type. |
svn:mime-type = application/octet-stream |
Property changes: |
Added: svn:mime-type |
+application/octet-stream |
\ No newline at end of property |
/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; |
} |