12,6 → 12,7 |
import dongfang.mkt.frames.ChangeParameterSetResponseFrame; |
import dongfang.mkt.frames.CompassHeadingResponseFrame; |
import dongfang.mkt.frames.ConfirmFrame; |
import dongfang.mkt.frames.DCMMatrixResponseFrame; |
import dongfang.mkt.frames.DebugResponseFrame; |
import dongfang.mkt.frames.MotorTestResponseFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
122,8 → 123,21 |
} |
return result; |
} |
} |
|
public float readFloat() throws IOException { |
int asInt = readByte() | (readByte()<<8) | (readByte()<<16) | (readByte()<<24); |
return Float.intBitsToFloat(asInt); |
} |
|
public float[] readFloats(int length) throws IOException { |
float[] result = new float[length]; |
for (int i = 0; i < length; i++) { |
result[i] = readFloat(); |
} |
return result; |
} |
} |
|
MKDataInputStream base64InputStream = new MKDataInputStream(); |
OutputStream nonPacketSpillway = null; //System.err; |
|
185,10 → 199,9 |
} |
case 'C': { |
AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
f.setPitch(base64InputStream.readSignedWord()); |
f.setRoll(base64InputStream.readSignedWord()); |
f.setHeading(base64InputStream.readSignedWord()); |
f.setExpansion(base64InputStream.readBytes(8)); |
f.setAttitude(base64InputStream.readFloats(3)); |
f.setRates(base64InputStream.readFloats(3)); |
f.setAcc(base64InputStream.readFloats(3)); |
result = f; |
break; |
} |
201,6 → 214,17 |
result = f; |
break; |
} |
case 'E': { |
DCMMatrixResponseFrame f= new DCMMatrixResponseFrame(address); |
float[][] matrix = new float[3][]; |
for (int i=0; i<3; i++) { |
float[] row = base64InputStream.readFloats(3); |
matrix[i] = row; |
} |
f.setMatrix(matrix); |
result = f; |
break; |
} |
case 'F': { |
ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
f.setParameterSetNumber(base64InputStream.readByte()); |