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