package dongfang.mkt.comm;
import java.io.IOException;
import java.io.InputStream;
import java.io.OutputStream;
import dongfang.mkt.datatype.GPSBearingAndRange;
import dongfang.mkt.datatype.GPSPosition;
import dongfang.mkt.frames.AllDisplaysResponseFrame;
import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;
import dongfang.mkt.frames.AttitudeDataResponseFrame;
import dongfang.mkt.frames.CompassHeadingResponseFrame;
import dongfang.mkt.frames.ConfirmFrame;
import dongfang.mkt.frames.DCMMatrixResponseFrame;
import dongfang.mkt.frames.DebugResponseFrame;
import dongfang.mkt.frames.MotorTestResponseFrame;
import dongfang.mkt.frames.OSDDataResponseFrame;
import dongfang.mkt.frames.ProfilerLabelResponseFrame;
import dongfang.mkt.frames.ProfilerResponseFrame;
import dongfang.mkt.frames.ReadExternalControlResponseFrame;
import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame;
import dongfang.mkt.frames.ReadMotorMixerResponseFrame;
import dongfang.mkt.frames.ReadParamSetResponseFrame;
import dongfang.mkt.frames.ReadRCChannelsResponseFrame;
import dongfang.mkt.frames.ReadVariablesResponseFrame;
import dongfang.mkt.frames.ResponseFrame;
import dongfang.mkt.frames.SetCompassHeadingResponseFrame;
import dongfang.mkt.frames.VersionResponseFrame;
import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame;
import dongfang.mkt.frames.WriteMotorMixerResponseFrame;
import dongfang.mkt.frames.WriteParamSetResponseFrame;
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 readSignedByte
() throws IOException {
byte result =
(byte)readByte
();
return result
;
}
public int readWord
() throws IOException {
int byte0 = readByte
();
int byte1 = readByte
();
return (byte1
<< 8) | byte0
;
}
public int readSignedWord
() throws IOException {
int word = readWord
();
if (word
> 32767)
word = word -
65536;
return word
;
}
public int readSignedDWord
() throws IOException {
int byte0 = readByte
();
int byte1 = readByte
();
int byte2 = readByte
();
int byte3 = readByte
();
return (byte3
<< 24) |
(byte2
<< 16) |
(byte1
<< 8) | byte0
;
}
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 int[] readSignedWords
(int length
) throws IOException {
int[] result =
new int[length
];
for (int i =
0; i
< length
; i++
) {
result
[i
] = readSignedWord
();
}
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
;
}
public float readFloat
() throws IOException {
int asInt = readByte
() |
(readByte
()<<8) |
(readByte
()<<16) |
(readByte
()<<24);
return Float.
intBitsToFloat(asInt
);
}
public float[] readFloats
(int length
) throws IOException {
float[] result =
new float[length
];
for (int i =
0; i
< length
; i++
) {
result
[i
] = readFloat
();
}
return result
;
}
}
MKDataInputStream base64InputStream =
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 base64InputStream
;
}
public ResponseFrame getNextFrame
() throws IOException {
int c
;
while ((c = read
()) !=
'#') {
// throw it on some scrap-text buffer.
if (nonPacketSpillway
!=
null)
nonPacketSpillway.
write(c
);
}
crc =
'#';
base64InputStream.
reset();
int address = readByte
() -
'a';
int iid = readByte
();
readByteCnt =
0;
//RESPONSE_IDS id = getResponseType(iid);
ResponseFrame result
;
// System.out.println("Received a: " + (char)iid + " from " + address);
switch (iid
) {
case 'A':
{
AnalogDebugLabelResponseFrame f =
new AnalogDebugLabelResponseFrame
(address
);
f.
setChannel(base64InputStream.
readByte());
f.
setLabel(base64InputStream.
readChars(16));
result = f
;
break;
}
case 'B':
{
ConfirmFrame f =
new ConfirmFrame
(address
);
f.
setFrameNum(base64InputStream.
readByte());
result = f
;
break;
}
case 'C':
{
AttitudeDataResponseFrame f =
new AttitudeDataResponseFrame
(address
);
f.
setAttitude(base64InputStream.
readFloats(3));
f.
setRates(base64InputStream.
readFloats(3));
f.
setAcc(base64InputStream.
readFloats(3));
result = f
;
break;
}
case 'D':
{
DebugResponseFrame f =
new DebugResponseFrame
(address
);
for (int i=
0; i
<2; i++
)
f.
setDigital(i, base64InputStream.
readByte());
for (int i=
0; i
<32; i++
)
f.
setAnalog(i, base64InputStream.
readSignedWord());
result = f
;
break;
}
case 'E':
{
DCMMatrixResponseFrame f=
new DCMMatrixResponseFrame
(address
);
float[][] matrix =
new float[3][];
for (int i=
0; i
<3; i++
) {
float[] row = base64InputStream.
readFloats(3);
matrix
[i
] = row
;
}
f.
setMatrix(matrix
);
result = f
;
break;
}
/*
case 'F': {
ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);
f.setParameterSetNumber(base64InputStream.readByte());
result = f;
break;
}
*/
case 'F':
{
ProfilerLabelResponseFrame f =
new ProfilerLabelResponseFrame
(address
);
f.
setChannel(base64InputStream.
readByte());
f.
setLabel(base64InputStream.
readChars(16));
result = f
;
break;
}
case 'G':
{
ReadExternalControlResponseFrame f =
new ReadExternalControlResponseFrame
(address
);
f.
setDigital(base64InputStream.
readBytes(2));
f.
setRemoteButtons(base64InputStream.
readByte());
f.
setPitch(base64InputStream.
readByte());
f.
setRoll(base64InputStream.
readByte());
f.
setYaw(base64InputStream.
readByte());
f.
setThrottle(base64InputStream.
readByte());
f.
setHeight(base64InputStream.
readByte());
f.
setCommand(base64InputStream.
readByte());
f.
setFrameNum(base64InputStream.
readByte());
f.
setArgument(base64InputStream.
readByte());
result = f
;
break;
}
case 'H':
{
AllDisplaysResponseFrame f =
new AllDisplaysResponseFrame
(address
);
f.
setLine(base64InputStream.
readByte());
//f.setMaxItem(getDataInputStream().readByte());
f.
setText(base64InputStream.
readChars(20));
result = f
;
break;
}
case 'I':
{
ReadIMUConfigurationResponseFrame f =
new ReadIMUConfigurationResponseFrame
(address
);
f.
setConfigurationVersion(base64InputStream.
readByte());
int length = base64InputStream.
readByte();
f.
setConfigurationSetLength(length
);
f.
setData(base64InputStream.
readBytes(length
));
result = f
;
break;
}
case 'J':
{
WriteIMUConfigurationResponseFrame f =
new WriteIMUConfigurationResponseFrame
(address
);
f.
setWasAccepted(base64InputStream.
readByte()==
1);
result = f
;
break;
}
case 'k' :
{
CompassHeadingResponseFrame f =
new CompassHeadingResponseFrame
(address
);
base64InputStream.
readSignedWords(2);
base64InputStream.
readBytes(2);
base64InputStream.
readByte();
base64InputStream.
readByte();
result = f
;
break;
}
case 'L':
{
AllDisplaysResponseFrame f =
new AllDisplaysResponseFrame
(address
);
f.
setItem(base64InputStream.
readByte());
// f.setMaxItem(getDataInputStream().readByte());
f.
setText(base64InputStream.
readChars(80));
result = f
;
break;
}
case 'M':
{
WriteMotorMixerResponseFrame f =
new WriteMotorMixerResponseFrame
(address
);
f.
setWasAccepted(base64InputStream.
readByte() !=
0);
result = f
;
break;
}
case 'N':
{
int numMotors =
12;
ReadMotorMixerResponseFrame f =
new ReadMotorMixerResponseFrame
();
f.
setConfigurationVersion(base64InputStream.
readByte());
int length = base64InputStream.
readByte();
f.
setDataLength(length
);
int testLength =
12; // the name is first!
char[] name = base64InputStream.
readChars(12);
f.
setName(name
);
int[][] matrix =
new int[numMotors
][];
for(int i=
0; i
<numMotors
; i++
) {
int[] row =
new int[5];
matrix
[i
] = row
;
for(int j=
0; j
<5; j++
) {
row
[j
] = base64InputStream.
readSignedByte();
testLength++
;
}
}
if (length
!= testLength
)
throw new IOException("Length of motor mixer data was not as expected (" + testLength +
" vs. " + length +
").");
/*
int[] opposite = new int[numMotors];
for(int i=0; i<numMotors; i++) {
opposite[i] = base64InputStream.readByte();
}
*/
f.
setMatrix(matrix
);
result = f
;
break;
}
case 'O':
{
OSDDataResponseFrame f =
new OSDDataResponseFrame
(address
);
f.
setVersion(base64InputStream.
readByte());
GPSPosition pos =
new GPSPosition
();
pos.
setLongitude(((double)base64InputStream.
readSignedDWord())/1E7
);
pos.
setLatitude(((double)base64InputStream.
readSignedDWord())/1E7
);
pos.
setAltitude(((double)base64InputStream.
readSignedDWord())/1E3
);
pos.
setStatus(base64InputStream.
readByte());
f.
setCurrentPosition(pos
);
pos =
new GPSPosition
();
pos.
setLongitude(((double)base64InputStream.
readSignedDWord())/1E7
);
pos.
setLatitude(((double)base64InputStream.
readSignedDWord())/1E7
);
pos.
setAltitude(((double)base64InputStream.
readSignedDWord())/1E3
);
pos.
setStatus(base64InputStream.
readByte());
f.
setTargetPosition(pos
);
GPSBearingAndRange rnb =
new GPSBearingAndRange
();
rnb.
setDistance(((double)base64InputStream.
readWord())/
10.0);
rnb.
setBearing(base64InputStream.
readSignedWord());
f.
setCurrentToTarget(rnb
);
pos =
new GPSPosition
();
pos.
setLongitude(((double)base64InputStream.
readSignedDWord())/1E7
);
pos.
setLatitude(((double)base64InputStream.
readSignedDWord())/1E7
);
pos.
setAltitude(((double)base64InputStream.
readSignedDWord())/1E3
);
pos.
setStatus(base64InputStream.
readByte());
f.
setHomePosition(pos
);
rnb =
new GPSBearingAndRange
();
rnb.
setDistance(((double)base64InputStream.
readWord())/
10.0);
rnb.
setBearing(base64InputStream.
readSignedWord());
f.
setCurrentToHome(rnb
);
f.
setWaypointIndex(base64InputStream.
readByte());
f.
setWaypointCount(base64InputStream.
readByte());
f.
setNumberOfSatellites(base64InputStream.
readByte());
// This stunt is a metric unit conversion: The height was supposed (H&I) to be in integral 5cm steps.
// However there is error factor in the measurement of 24% too much.
// h[m] = h[int] * 0.05 / 1.24 = h[int]
f._setHeightByPressure
(((double)base64InputStream.
readSignedWord()) * 0.05 /
1.24);
f.
setVerticalVelocityByPressure(((double)base64InputStream.
readSignedWord()) * 0.05 /
1.24); // clueless!
f.
setFlightTime(base64InputStream.
readWord());
f.
setBatteryVoltage(base64InputStream.
readByte());
f.
setGroundSpeed(((double)base64InputStream.
readWord()) / 1E2
);
f.
setDirectionOfFlight(base64InputStream.
readSignedWord());
f.
setCompassHeading(base64InputStream.
readSignedWord());
f.
setPitchAngle(base64InputStream.
readSignedByte());
f.
setRollAngle(base64InputStream.
readSignedByte());
f.
setRcQuality(base64InputStream.
readByte());
f.
setFcFlags(base64InputStream.
readByte());
f.
setNcFlags(base64InputStream.
readByte());
f.
setErrorCode(base64InputStream.
readByte());
f.
setOperatingRadius(base64InputStream.
readByte());
f.
setVerticalVelocityByGPS(((double)base64InputStream.
readSignedWord()) / 1E2
);
f.
setTargetLoiterTime(base64InputStream.
readByte());
f.
setFcFlags2(base64InputStream.
readByte());
f.
setSetpointForAltitude(((double)base64InputStream.
readSignedWord()) * 0.05 /
1.24);
f.
setThrottle(base64InputStream.
readByte());
f.
setCurrent(base64InputStream.
readWord());
f.
setCapacityUsed(base64InputStream.
readWord());
result = f
;
break;
}
case 'P':
{
ReadRCChannelsResponseFrame f =
new ReadRCChannelsResponseFrame
(address
);
int numberOfChannels = base64InputStream.
readByte();
f.
setNumberOfChannels(numberOfChannels
);
f.
setChannels(base64InputStream.
readSignedWords(numberOfChannels
));
result = f
;
break;
}
case 'S':
{
WriteParamSetResponseFrame f =
new WriteParamSetResponseFrame
(address
);
f.
setParameterSetNumber(base64InputStream.
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 'U':
{
ProfilerResponseFrame f =
new ProfilerResponseFrame
(address
);
f.
setTotalHits(base64InputStream.
readSignedDWord());
for (int i=
0; i
<16; i++
)
f.
setActivity(i, base64InputStream.
readWord());
result = f
;
break;
}
case 'V':
{
VersionResponseFrame f =
new VersionResponseFrame
(address
);
f.
setSWMajor(base64InputStream.
readByte());
f.
setSWMinor(base64InputStream.
readByte());
f.
setSWPatch(base64InputStream.
readByte());
f.
setProtoMajor(base64InputStream.
readByte());
f.
setProtoMinor(base64InputStream.
readByte());
f.
setHardwareErrors(base64InputStream.
readBytes(5));
result = f
;
break;
}
// This is my own creation. The ID collides with the waypoint one of FC.
case 'X':
{
ReadVariablesResponseFrame f =
new ReadVariablesResponseFrame
(address
);
f.
setVariables(base64InputStream.
readSignedWords(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.
base64InputStream.
readBytes(8);
result = f
;
break;
}
case 'Q':
ReadParamSetResponseFrame f =
new ReadParamSetResponseFrame
(address
);
f.
setConfigurationSetNumber(base64InputStream.
readByte());
f.
setConfigurationVersion(base64InputStream.
readByte());
int length = base64InputStream.
readByte();
f.
setConfigurationSetLength(length
);
f.
setData(base64InputStream.
readBytes(length
));
result = f
;
break;
default:
int count =
0;
while(read
() !=
'\r') {
count++
;
}
System.
err.
println("Unknown frame " +
(char)iid +
" received from " + address
);
System.
err.
println("It appears to have " +
(count-
2) +
" data bytes (encoded)");
System.
err.
println("(" +
(count-
2) * 6/
8 +
" data bytes decoded)");
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
;
}
}