/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/FrameFactory.java |
---|
0,0 → 1,21 |
package dongfang.mkt.frames; |
import dongfang.mkt.datatypes.ParamSet; |
public interface FrameFactory { |
AllDisplaysRequestFrame createAllDisplaysRequestFrame(int address); |
AnalogDebugLabelRequestFrame createAnalogDebugLabelRequestFrame(int address, int channel); |
AttitudeDataRequestFrame createAttitudeDataRequestFrame(int address); |
ChangeParameterSetRequestFrame createChangeParameterSetRequestFrame(int parameterSetNumber); |
DebugRequestFrame createDebugRequestFrame(int address); |
// DisplayRequestFrame createDisplayRequestFrame(int address); |
ExternalControlRequestFrame createExternalControlRequestFrame(int address); |
LoopbackTestRequestFrame createLoopbackTestRequestFrame(int address); |
MotorTestRequestFrame createMotorTestRequestFrame(int address); |
ReadParamSetRequestFrame createReadParamSetRequestFrame(int address); |
ResetRequestFrame createResetRequestFrame(int address); |
SingleDisplayRequestFrame createSingleDisplayRequestFrame(int address); |
VariablesRequestFrame createVariablesRequestFrame(int address); |
VersionRequestFrame createVersionRequestFrame(int address); |
WriteParamSetRequestFrame createWriteParamSetRequestFrame(ParamSet paramSet); |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/serial/FrameQueue.java |
---|
0,0 → 1,88 |
package dongfang.mkt.serial; |
import java.io.IOException; |
import java.io.InputStream; |
import java.io.OutputStream; |
import dongfang.mkt.frames.RequestFrame; |
import dongfang.mkt.frames.ResponseFrame; |
import dongfang.mkt.io.MKCommPort; |
/** |
* 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(MKCommPort 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/serial/MKInputStream.java |
---|
0,0 → 1,276 |
package dongfang.mkt.serial; |
import java.io.IOException; |
import java.io.InputStream; |
import java.io.OutputStream; |
import dongfang.mkt.frames.AllDisplaysResponseFrame; |
import dongfang.mkt.frames.AnalogDebugLabelResponseFrame; |
import dongfang.mkt.frames.AttitudeDataResponseFrame; |
import dongfang.mkt.frames.ChangeParameterSetResponseFrame; |
import dongfang.mkt.frames.ConfirmFrame; |
import dongfang.mkt.frames.DebugResponseFrame; |
import dongfang.mkt.frames.MotorTestResponseFrame; |
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; |
import dongfang.mkt.main.UniversalConfigurator; |
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 readWord() throws IOException { |
int byte2 = readByte(); |
int byte1 = readByte(); |
return (byte1 << 8) | byte2; |
} |
public int readSignedWord() throws IOException { |
int word = readWord(); |
if (word > 32767) |
word = word - 65536; |
return word; |
} |
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 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 dis = 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 dis; |
} |
public ResponseFrame getNextFrame() throws IOException { |
int c; |
while ((c = read()) != '#') { |
// throw it on some scrap-text buffer. |
if (nonPacketSpillway != null) |
nonPacketSpillway.write(c); |
} |
crc = '#'; |
dis.reset(); |
int address = readByte() - 'a'; |
int iid = readByte(); |
readByteCnt = 0; |
//RESPONSE_IDS id = getResponseType(iid); |
ResponseFrame result; |
switch (iid) { |
case 'A': { |
AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address); |
f.setChannel(getBase64InputStream().readByte()); |
f.setLabel(getBase64InputStream().readChars(16)); |
result = f; |
break; |
} |
case 'B': { |
ConfirmFrame f = new ConfirmFrame(address); |
f.setFrameNum(getBase64InputStream().readByte()); |
result = f; |
break; |
} |
case 'C': { |
AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
f.setPitch(getBase64InputStream().readSignedWord()); |
f.setRoll(getBase64InputStream().readSignedWord()); |
f.setHeading(getBase64InputStream().readSignedWord()); |
f.setExpansion(getBase64InputStream().readBytes(8)); |
result = f; |
break; |
} |
case 'D': { |
DebugResponseFrame f = new DebugResponseFrame(address); |
for (int i=0; i<2; i++) |
f.setDigital(i, getBase64InputStream().readByte()); |
for (int i=0; i<32; i++) |
f.setAnalog(i, getBase64InputStream().readSignedWord()); |
result = f; |
break; |
} |
case 'F': { |
ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
f.setParameterSetNumber(getBase64InputStream().readByte()); |
result = f; |
break; |
} |
case 'H': { |
AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address); |
f.setLine(getBase64InputStream().readByte()); |
//f.setMaxItem(getDataInputStream().readByte()); |
f.setText(getBase64InputStream().readChars(20)); |
result = f; |
break; |
} |
case 'L': { |
AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address); |
f.setItem(getBase64InputStream().readByte()); |
// f.setMaxItem(getDataInputStream().readByte()); |
f.setText(getBase64InputStream().readChars(80)); |
result = f; |
break; |
} |
case 'S': { |
UniversalWriteParamSetResponseFrame f = new UniversalWriteParamSetResponseFrame(address); |
f.setParameterSetNumber(getBase64InputStream().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(getBase64InputStream().readByte()); |
f.setSWMinor(getBase64InputStream().readByte()); |
f.setProtoMajor(getBase64InputStream().readByte()); |
f.setProtoMinor(getBase64InputStream().readByte()); |
f.setSWPatch(getBase64InputStream().readByte()); |
f.setHardwareErrors(getBase64InputStream().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(getBase64InputStream().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. |
getBase64InputStream().readBytes(8); |
result = f; |
break; |
} |
case 'Q': |
UniversalReadParamSetResponseFrame f = new UniversalReadParamSetResponseFrame(address); |
f.setConfigurationSetNumber(getBase64InputStream().readByte()); |
f.setConfigurationVersion(getBase64InputStream().readByte()); |
int length = getBase64InputStream().readByte(); |
f.setData(getBase64InputStream().readBytes(length)); |
result = f; |
break; |
default: |
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/serial/MKOutputStream.java |
---|
0,0 → 1,202 |
package dongfang.mkt.serial; |
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.DebugRequestFrame; |
import dongfang.mkt.frames.ExternalControlRequestFrame; |
import dongfang.mkt.frames.LoopbackTestRequestFrame; |
import dongfang.mkt.frames.MotorTestRequestFrame; |
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('y'); |
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(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'); |
} |
} |