0,0 → 1,401 |
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.ChangeParameterSetResponseFrame; |
import dongfang.mkt.frames.CompassHeadingResponseFrame; |
import dongfang.mkt.frames.ConfirmFrame; |
import dongfang.mkt.frames.DebugResponseFrame; |
import dongfang.mkt.frames.MotorTestResponseFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
import dongfang.mkt.frames.ReadExternalControlResponseFrame; |
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; |
|
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; |
} |
} |
|
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.setPitch(base64InputStream.readSignedWord()); |
f.setRoll(base64InputStream.readSignedWord()); |
f.setHeading(base64InputStream.readSignedWord()); |
f.setExpansion(base64InputStream.readBytes(8)); |
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 'F': { |
ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
f.setParameterSetNumber(base64InputStream.readByte()); |
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 '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 'O': { |
OSDDataResponseFrame f = new OSDDataResponseFrame(address); |
f.setVersion(base64InputStream.readByte()); |
|
GPSPosition pos = new GPSPosition(); |
pos.setLongitude(base64InputStream.readSignedDWord()); |
pos.setLatitude(base64InputStream.readSignedDWord()); |
pos.setAltitude(base64InputStream.readSignedDWord()); |
pos.setStatus(base64InputStream.readByte()); |
f.setCurrentPosition(pos); |
|
pos = new GPSPosition(); |
pos.setLongitude(base64InputStream.readSignedDWord()); |
pos.setLatitude(base64InputStream.readSignedDWord()); |
pos.setAltitude(base64InputStream.readSignedDWord()); |
pos.setStatus(base64InputStream.readByte()); |
f.setTargetPosition(pos); |
|
GPSBearingAndRange rnb = new GPSBearingAndRange(); |
rnb.setDistance(base64InputStream.readWord()); |
rnb.setBearing(base64InputStream.readSignedWord()); |
f.setCurrentToTarget(rnb); |
|
pos = new GPSPosition(); |
pos.setLongitude(base64InputStream.readSignedDWord()); |
pos.setLatitude(base64InputStream.readSignedDWord()); |
pos.setAltitude(base64InputStream.readSignedDWord()); |
pos.setStatus(base64InputStream.readByte()); |
f.setHomePosition(pos); |
|
rnb = new GPSBearingAndRange(); |
rnb.setDistance(base64InputStream.readWord()); |
rnb.setBearing(base64InputStream.readSignedWord()); |
f.setCurrentToHome(rnb); |
|
f.setWaypointIndex(base64InputStream.readByte()); |
f.setWaypointCount(base64InputStream.readByte()); |
f.setNumberOfSatellites(base64InputStream.readByte()); |
|
f.setHeightByPressure(base64InputStream.readSignedWord()); |
f.setVerticalVelocityByPressure(base64InputStream.readSignedWord()); |
f.setFlightTime(base64InputStream.readWord()); |
f.setBatteryVoltage(base64InputStream.readByte()); |
f.setGroundSpeed(base64InputStream.readWord()); |
|
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(base64InputStream.readSignedWord()); |
f.setTargetLoiterTime(base64InputStream.readByte()); |
f.setFcFlags2(base64InputStream.readByte()); |
f.setSetpointForAltitude(base64InputStream.readSignedWord()); |
f.setThrottle(base64InputStream.readByte()); |
f.setCurrent(base64InputStream.readWord()); |
f.setCapacityUsed(base64InputStream.readWord()); |
result = f; |
break; |
} |
case 'S': { |
UniversalWriteParamSetResponseFrame f = new UniversalWriteParamSetResponseFrame(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 'V': { |
VersionResponseFrame f = new VersionResponseFrame(address); |
f.setSWMajor(base64InputStream.readByte()); |
f.setSWMinor(base64InputStream.readByte()); |
f.setProtoMajor(base64InputStream.readByte()); |
f.setProtoMinor(base64InputStream.readByte()); |
f.setSWPatch(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': { |
VariablesResponseFrame f = new VariablesResponseFrame(address); |
f.setVariables(base64InputStream.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. |
base64InputStream.readBytes(8); |
result = f; |
break; |
} |
case 'Q': |
UniversalReadParamSetResponseFrame f = new UniversalReadParamSetResponseFrame(address); |
f.setConfigurationSetNumber(base64InputStream.readByte()); |
f.setConfigurationVersion(base64InputStream.readByte()); |
int length = base64InputStream.readByte(); |
f.setData(base64InputStream.readBytes(length)); |
result = f; |
break; |
default: |
int b; |
int count = 0; |
while((b=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; |
} |
} |