8,6 → 8,7 |
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; |
98,6 → 99,14 |
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++) { |
154,6 → 163,7 |
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); |
215,6 → 225,15 |
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()); |
268,7 → 287,7 |
f.setBatteryVoltage(base64InputStream.readByte()); |
f.setGroundSpeed(base64InputStream.readWord()); |
|
f.setHeading(base64InputStream.readSignedWord()); |
f.setDirectionOfFlight(base64InputStream.readSignedWord()); |
f.setCompassHeading(base64InputStream.readSignedWord()); |
|
f.setPitchAngle(base64InputStream.readSignedByte()); |
355,6 → 374,14 |
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; |
} |
|