Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1562 → Rev 1563

/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKInputStream.java
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;
}
}