Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1538 → Rev 1539

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