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