Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1538 → Rev 1539

/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');
}
}