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