Rev 1696 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1696 | Rev 1698 | ||
---|---|---|---|
1 | package dongfang.mkt.comm; |
1 | package dongfang.mkt.comm; |
2 | 2 | ||
3 | import java.io.IOException; |
3 | import java.io.IOException; |
4 | import java.io.InputStream; |
4 | import java.io.InputStream; |
5 | import java.io.OutputStream; |
5 | import java.io.OutputStream; |
6 | 6 | ||
7 | import dongfang.mkt.datatype.GPSBearingAndRange; |
7 | import dongfang.mkt.datatype.GPSBearingAndRange; |
8 | import dongfang.mkt.datatype.GPSPosition; |
8 | import dongfang.mkt.datatype.GPSPosition; |
9 | import dongfang.mkt.frames.AllDisplaysResponseFrame; |
9 | import dongfang.mkt.frames.AllDisplaysResponseFrame; |
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.CompassHeadingResponseFrame; |
12 | import dongfang.mkt.frames.CompassHeadingResponseFrame; |
13 | import dongfang.mkt.frames.ConfirmFrame; |
13 | import dongfang.mkt.frames.ConfirmFrame; |
14 | import dongfang.mkt.frames.DCMMatrixResponseFrame; |
14 | import dongfang.mkt.frames.DCMMatrixResponseFrame; |
15 | import dongfang.mkt.frames.DebugResponseFrame; |
15 | import dongfang.mkt.frames.DebugResponseFrame; |
16 | import dongfang.mkt.frames.MotorTestResponseFrame; |
16 | import dongfang.mkt.frames.MotorTestResponseFrame; |
17 | import dongfang.mkt.frames.OSDDataResponseFrame; |
17 | import dongfang.mkt.frames.OSDDataResponseFrame; |
18 | import dongfang.mkt.frames.ProfilerLabelResponseFrame; |
18 | import dongfang.mkt.frames.ProfilerLabelResponseFrame; |
19 | import dongfang.mkt.frames.ProfilerResponseFrame; |
19 | import dongfang.mkt.frames.ProfilerResponseFrame; |
20 | import dongfang.mkt.frames.ReadExternalControlResponseFrame; |
20 | import dongfang.mkt.frames.ReadExternalControlResponseFrame; |
21 | import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
21 | import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
22 | import dongfang.mkt.frames.ReadMotorMixerResponseFrame; |
22 | import dongfang.mkt.frames.ReadMotorMixerResponseFrame; |
23 | import dongfang.mkt.frames.ReadParamSetResponseFrame; |
23 | import dongfang.mkt.frames.ReadParamSetResponseFrame; |
24 | import dongfang.mkt.frames.ReadRCChannelsResponseFrame; |
24 | import dongfang.mkt.frames.ReadRCChannelsResponseFrame; |
25 | import dongfang.mkt.frames.ReadVariablesResponseFrame; |
25 | import dongfang.mkt.frames.ReadVariablesResponseFrame; |
26 | import dongfang.mkt.frames.ResponseFrame; |
26 | import dongfang.mkt.frames.ResponseFrame; |
27 | import dongfang.mkt.frames.SetCompassHeadingResponseFrame; |
27 | import dongfang.mkt.frames.SetCompassHeadingResponseFrame; |
28 | import dongfang.mkt.frames.VersionResponseFrame; |
28 | import dongfang.mkt.frames.VersionResponseFrame; |
29 | import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame; |
29 | import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame; |
30 | import dongfang.mkt.frames.WriteMotorMixerResponseFrame; |
30 | import dongfang.mkt.frames.WriteMotorMixerResponseFrame; |
31 | import dongfang.mkt.frames.WriteParamSetResponseFrame; |
31 | import dongfang.mkt.frames.WriteParamSetResponseFrame; |
32 | 32 | ||
33 | public class MKInputStream extends InputStream { |
33 | public class MKInputStream extends InputStream { |
34 | int readByteCnt; |
34 | int readByteCnt; |
35 | class MKDataInputStream { |
35 | class MKDataInputStream { |
36 | int[] inbuf = new int[4]; |
36 | int[] inbuf = new int[4]; |
37 | int[] outbuf = new int[3]; |
37 | int[] outbuf = new int[3]; |
38 | int outbufptr = outbuf.length; // reset to "buffer empty" |
38 | int outbufptr = outbuf.length; // reset to "buffer empty" |
39 | 39 | ||
40 | private boolean decode() throws IOException { |
40 | private boolean decode() throws IOException { |
41 | for (int i = 0; i < 4; i++) { |
41 | for (int i = 0; i < 4; i++) { |
42 | int raw = MKInputStream.this.readByte(); |
42 | int raw = MKInputStream.this.readByte(); |
43 | int in = raw - '='; |
43 | int in = raw - '='; |
44 | if (in < 0 || in > 63) |
44 | if (in < 0 || in > 63) |
45 | return false; |
45 | return false; |
46 | // throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!"); |
46 | // throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!"); |
47 | inbuf[i] = in; |
47 | inbuf[i] = in; |
48 | readByteCnt++; |
48 | readByteCnt++; |
49 | } |
49 | } |
50 | outbuf[0] = (inbuf[0] << 2) | (inbuf[1] >>> 4); |
50 | outbuf[0] = (inbuf[0] << 2) | (inbuf[1] >>> 4); |
51 | outbuf[1] = ((inbuf[1] & 0x0f) << 4) | (inbuf[2] >>> 2); |
51 | outbuf[1] = ((inbuf[1] & 0x0f) << 4) | (inbuf[2] >>> 2); |
52 | outbuf[2] = ((inbuf[2] & 0x03) << 6) | inbuf[3]; |
52 | outbuf[2] = ((inbuf[2] & 0x03) << 6) | inbuf[3]; |
53 | outbufptr = 0; |
53 | outbufptr = 0; |
54 | return true; |
54 | return true; |
55 | } |
55 | } |
56 | 56 | ||
57 | public void reset() { |
57 | public void reset() { |
58 | outbufptr = outbuf.length; // reset to "buffer empty" |
58 | outbufptr = outbuf.length; // reset to "buffer empty" |
59 | } |
59 | } |
60 | 60 | ||
61 | public int readByte() throws IOException { |
61 | public int readByte() throws IOException { |
62 | if (outbufptr > 2 && !decode()) |
62 | if (outbufptr > 2 && !decode()) |
63 | throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!"); |
63 | throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!"); |
64 | return outbuf[outbufptr++]; |
64 | return outbuf[outbufptr++]; |
65 | } |
65 | } |
66 | 66 | ||
67 | public int readSignedByte() throws IOException { |
67 | public int readSignedByte() throws IOException { |
68 | byte result = (byte)readByte(); |
68 | byte result = (byte)readByte(); |
69 | return result; |
69 | return result; |
70 | } |
70 | } |
71 | 71 | ||
72 | public int readWord() throws IOException { |
72 | public int readWord() throws IOException { |
73 | int byte0 = readByte(); |
73 | int byte0 = readByte(); |
74 | int byte1 = readByte(); |
74 | int byte1 = readByte(); |
75 | return (byte1 << 8) | byte0; |
75 | return (byte1 << 8) | byte0; |
76 | } |
76 | } |
77 | 77 | ||
78 | public int readSignedWord() throws IOException { |
78 | public int readSignedWord() throws IOException { |
79 | int word = readWord(); |
79 | int word = readWord(); |
80 | if (word > 32767) |
80 | if (word > 32767) |
81 | word = word - 65536; |
81 | word = word - 65536; |
82 | return word; |
82 | return word; |
83 | } |
83 | } |
84 | 84 | ||
85 | public int readSignedDWord() throws IOException { |
85 | public int readSignedDWord() throws IOException { |
86 | int byte0 = readByte(); |
86 | int byte0 = readByte(); |
87 | int byte1 = readByte(); |
87 | int byte1 = readByte(); |
88 | int byte2 = readByte(); |
88 | int byte2 = readByte(); |
89 | int byte3 = readByte(); |
89 | int byte3 = readByte(); |
90 | return (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0; |
90 | return (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0; |
91 | } |
91 | } |
92 | 92 | ||
93 | public int[] readBytes(int length) throws IOException { |
93 | public int[] readBytes(int length) throws IOException { |
94 | int[] result = new int[length]; |
94 | int[] result = new int[length]; |
95 | for (int i = 0; i < length; i++) { |
95 | for (int i = 0; i < length; i++) { |
96 | result[i] = readByte(); |
96 | result[i] = readByte(); |
97 | } |
97 | } |
98 | return result; |
98 | return result; |
99 | } |
99 | } |
100 | 100 | ||
101 | public int[] readWords(int length) throws IOException { |
101 | public int[] readWords(int length) throws IOException { |
102 | int[] result = new int[length]; |
102 | int[] result = new int[length]; |
103 | for (int i = 0; i < length; i++) { |
103 | for (int i = 0; i < length; i++) { |
104 | result[i] = readWord(); |
104 | result[i] = readWord(); |
105 | } |
105 | } |
106 | return result; |
106 | return result; |
107 | } |
107 | } |
108 | 108 | ||
109 | public int[] readSignedWords(int length) throws IOException { |
109 | public int[] readSignedWords(int length) throws IOException { |
110 | int[] result = new int[length]; |
110 | int[] result = new int[length]; |
111 | for (int i = 0; i < length; i++) { |
111 | for (int i = 0; i < length; i++) { |
112 | result[i] = readSignedWord(); |
112 | result[i] = readSignedWord(); |
113 | } |
113 | } |
114 | return result; |
114 | return result; |
115 | } |
115 | } |
116 | 116 | ||
117 | public char[] readChars(int length) throws IOException { |
117 | public char[] readChars(int length) throws IOException { |
118 | char[] result = new char[length]; |
118 | char[] result = new char[length]; |
119 | for (int i = 0; i < length; i++) { |
119 | for (int i = 0; i < length; i++) { |
120 | // Here, a 1:1 mapping between byte values and char codes is assumed. |
120 | // Here, a 1:1 mapping between byte values and char codes is assumed. |
121 | // That means we're assuming ISO-8859-1 (= the first 256 code points |
121 | // That means we're assuming ISO-8859-1 (= the first 256 code points |
122 | // of Unicode, which Java uses for chars) |
122 | // of Unicode, which Java uses for chars) |
123 | result[i] = (char) readByte(); |
123 | result[i] = (char) readByte(); |
124 | } |
124 | } |
125 | return result; |
125 | return result; |
126 | } |
126 | } |
127 | 127 | ||
128 | public float readFloat() throws IOException { |
128 | public float readFloat() throws IOException { |
129 | int asInt = readByte() | (readByte()<<8) | (readByte()<<16) | (readByte()<<24); |
129 | int asInt = readByte() | (readByte()<<8) | (readByte()<<16) | (readByte()<<24); |
130 | return Float.intBitsToFloat(asInt); |
130 | return Float.intBitsToFloat(asInt); |
131 | } |
131 | } |
132 | 132 | ||
133 | public float[] readFloats(int length) throws IOException { |
133 | public float[] readFloats(int length) throws IOException { |
134 | float[] result = new float[length]; |
134 | float[] result = new float[length]; |
135 | for (int i = 0; i < length; i++) { |
135 | for (int i = 0; i < length; i++) { |
136 | result[i] = readFloat(); |
136 | result[i] = readFloat(); |
137 | } |
137 | } |
138 | return result; |
138 | return result; |
139 | } |
139 | } |
140 | } |
140 | } |
141 | 141 | ||
142 | MKDataInputStream base64InputStream = new MKDataInputStream(); |
142 | MKDataInputStream base64InputStream = new MKDataInputStream(); |
143 | OutputStream nonPacketSpillway = null; //System.err; |
143 | OutputStream nonPacketSpillway = null; //System.err; |
144 | 144 | ||
145 | final InputStream is; |
145 | final InputStream is; |
146 | int crc; |
146 | int crc; |
147 | 147 | ||
148 | public MKInputStream(InputStream is) { |
148 | public MKInputStream(InputStream is) { |
149 | this.is = is; |
149 | this.is = is; |
150 | } |
150 | } |
151 | 151 | ||
152 | @Override |
152 | @Override |
153 | public int read() throws IOException { |
153 | public int read() throws IOException { |
154 | int i; |
154 | int i; |
155 | while ((i=is.read()) == -1); |
155 | while ((i=is.read()) == -1); |
156 | // System.out.print("Received: " + i + " (as char: " + (char)i + ")\n"); |
156 | // System.out.print("Received: " + i + " (as char: " + (char)i + ")\n"); |
157 | return i; |
157 | return i; |
158 | } |
158 | } |
159 | 159 | ||
160 | public int readByte() throws IOException { |
160 | public int readByte() throws IOException { |
161 | int _byte = read(); |
161 | int _byte = read(); |
162 | if (_byte < 0) |
162 | if (_byte < 0) |
163 | throw new IOException("End of Stream!"); |
163 | throw new IOException("End of Stream!"); |
164 | crc += _byte; |
164 | crc += _byte; |
165 | return _byte; |
165 | return _byte; |
166 | } |
166 | } |
167 | 167 | ||
168 | public MKDataInputStream getBase64InputStream() { |
168 | public MKDataInputStream getBase64InputStream() { |
169 | return base64InputStream; |
169 | return base64InputStream; |
170 | } |
170 | } |
171 | 171 | ||
172 | public ResponseFrame getNextFrame() throws IOException { |
172 | public ResponseFrame getNextFrame() throws IOException { |
173 | int c; |
173 | int c; |
174 | while ((c = read()) != '#') { |
174 | while ((c = read()) != '#') { |
175 | // throw it on some scrap-text buffer. |
175 | // throw it on some scrap-text buffer. |
176 | if (nonPacketSpillway != null) |
176 | if (nonPacketSpillway != null) |
177 | nonPacketSpillway.write(c); |
177 | nonPacketSpillway.write(c); |
178 | } |
178 | } |
179 | crc = '#'; |
179 | crc = '#'; |
180 | base64InputStream.reset(); |
180 | base64InputStream.reset(); |
181 | int address = readByte() - 'a'; |
181 | int address = readByte() - 'a'; |
182 | int iid = readByte(); |
182 | int iid = readByte(); |
183 | readByteCnt = 0; |
183 | readByteCnt = 0; |
184 | //RESPONSE_IDS id = getResponseType(iid); |
184 | //RESPONSE_IDS id = getResponseType(iid); |
185 | ResponseFrame result; |
185 | ResponseFrame result; |
186 | // System.out.println("Received a: " + (char)iid + " from " + address); |
186 | // System.out.println("Received a: " + (char)iid + " from " + address); |
187 | switch (iid) { |
187 | switch (iid) { |
188 | case 'A': { |
188 | case 'A': { |
189 | AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address); |
189 | AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address); |
190 | f.setChannel(base64InputStream.readByte()); |
190 | f.setChannel(base64InputStream.readByte()); |
191 | f.setLabel(base64InputStream.readChars(16)); |
191 | f.setLabel(base64InputStream.readChars(16)); |
192 | result = f; |
192 | result = f; |
193 | break; |
193 | break; |
194 | } |
194 | } |
195 | case 'B': { |
195 | case 'B': { |
196 | ConfirmFrame f = new ConfirmFrame(address); |
196 | ConfirmFrame f = new ConfirmFrame(address); |
197 | f.setFrameNum(base64InputStream.readByte()); |
197 | f.setFrameNum(base64InputStream.readByte()); |
198 | result = f; |
198 | result = f; |
199 | break; |
199 | break; |
200 | } |
200 | } |
201 | case 'C': { |
201 | case 'C': { |
202 | AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
202 | AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
203 | f.setAttitude(base64InputStream.readFloats(3)); |
203 | f.setAttitude(base64InputStream.readFloats(3)); |
204 | f.setRates(base64InputStream.readFloats(3)); |
204 | f.setRates(base64InputStream.readFloats(3)); |
205 | f.setAcc(base64InputStream.readFloats(3)); |
205 | f.setAcc(base64InputStream.readFloats(3)); |
206 | result = f; |
206 | result = f; |
207 | break; |
207 | break; |
208 | } |
208 | } |
209 | case 'D': { |
209 | case 'D': { |
210 | DebugResponseFrame f = new DebugResponseFrame(address); |
210 | DebugResponseFrame f = new DebugResponseFrame(address); |
211 | for (int i=0; i<2; i++) |
211 | for (int i=0; i<2; i++) |
212 | f.setDigital(i, base64InputStream.readByte()); |
212 | f.setDigital(i, base64InputStream.readByte()); |
213 | for (int i=0; i<32; i++) |
213 | for (int i=0; i<32; i++) |
214 | f.setAnalog(i, base64InputStream.readSignedWord()); |
214 | f.setAnalog(i, base64InputStream.readSignedWord()); |
215 | result = f; |
215 | result = f; |
216 | break; |
216 | break; |
217 | } |
217 | } |
218 | case 'E': { |
218 | case 'E': { |
219 | DCMMatrixResponseFrame f= new DCMMatrixResponseFrame(address); |
219 | DCMMatrixResponseFrame f= new DCMMatrixResponseFrame(address); |
220 | float[][] matrix = new float[3][]; |
220 | float[][] matrix = new float[3][]; |
221 | for (int i=0; i<3; i++) { |
221 | for (int i=0; i<3; i++) { |
222 | float[] row = base64InputStream.readFloats(3); |
222 | float[] row = base64InputStream.readFloats(3); |
223 | matrix[i] = row; |
223 | matrix[i] = row; |
224 | } |
224 | } |
225 | f.setMatrix(matrix); |
225 | f.setMatrix(matrix); |
226 | result = f; |
226 | result = f; |
227 | break; |
227 | break; |
228 | } |
228 | } |
229 | /* |
229 | /* |
230 | case 'F': { |
230 | case 'F': { |
231 | ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
231 | ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
232 | f.setParameterSetNumber(base64InputStream.readByte()); |
232 | f.setParameterSetNumber(base64InputStream.readByte()); |
233 | result = f; |
233 | result = f; |
234 | break; |
234 | break; |
235 | } |
235 | } |
236 | */ |
236 | */ |
237 | case 'F': { |
237 | case 'F': { |
238 | ProfilerLabelResponseFrame f = new ProfilerLabelResponseFrame(address); |
238 | ProfilerLabelResponseFrame f = new ProfilerLabelResponseFrame(address); |
239 | f.setChannel(base64InputStream.readByte()); |
239 | f.setChannel(base64InputStream.readByte()); |
240 | f.setLabel(base64InputStream.readChars(16)); |
240 | f.setLabel(base64InputStream.readChars(16)); |
241 | result = f; |
241 | result = f; |
242 | break; |
242 | break; |
243 | } |
243 | } |
244 | 244 | ||
245 | case 'G': { |
245 | case 'G': { |
246 | ReadExternalControlResponseFrame f = new ReadExternalControlResponseFrame(address); |
246 | ReadExternalControlResponseFrame f = new ReadExternalControlResponseFrame(address); |
247 | f.setDigital(base64InputStream.readBytes(2)); |
247 | f.setDigital(base64InputStream.readBytes(2)); |
248 | f.setRemoteButtons(base64InputStream.readByte()); |
248 | f.setRemoteButtons(base64InputStream.readByte()); |
249 | f.setPitch(base64InputStream.readByte()); |
249 | f.setPitch(base64InputStream.readByte()); |
250 | f.setRoll(base64InputStream.readByte()); |
250 | f.setRoll(base64InputStream.readByte()); |
251 | f.setYaw(base64InputStream.readByte()); |
251 | f.setYaw(base64InputStream.readByte()); |
252 | f.setThrottle(base64InputStream.readByte()); |
252 | f.setThrottle(base64InputStream.readByte()); |
253 | f.setHeight(base64InputStream.readByte()); |
253 | f.setHeight(base64InputStream.readByte()); |
254 | f.setCommand(base64InputStream.readByte()); |
254 | f.setCommand(base64InputStream.readByte()); |
255 | f.setFrameNum(base64InputStream.readByte()); |
255 | f.setFrameNum(base64InputStream.readByte()); |
256 | f.setArgument(base64InputStream.readByte()); |
256 | f.setArgument(base64InputStream.readByte()); |
257 | result = f; |
257 | result = f; |
258 | break; |
258 | break; |
259 | } |
259 | } |
260 | case 'H': { |
260 | case 'H': { |
261 | AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address); |
261 | AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address); |
262 | f.setLine(base64InputStream.readByte()); |
262 | f.setLine(base64InputStream.readByte()); |
263 | //f.setMaxItem(getDataInputStream().readByte()); |
263 | //f.setMaxItem(getDataInputStream().readByte()); |
264 | f.setText(base64InputStream.readChars(20)); |
264 | f.setText(base64InputStream.readChars(20)); |
265 | result = f; |
265 | result = f; |
266 | break; |
266 | break; |
267 | } |
267 | } |
268 | case 'I': { |
268 | case 'I': { |
269 | ReadIMUConfigurationResponseFrame f = new ReadIMUConfigurationResponseFrame(address); |
269 | ReadIMUConfigurationResponseFrame f = new ReadIMUConfigurationResponseFrame(address); |
270 | f.setConfigurationVersion(base64InputStream.readByte()); |
270 | f.setConfigurationVersion(base64InputStream.readByte()); |
271 | int length = base64InputStream.readByte(); |
271 | int length = base64InputStream.readByte(); |
272 | f.setConfigurationSetLength(length); |
272 | f.setConfigurationSetLength(length); |
273 | f.setData(base64InputStream.readBytes(length)); |
273 | f.setData(base64InputStream.readBytes(length)); |
274 | result = f; |
274 | result = f; |
275 | break; |
275 | break; |
276 | } |
276 | } |
277 | case 'J': { |
277 | case 'J': { |
278 | WriteIMUConfigurationResponseFrame f = new WriteIMUConfigurationResponseFrame(address); |
278 | WriteIMUConfigurationResponseFrame f = new WriteIMUConfigurationResponseFrame(address); |
279 | f.setWasAccepted(base64InputStream.readByte()==1); |
279 | f.setWasAccepted(base64InputStream.readByte()==1); |
280 | result = f; |
280 | result = f; |
281 | break; |
281 | break; |
282 | } |
282 | } |
283 | case 'k' : { |
283 | case 'k' : { |
284 | CompassHeadingResponseFrame f = new CompassHeadingResponseFrame(address); |
284 | CompassHeadingResponseFrame f = new CompassHeadingResponseFrame(address); |
285 | base64InputStream.readSignedWords(2); |
285 | base64InputStream.readSignedWords(2); |
286 | base64InputStream.readBytes(2); |
286 | base64InputStream.readBytes(2); |
287 | base64InputStream.readByte(); |
287 | base64InputStream.readByte(); |
288 | base64InputStream.readByte(); |
288 | base64InputStream.readByte(); |
289 | result = f; |
289 | result = f; |
290 | break; |
290 | break; |
291 | } |
291 | } |
292 | case 'L': { |
292 | case 'L': { |
293 | AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address); |
293 | AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address); |
294 | f.setItem(base64InputStream.readByte()); |
294 | f.setItem(base64InputStream.readByte()); |
295 | // f.setMaxItem(getDataInputStream().readByte()); |
295 | // f.setMaxItem(getDataInputStream().readByte()); |
296 | f.setText(base64InputStream.readChars(80)); |
296 | f.setText(base64InputStream.readChars(80)); |
297 | result = f; |
297 | result = f; |
298 | break; |
298 | break; |
299 | } |
299 | } |
300 | case 'M': { |
300 | case 'M': { |
301 | WriteMotorMixerResponseFrame f = new WriteMotorMixerResponseFrame(address); |
301 | WriteMotorMixerResponseFrame f = new WriteMotorMixerResponseFrame(address); |
302 | f.setWasAccepted(base64InputStream.readByte() != 0); |
302 | f.setWasAccepted(base64InputStream.readByte() != 0); |
303 | result = f; |
303 | result = f; |
304 | break; |
304 | break; |
305 | } |
305 | } |
306 | case 'N': { |
306 | case 'N': { |
307 | int numMotors = 12; |
307 | int numMotors = 12; |
308 | ReadMotorMixerResponseFrame f = new ReadMotorMixerResponseFrame(); |
308 | ReadMotorMixerResponseFrame f = new ReadMotorMixerResponseFrame(); |
309 | f.setConfigurationVersion(base64InputStream.readByte()); |
309 | f.setConfigurationVersion(base64InputStream.readByte()); |
310 | int length = base64InputStream.readByte(); |
310 | int length = base64InputStream.readByte(); |
311 | f.setDataLength(length); |
311 | f.setDataLength(length); |
312 | int testLength = 12; // the name is first! |
312 | int testLength = 12; // the name is first! |
313 | char[] name = base64InputStream.readChars(12); |
313 | char[] name = base64InputStream.readChars(12); |
314 | f.setName(name); |
314 | f.setName(name); |
315 | int[][] matrix = new int[numMotors][]; |
315 | int[][] matrix = new int[numMotors][]; |
316 | for(int i=0; i<numMotors; i++) { |
316 | for(int i=0; i<numMotors; i++) { |
317 | int[] row = new int[5]; |
317 | int[] row = new int[5]; |
318 | matrix[i] = row; |
318 | matrix[i] = row; |
319 | for(int j=0; j<5; j++) { |
319 | for(int j=0; j<5; j++) { |
320 | row[j] = base64InputStream.readSignedByte(); |
320 | row[j] = base64InputStream.readSignedByte(); |
321 | testLength++; |
321 | testLength++; |
322 | } |
322 | } |
323 | } |
323 | } |
324 | 324 | ||
325 | if (length != testLength) |
325 | if (length != testLength) |
326 | throw new IOException("Length of motor mixer data was not as expected (" + testLength + " vs. " + length + ")."); |
326 | throw new IOException("Length of motor mixer data was not as expected (" + testLength + " vs. " + length + ")."); |
327 | /* |
327 | /* |
328 | int[] opposite = new int[numMotors]; |
328 | int[] opposite = new int[numMotors]; |
329 | for(int i=0; i<numMotors; i++) { |
329 | for(int i=0; i<numMotors; i++) { |
330 | opposite[i] = base64InputStream.readByte(); |
330 | opposite[i] = base64InputStream.readByte(); |
331 | } |
331 | } |
332 | */ |
332 | */ |
333 | f.setMatrix(matrix); |
333 | f.setMatrix(matrix); |
334 | result = f; |
334 | result = f; |
335 | break; |
335 | break; |
336 | } |
336 | } |
337 | case 'O': { |
337 | case 'O': { |
338 | OSDDataResponseFrame f = new OSDDataResponseFrame(address); |
338 | OSDDataResponseFrame f = new OSDDataResponseFrame(address); |
339 | f.setVersion(base64InputStream.readByte()); |
339 | f.setVersion(base64InputStream.readByte()); |
340 | 340 | ||
341 | GPSPosition pos = new GPSPosition(); |
341 | GPSPosition pos = new GPSPosition(); |
342 | pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7); |
342 | pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7); |
343 | pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7); |
343 | pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7); |
344 | pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3); |
344 | pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3); |
345 | pos.setStatus(base64InputStream.readByte()); |
345 | pos.setStatus(base64InputStream.readByte()); |
346 | f.setCurrentPosition(pos); |
346 | f.setCurrentPosition(pos); |
347 | 347 | ||
348 | pos = new GPSPosition(); |
348 | pos = new GPSPosition(); |
349 | pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7); |
349 | pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7); |
350 | pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7); |
350 | pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7); |
351 | pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3); |
351 | pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3); |
352 | pos.setStatus(base64InputStream.readByte()); |
352 | pos.setStatus(base64InputStream.readByte()); |
353 | f.setTargetPosition(pos); |
353 | f.setTargetPosition(pos); |
354 | 354 | ||
355 | GPSBearingAndRange rnb = new GPSBearingAndRange(); |
355 | GPSBearingAndRange rnb = new GPSBearingAndRange(); |
356 | rnb.setDistance(((double)base64InputStream.readWord())/10.0); |
356 | rnb.setDistance(((double)base64InputStream.readWord())/10.0); |
357 | rnb.setBearing(base64InputStream.readSignedWord()); |
357 | rnb.setBearing(base64InputStream.readSignedWord()); |
358 | f.setCurrentToTarget(rnb); |
358 | f.setCurrentToTarget(rnb); |
359 | 359 | ||
360 | pos = new GPSPosition(); |
360 | pos = new GPSPosition(); |
361 | pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7); |
361 | pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7); |
362 | pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7); |
362 | pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7); |
363 | pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3); |
363 | pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3); |
364 | pos.setStatus(base64InputStream.readByte()); |
364 | pos.setStatus(base64InputStream.readByte()); |
365 | f.setHomePosition(pos); |
365 | f.setHomePosition(pos); |
366 | 366 | ||
367 | rnb = new GPSBearingAndRange(); |
367 | rnb = new GPSBearingAndRange(); |
368 | rnb.setDistance(((double)base64InputStream.readWord())/10.0); |
368 | rnb.setDistance(((double)base64InputStream.readWord())/10.0); |
369 | rnb.setBearing(base64InputStream.readSignedWord()); |
369 | rnb.setBearing(base64InputStream.readSignedWord()); |
370 | f.setCurrentToHome(rnb); |
370 | f.setCurrentToHome(rnb); |
371 | 371 | ||
372 | f.setWaypointIndex(base64InputStream.readByte()); |
372 | f.setWaypointIndex(base64InputStream.readByte()); |
373 | f.setWaypointCount(base64InputStream.readByte()); |
373 | f.setWaypointCount(base64InputStream.readByte()); |
374 | f.setNumberOfSatellites(base64InputStream.readByte()); |
374 | f.setNumberOfSatellites(base64InputStream.readByte()); |
375 | 375 | ||
376 | // This stunt is a metric unit conversion: The height was supposed (H&I) to be in integral 5cm steps. |
376 | // This stunt is a metric unit conversion: The height was supposed (H&I) to be in integral 5cm steps. |
377 | // However there is error factor in the measurement of 24% too much. |
377 | // However there is error factor in the measurement of 24% too much. |
378 | // h[m] = h[int] * 0.05 / 1.24 = h[int] |
378 | // h[m] = h[int] * 0.05 / 1.24 = h[int] |
379 | f._setHeightByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); |
379 | f._setHeightByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); |
380 | 380 | ||
381 | f.setVerticalVelocityByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); // clueless! |
381 | f.setVerticalVelocityByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); // clueless! |
382 | f.setFlightTime(base64InputStream.readWord()); |
382 | f.setFlightTime(base64InputStream.readWord()); |
383 | f.setBatteryVoltage(base64InputStream.readByte()); |
383 | f.setBatteryVoltage(base64InputStream.readByte()); |
384 | f.setGroundSpeed(((double)base64InputStream.readWord()) / 1E2); |
384 | f.setGroundSpeed(((double)base64InputStream.readWord()) / 1E2); |
385 | 385 | ||
386 | f.setDirectionOfFlight(base64InputStream.readSignedWord()); |
386 | f.setDirectionOfFlight(base64InputStream.readSignedWord()); |
387 | f.setCompassHeading(base64InputStream.readSignedWord()); |
387 | f.setCompassHeading(base64InputStream.readSignedWord()); |
388 | 388 | ||
389 | f.setPitchAngle(base64InputStream.readSignedByte()); |
389 | f.setPitchAngle(base64InputStream.readSignedByte()); |
390 | f.setRollAngle(base64InputStream.readSignedByte()); |
390 | f.setRollAngle(base64InputStream.readSignedByte()); |
391 | 391 | ||
392 | f.setRcQuality(base64InputStream.readByte()); |
392 | f.setRcQuality(base64InputStream.readByte()); |
393 | f.setFcFlags(base64InputStream.readByte()); |
393 | f.setFcFlags(base64InputStream.readByte()); |
394 | f.setNcFlags(base64InputStream.readByte()); |
394 | f.setNcFlags(base64InputStream.readByte()); |
395 | f.setErrorCode(base64InputStream.readByte()); |
395 | f.setErrorCode(base64InputStream.readByte()); |
396 | f.setOperatingRadius(base64InputStream.readByte()); |
396 | f.setOperatingRadius(base64InputStream.readByte()); |
397 | 397 | ||
398 | f.setVerticalVelocityByGPS(((double)base64InputStream.readSignedWord()) / 1E2); |
398 | f.setVerticalVelocityByGPS(((double)base64InputStream.readSignedWord()) / 1E2); |
399 | f.setTargetLoiterTime(base64InputStream.readByte()); |
399 | f.setTargetLoiterTime(base64InputStream.readByte()); |
400 | f.setFcFlags2(base64InputStream.readByte()); |
400 | f.setFcFlags2(base64InputStream.readByte()); |
401 | f.setSetpointForAltitude(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); |
401 | f.setSetpointForAltitude(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); |
402 | f.setThrottle(base64InputStream.readByte()); |
402 | f.setThrottle(base64InputStream.readByte()); |
403 | f.setCurrent(base64InputStream.readWord()); |
403 | f.setCurrent(base64InputStream.readWord()); |
404 | f.setCapacityUsed(base64InputStream.readWord()); |
404 | f.setCapacityUsed(base64InputStream.readWord()); |
405 | result = f; |
405 | result = f; |
406 | break; |
406 | break; |
407 | } |
407 | } |
408 | case 'P': { |
408 | case 'P': { |
409 | ReadRCChannelsResponseFrame f = new ReadRCChannelsResponseFrame(address); |
409 | ReadRCChannelsResponseFrame f = new ReadRCChannelsResponseFrame(address); |
410 | int numberOfChannels = base64InputStream.readByte(); |
410 | int numberOfChannels = base64InputStream.readByte(); |
411 | f.setNumberOfChannels(numberOfChannels); |
411 | f.setNumberOfChannels(numberOfChannels); |
412 | f.setChannels(base64InputStream.readSignedWords(numberOfChannels)); |
412 | f.setChannels(base64InputStream.readSignedWords(numberOfChannels)); |
413 | result = f; |
413 | result = f; |
414 | break; |
414 | break; |
415 | } |
415 | } |
416 | case 'S': { |
416 | case 'S': { |
417 | WriteParamSetResponseFrame f = new WriteParamSetResponseFrame(address); |
417 | WriteParamSetResponseFrame f = new WriteParamSetResponseFrame(address); |
418 | f.setParameterSetNumber(base64InputStream.readByte()); |
418 | f.setParameterSetNumber(base64InputStream.readByte()); |
419 | result = f; |
419 | result = f; |
420 | break; |
420 | break; |
421 | } |
421 | } |
422 | case 'T': { |
422 | case 'T': { |
423 | MotorTestResponseFrame f = new MotorTestResponseFrame(address); |
423 | MotorTestResponseFrame f = new MotorTestResponseFrame(address); |
424 | result = f; |
424 | result = f; |
425 | break; |
425 | break; |
426 | } |
426 | } |
427 | /* |
427 | /* |
428 | * We have a collision with the 'x' token: Also used for VariablesRequest. |
428 | * We have a collision with the 'x' token: Also used for VariablesRequest. |
429 | case 'x': { |
429 | case 'x': { |
430 | LoopbackTestResponseFrame f = new LoopbackTestResponseFrame(address); |
430 | LoopbackTestResponseFrame f = new LoopbackTestResponseFrame(address); |
431 | f.setByte(getDataInputStream().readByte()); |
431 | f.setByte(getDataInputStream().readByte()); |
432 | f.setWord(getDataInputStream().readWord()); |
432 | f.setWord(getDataInputStream().readWord()); |
433 | f.setChararray(getDataInputStream().readChars(8)); |
433 | f.setChararray(getDataInputStream().readChars(8)); |
434 | result = f; |
434 | result = f; |
435 | break; |
435 | break; |
436 | } |
436 | } |
437 | */ |
437 | */ |
438 | case 'U': { |
438 | case 'U': { |
439 | ProfilerResponseFrame f = new ProfilerResponseFrame(address); |
439 | ProfilerResponseFrame f = new ProfilerResponseFrame(address); |
440 | f.setTotalHits(base64InputStream.readSignedDWord()); |
440 | f.setTotalHits(base64InputStream.readSignedDWord()); |
441 | for (int i=0; i<16; i++) |
441 | for (int i=0; i<16; i++) |
442 | f.setActivity(i, base64InputStream.readWord()); |
442 | f.setActivity(i, base64InputStream.readWord()); |
443 | result = f; |
443 | result = f; |
444 | break; |
444 | break; |
445 | } |
445 | } |
446 | case 'V': { |
446 | case 'V': { |
447 | VersionResponseFrame f = new VersionResponseFrame(address); |
447 | VersionResponseFrame f = new VersionResponseFrame(address); |
448 | f.setSWMajor(base64InputStream.readByte()); |
448 | f.setSWMajor(base64InputStream.readByte()); |
449 | f.setSWMinor(base64InputStream.readByte()); |
449 | f.setSWMinor(base64InputStream.readByte()); |
- | 450 | f.setSWPatch(base64InputStream.readByte()); |
|
450 | f.setProtoMajor(base64InputStream.readByte()); |
451 | f.setProtoMajor(base64InputStream.readByte()); |
451 | f.setProtoMinor(base64InputStream.readByte()); |
452 | f.setProtoMinor(base64InputStream.readByte()); |
452 | f.setSWPatch(base64InputStream.readByte()); |
- | |
453 | f.setHardwareErrors(base64InputStream.readBytes(5)); |
453 | f.setHardwareErrors(base64InputStream.readBytes(5)); |
454 | result = f; |
454 | result = f; |
455 | break; |
455 | break; |
456 | } |
456 | } |
457 | 457 | ||
458 | // This is my own creation. The ID collides with the waypoint one of FC. |
458 | // This is my own creation. The ID collides with the waypoint one of FC. |
459 | case 'X': { |
459 | case 'X': { |
460 | ReadVariablesResponseFrame f = new ReadVariablesResponseFrame(address); |
460 | ReadVariablesResponseFrame f = new ReadVariablesResponseFrame(address); |
461 | f.setVariables(base64InputStream.readSignedWords(8)); |
461 | f.setVariables(base64InputStream.readSignedWords(8)); |
462 | result = f; |
462 | result = f; |
463 | break; |
463 | break; |
464 | } |
464 | } |
465 | case 'w': { |
465 | case 'w': { |
466 | SetCompassHeadingResponseFrame f = new SetCompassHeadingResponseFrame(address); |
466 | SetCompassHeadingResponseFrame f = new SetCompassHeadingResponseFrame(address); |
467 | // do stuff. |
467 | // do stuff. |
468 | /* |
468 | /* |
469 | ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
469 | ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
470 | ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
470 | ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg |
471 | ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
471 | ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0]; |
472 | ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
472 | ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1]; |
473 | ToMk3Mag.CalState = compassCalState; |
473 | ToMk3Mag.CalState = compassCalState; |
474 | */ |
474 | */ |
475 | // Waste 8 bytes to make CRC match. |
475 | // Waste 8 bytes to make CRC match. |
476 | base64InputStream.readBytes(8); |
476 | base64InputStream.readBytes(8); |
477 | result = f; |
477 | result = f; |
478 | break; |
478 | break; |
479 | } |
479 | } |
480 | case 'Q': |
480 | case 'Q': |
481 | ReadParamSetResponseFrame f = new ReadParamSetResponseFrame(address); |
481 | ReadParamSetResponseFrame f = new ReadParamSetResponseFrame(address); |
482 | f.setConfigurationSetNumber(base64InputStream.readByte()); |
482 | f.setConfigurationSetNumber(base64InputStream.readByte()); |
483 | f.setConfigurationVersion(base64InputStream.readByte()); |
483 | f.setConfigurationVersion(base64InputStream.readByte()); |
484 | int length = base64InputStream.readByte(); |
484 | int length = base64InputStream.readByte(); |
485 | f.setConfigurationSetLength(length); |
485 | f.setConfigurationSetLength(length); |
486 | f.setData(base64InputStream.readBytes(length)); |
486 | f.setData(base64InputStream.readBytes(length)); |
487 | result = f; |
487 | result = f; |
488 | break; |
488 | break; |
489 | default: |
489 | default: |
490 | int count = 0; |
490 | int count = 0; |
491 | while(read() != '\r') { |
491 | while(read() != '\r') { |
492 | count++; |
492 | count++; |
493 | } |
493 | } |
494 | System.err.println("Unknown frame " + (char)iid + " received from " + address); |
494 | System.err.println("Unknown frame " + (char)iid + " received from " + address); |
495 | System.err.println("It appears to have " + (count-2) + " data bytes (encoded)"); |
495 | System.err.println("It appears to have " + (count-2) + " data bytes (encoded)"); |
496 | System.err.println("(" + (count-2) * 6/8 + " data bytes decoded)"); |
496 | System.err.println("(" + (count-2) * 6/8 + " data bytes decoded)"); |
497 | result = null; |
497 | result = null; |
498 | } |
498 | } |
499 | 499 | ||
500 | int receivedCRC = (read() - '=') << 6; |
500 | int receivedCRC = (read() - '=') << 6; |
501 | receivedCRC += (read() - '='); |
501 | receivedCRC += (read() - '='); |
502 | crc %= 4096; |
502 | crc %= 4096; |
503 | if (receivedCRC != crc) { |
503 | if (receivedCRC != crc) { |
504 | /// System.err.println("Expected CRC: " + crc + ", got CRC: " + receivedCRC); |
504 | /// System.err.println("Expected CRC: " + crc + ", got CRC: " + receivedCRC); |
505 | throw new IOException("CRC mismatch! Calculated crc: " + (int)crc + "; received check crc: " + receivedCRC + ", difference: " + Math.abs(crc - receivedCRC)); |
505 | throw new IOException("CRC mismatch! Calculated crc: " + (int)crc + "; received check crc: " + receivedCRC + ", difference: " + Math.abs(crc - receivedCRC)); |
506 | } |
506 | } |
507 | if (read() != '\r') { |
507 | if (read() != '\r') { |
508 | throw new IOException("CR at end of frame missing"); |
508 | throw new IOException("CR at end of frame missing"); |
509 | } |
509 | } |
510 | 510 | ||
511 | return result; |
511 | return result; |
512 | } |
512 | } |
513 | } |
513 | } |
514 | 514 |