Rev 1690 | Rev 1696 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1690 | Rev 1695 | ||
---|---|---|---|
Line 10... | Line 10... | ||
10 | import dongfang.mkt.frames.AnalogDebugLabelResponseFrame; |
10 | import dongfang.mkt.frames.AnalogDebugLabelResponseFrame; |
11 | import dongfang.mkt.frames.AttitudeDataResponseFrame; |
11 | import dongfang.mkt.frames.AttitudeDataResponseFrame; |
12 | import dongfang.mkt.frames.ChangeParameterSetResponseFrame; |
12 | import dongfang.mkt.frames.ChangeParameterSetResponseFrame; |
13 | import dongfang.mkt.frames.CompassHeadingResponseFrame; |
13 | import dongfang.mkt.frames.CompassHeadingResponseFrame; |
14 | import dongfang.mkt.frames.ConfirmFrame; |
14 | import dongfang.mkt.frames.ConfirmFrame; |
- | 15 | import dongfang.mkt.frames.DCMMatrixResponseFrame; |
|
15 | import dongfang.mkt.frames.DebugResponseFrame; |
16 | import dongfang.mkt.frames.DebugResponseFrame; |
16 | import dongfang.mkt.frames.MotorTestResponseFrame; |
17 | import dongfang.mkt.frames.MotorTestResponseFrame; |
17 | import dongfang.mkt.frames.OSDDataResponseFrame; |
18 | import dongfang.mkt.frames.OSDDataResponseFrame; |
18 | import dongfang.mkt.frames.ReadExternalControlResponseFrame; |
19 | import dongfang.mkt.frames.ReadExternalControlResponseFrame; |
19 | import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
20 | import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
Line 120... | Line 121... | ||
120 | // of Unicode, which Java uses for chars) |
121 | // of Unicode, which Java uses for chars) |
121 | result[i] = (char) readByte(); |
122 | result[i] = (char) readByte(); |
122 | } |
123 | } |
123 | return result; |
124 | return result; |
124 | } |
125 | } |
- | 126 | ||
- | 127 | public float readFloat() throws IOException { |
|
- | 128 | int asInt = readByte() | (readByte()<<8) | (readByte()<<16) | (readByte()<<24); |
|
- | 129 | return Float.intBitsToFloat(asInt); |
|
125 | } |
130 | } |
- | 131 | ||
- | 132 | public float[] readFloats(int length) throws IOException { |
|
- | 133 | float[] result = new float[length]; |
|
- | 134 | for (int i = 0; i < length; i++) { |
|
- | 135 | result[i] = readFloat(); |
|
- | 136 | } |
|
- | 137 | return result; |
|
- | 138 | } |
|
- | 139 | } |
|
Line 126... | Line 140... | ||
126 | 140 | ||
127 | MKDataInputStream base64InputStream = new MKDataInputStream(); |
141 | MKDataInputStream base64InputStream = new MKDataInputStream(); |
Line 128... | Line 142... | ||
128 | OutputStream nonPacketSpillway = null; //System.err; |
142 | OutputStream nonPacketSpillway = null; //System.err; |
Line 183... | Line 197... | ||
183 | result = f; |
197 | result = f; |
184 | break; |
198 | break; |
185 | } |
199 | } |
186 | case 'C': { |
200 | case 'C': { |
187 | AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
201 | AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
188 | f.setPitch(base64InputStream.readSignedWord()); |
202 | f.setAttitude(base64InputStream.readFloats(3)); |
189 | f.setRoll(base64InputStream.readSignedWord()); |
203 | f.setRates(base64InputStream.readFloats(3)); |
190 | f.setHeading(base64InputStream.readSignedWord()); |
- | |
191 | f.setExpansion(base64InputStream.readBytes(8)); |
204 | f.setAcc(base64InputStream.readFloats(3)); |
192 | result = f; |
205 | result = f; |
193 | break; |
206 | break; |
194 | } |
207 | } |
195 | case 'D': { |
208 | case 'D': { |
196 | DebugResponseFrame f = new DebugResponseFrame(address); |
209 | DebugResponseFrame f = new DebugResponseFrame(address); |
Line 199... | Line 212... | ||
199 | for (int i=0; i<32; i++) |
212 | for (int i=0; i<32; i++) |
200 | f.setAnalog(i, base64InputStream.readSignedWord()); |
213 | f.setAnalog(i, base64InputStream.readSignedWord()); |
201 | result = f; |
214 | result = f; |
202 | break; |
215 | break; |
203 | } |
216 | } |
- | 217 | case 'E': { |
|
- | 218 | DCMMatrixResponseFrame f= new DCMMatrixResponseFrame(address); |
|
- | 219 | float[][] matrix = new float[3][]; |
|
- | 220 | for (int i=0; i<3; i++) { |
|
- | 221 | float[] row = base64InputStream.readFloats(3); |
|
- | 222 | matrix[i] = row; |
|
- | 223 | } |
|
- | 224 | f.setMatrix(matrix); |
|
- | 225 | result = f; |
|
- | 226 | break; |
|
- | 227 | } |
|
204 | case 'F': { |
228 | case 'F': { |
205 | ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
229 | ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
206 | f.setParameterSetNumber(base64InputStream.readByte()); |
230 | f.setParameterSetNumber(base64InputStream.readByte()); |
207 | result = f; |
231 | result = f; |
208 | break; |
232 | break; |