| 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()); |