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: |