Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1558 → Rev 1559

/dongfang_FC_rewrite_tool/src/dongfang/mkt/serial/MKInputStream.java
11,6 → 11,10
import dongfang.mkt.frames.ConfirmFrame;
import dongfang.mkt.frames.DebugResponseFrame;
import dongfang.mkt.frames.MotorTestResponseFrame;
import dongfang.mkt.frames.OSDDataResponseFrame;
import dongfang.mkt.frames.OSDDataResponseFrame.GPSDistanceAndBearing;
import dongfang.mkt.frames.OSDDataResponseFrame.GPSPosition;
import dongfang.mkt.frames.ReadExternalControlResponseFrame;
import dongfang.mkt.frames.ResponseFrame;
import dongfang.mkt.frames.SetCompassHeadingResponseFrame;
import dongfang.mkt.frames.UniversalReadParamSetResponseFrame;
17,7 → 21,6
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;
53,10 → 56,15
return outbuf[outbufptr++];
}
 
public int readSignedByte() throws IOException {
byte result = (byte)readByte();
return result;
}
 
public int readWord() throws IOException {
int byte2 = readByte();
int byte0 = readByte();
int byte1 = readByte();
return (byte1 << 8) | byte2;
return (byte1 << 8) | byte0;
}
 
public int readSignedWord() throws IOException {
65,6 → 73,14
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];
94,7 → 110,7
}
}
 
MKDataInputStream dis = new MKDataInputStream();
MKDataInputStream base64InputStream = new MKDataInputStream();
OutputStream nonPacketSpillway = null; //System.err;
final InputStream is;
121,7 → 137,7
}
 
public MKDataInputStream getBase64InputStream() {
return dis;
return base64InputStream;
}
 
public ResponseFrame getNextFrame() throws IOException {
132,7 → 148,7
nonPacketSpillway.write(c);
}
crc = '#';
dis.reset();
base64InputStream.reset();
int address = readByte() - 'a';
int iid = readByte();
readByteCnt = 0;
141,23 → 157,23
switch (iid) {
case 'A': {
AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address);
f.setChannel(getBase64InputStream().readByte());
f.setLabel(getBase64InputStream().readChars(16));
f.setChannel(base64InputStream.readByte());
f.setLabel(base64InputStream.readChars(16));
result = f;
break;
}
case 'B': {
ConfirmFrame f = new ConfirmFrame(address);
f.setFrameNum(getBase64InputStream().readByte());
f.setFrameNum(base64InputStream.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));
f.setPitch(base64InputStream.readSignedWord());
f.setRoll(base64InputStream.readSignedWord());
f.setHeading(base64InputStream.readSignedWord());
f.setExpansion(base64InputStream.readBytes(8));
result = f;
break;
}
164,37 → 180,119
case 'D': {
DebugResponseFrame f = new DebugResponseFrame(address);
for (int i=0; i<2; i++)
f.setDigital(i, getBase64InputStream().readByte());
f.setDigital(i, base64InputStream.readByte());
for (int i=0; i<32; i++)
f.setAnalog(i, getBase64InputStream().readSignedWord());
f.setAnalog(i, base64InputStream.readSignedWord());
result = f;
break;
}
case 'F': {
ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);
f.setParameterSetNumber(getBase64InputStream().readByte());
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(getBase64InputStream().readByte());
f.setLine(base64InputStream.readByte());
//f.setMaxItem(getDataInputStream().readByte());
f.setText(getBase64InputStream().readChars(20));
f.setText(base64InputStream.readChars(20));
result = f;
break;
}
case 'L': {
AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
f.setItem(getBase64InputStream().readByte());
f.setItem(base64InputStream.readByte());
// f.setMaxItem(getDataInputStream().readByte());
f.setText(getBase64InputStream().readChars(80));
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);
GPSDistanceAndBearing rnb = new GPSDistanceAndBearing();
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 GPSDistanceAndBearing();
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.setHeading(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(getBase64InputStream().readByte());
f.setParameterSetNumber(base64InputStream.readByte());
result = f;
break;
}
216,12 → 314,12
*/
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));
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;
}
229,7 → 327,7
// 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));
f.setVariables(base64InputStream.readWords(8));
result = f;
break;
}
244,16 → 342,16
ToMk3Mag.CalState = compassCalState;
*/
// Waste 8 bytes to make CRC match.
getBase64InputStream().readBytes(8);
base64InputStream.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));
f.setConfigurationSetNumber(base64InputStream.readByte());
f.setConfigurationVersion(base64InputStream.readByte());
int length = base64InputStream.readByte();
f.setData(base64InputStream.readBytes(length));
result = f;
break;
default: