Subversion Repositories Projects

Rev

Rev 1539 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1539 Rev 1559
Line 9... Line 9...
9
import dongfang.mkt.frames.AttitudeDataResponseFrame;
9
import dongfang.mkt.frames.AttitudeDataResponseFrame;
10
import dongfang.mkt.frames.ChangeParameterSetResponseFrame;
10
import dongfang.mkt.frames.ChangeParameterSetResponseFrame;
11
import dongfang.mkt.frames.ConfirmFrame;
11
import dongfang.mkt.frames.ConfirmFrame;
12
import dongfang.mkt.frames.DebugResponseFrame;
12
import dongfang.mkt.frames.DebugResponseFrame;
13
import dongfang.mkt.frames.MotorTestResponseFrame;
13
import dongfang.mkt.frames.MotorTestResponseFrame;
-
 
14
import dongfang.mkt.frames.OSDDataResponseFrame;
-
 
15
import dongfang.mkt.frames.OSDDataResponseFrame.GPSDistanceAndBearing;
-
 
16
import dongfang.mkt.frames.OSDDataResponseFrame.GPSPosition;
-
 
17
import dongfang.mkt.frames.ReadExternalControlResponseFrame;
14
import dongfang.mkt.frames.ResponseFrame;
18
import dongfang.mkt.frames.ResponseFrame;
15
import dongfang.mkt.frames.SetCompassHeadingResponseFrame;
19
import dongfang.mkt.frames.SetCompassHeadingResponseFrame;
16
import dongfang.mkt.frames.UniversalReadParamSetResponseFrame;
20
import dongfang.mkt.frames.UniversalReadParamSetResponseFrame;
17
import dongfang.mkt.frames.UniversalWriteParamSetResponseFrame;
21
import dongfang.mkt.frames.UniversalWriteParamSetResponseFrame;
18
import dongfang.mkt.frames.VariablesResponseFrame;
22
import dongfang.mkt.frames.VariablesResponseFrame;
19
import dongfang.mkt.frames.VersionResponseFrame;
23
import dongfang.mkt.frames.VersionResponseFrame;
20
import dongfang.mkt.main.UniversalConfigurator;
-
 
21
 
24
 
22
public class MKInputStream extends InputStream {
25
public class MKInputStream extends InputStream {
23
        int readByteCnt;
26
        int readByteCnt;
24
        class MKDataInputStream {
27
        class MKDataInputStream {
25
                int[] inbuf = new int[4];
28
                int[] inbuf = new int[4];
Line 51... Line 54...
51
                        if (outbufptr > 2 && !decode())
54
                        if (outbufptr > 2 && !decode())
52
                                        throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!");            
55
                                        throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!");            
53
                        return outbuf[outbufptr++];
56
                        return outbuf[outbufptr++];
54
                }
57
                }
55
 
58
 
-
 
59
                public int readSignedByte() throws IOException {
-
 
60
                        byte result = (byte)readByte();
-
 
61
                        return result;
-
 
62
                }
-
 
63
 
56
                public int readWord() throws IOException {
64
                public int readWord() throws IOException {
57
                        int byte2 = readByte();
65
                        int byte0 = readByte();
58
                        int byte1 = readByte();
66
                        int byte1 = readByte();
59
                        return (byte1 << 8) | byte2;
67
                        return (byte1 << 8) | byte0;
60
                }
68
                }
61
 
69
 
62
                public int readSignedWord() throws IOException {
70
                public int readSignedWord() throws IOException {
63
                        int word = readWord();
71
                        int word = readWord();
64
                        if (word > 32767)
72
                        if (word > 32767)
65
                                word = word - 65536;
73
                                word = word - 65536;
66
                        return word;
74
                        return word;
67
                }
75
                }
-
 
76
               
-
 
77
                public int readSignedDWord() throws IOException {
-
 
78
                        int byte0 = readByte();
-
 
79
                        int byte1 = readByte();
-
 
80
                        int byte2 = readByte();
-
 
81
                        int byte3 = readByte();
-
 
82
                        return (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0;
-
 
83
                }
68
 
84
 
69
                public int[] readBytes(int length) throws IOException {
85
                public int[] readBytes(int length) throws IOException {
70
                        int[] result = new int[length];
86
                        int[] result = new int[length];
71
                        for (int i = 0; i < length; i++) {
87
                        for (int i = 0; i < length; i++) {
72
                                result[i] = readByte();
88
                                result[i] = readByte();
Line 92... Line 108...
92
                        }
108
                        }
93
                        return result;
109
                        return result;
94
                }
110
                }
95
        }
111
        }
96
 
112
 
97
        MKDataInputStream dis = new MKDataInputStream();
113
        MKDataInputStream base64InputStream = new MKDataInputStream();
98
        OutputStream nonPacketSpillway = null; //System.err;
114
        OutputStream nonPacketSpillway = null; //System.err;
99
       
115
       
100
        final InputStream is;
116
        final InputStream is;
101
        int crc;
117
        int crc;
102
 
118
 
Line 119... Line 135...
119
                crc += _byte;
135
                crc += _byte;
120
                return _byte;
136
                return _byte;
121
        }
137
        }
122
 
138
 
123
        public MKDataInputStream getBase64InputStream() {
139
        public MKDataInputStream getBase64InputStream() {
124
                return dis;
140
                return base64InputStream;
125
        }
141
        }
126
 
142
 
127
        public ResponseFrame getNextFrame() throws IOException {
143
        public ResponseFrame getNextFrame() throws IOException {
128
                int c;
144
                int c;
129
                while ((c = read()) != '#') {
145
                while ((c = read()) != '#') {
130
                        // throw it on some scrap-text buffer.
146
                        // throw it on some scrap-text buffer.
131
                        if (nonPacketSpillway != null)
147
                        if (nonPacketSpillway != null)
132
                                nonPacketSpillway.write(c);
148
                                nonPacketSpillway.write(c);
133
                }
149
                }
134
                crc = '#';
150
                crc = '#';
135
                dis.reset();
151
                base64InputStream.reset();
136
                int address = readByte() - 'a';
152
                int address = readByte() - 'a';
137
                int iid = readByte();
153
                int iid = readByte();
138
                readByteCnt = 0;
154
                readByteCnt = 0;
139
                //RESPONSE_IDS id = getResponseType(iid);
155
                //RESPONSE_IDS id = getResponseType(iid);
140
                ResponseFrame result;
156
                ResponseFrame result;
141
                switch (iid) {
157
                switch (iid) {
142
                case 'A': {
158
                case 'A': {
143
                        AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address);
159
                        AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address);
144
                        f.setChannel(getBase64InputStream().readByte());
160
                        f.setChannel(base64InputStream.readByte());
145
                        f.setLabel(getBase64InputStream().readChars(16));
161
                        f.setLabel(base64InputStream.readChars(16));
146
                        result = f;
162
                        result = f;
147
                        break;
163
                        break;
148
                }
164
                }
149
                case 'B': {
165
                case 'B': {
150
                        ConfirmFrame f = new ConfirmFrame(address);
166
                        ConfirmFrame f = new ConfirmFrame(address);
151
                        f.setFrameNum(getBase64InputStream().readByte());
167
                        f.setFrameNum(base64InputStream.readByte());
152
                        result = f;
168
                        result = f;
153
                        break;
169
                        break;
154
                }
170
                }
155
                case 'C': {
171
                case 'C': {
156
                        AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address);
172
                        AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address);
157
                        f.setPitch(getBase64InputStream().readSignedWord());
173
                        f.setPitch(base64InputStream.readSignedWord());
158
                        f.setRoll(getBase64InputStream().readSignedWord());
174
                        f.setRoll(base64InputStream.readSignedWord());
159
                        f.setHeading(getBase64InputStream().readSignedWord());
175
                        f.setHeading(base64InputStream.readSignedWord());
160
                        f.setExpansion(getBase64InputStream().readBytes(8));
176
                        f.setExpansion(base64InputStream.readBytes(8));
161
                        result = f;
177
                        result = f;
162
                        break;
178
                        break;
163
                }
179
                }
164
                case 'D': {
180
                case 'D': {
165
                        DebugResponseFrame f = new DebugResponseFrame(address);
181
                        DebugResponseFrame f = new DebugResponseFrame(address);
166
                        for (int i=0; i<2; i++)
182
                        for (int i=0; i<2; i++)
167
                                f.setDigital(i, getBase64InputStream().readByte());
183
                                f.setDigital(i, base64InputStream.readByte());
168
                        for (int i=0; i<32; i++)
184
                        for (int i=0; i<32; i++)
169
                                f.setAnalog(i, getBase64InputStream().readSignedWord());
185
                                f.setAnalog(i, base64InputStream.readSignedWord());
170
                        result = f;
186
                        result = f;
171
                        break;
187
                        break;
172
                }
188
                }
173
                case 'F': {
189
                case 'F': {
174
                        ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);
190
                        ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);
175
                        f.setParameterSetNumber(getBase64InputStream().readByte());
191
                        f.setParameterSetNumber(base64InputStream.readByte());
-
 
192
                        result = f;
-
 
193
                        break;
-
 
194
                }
-
 
195
                case 'G': {
-
 
196
                        ReadExternalControlResponseFrame f = new ReadExternalControlResponseFrame(address);
-
 
197
                        f.setDigital(base64InputStream.readBytes(2));
-
 
198
                        f.setRemoteButtons(base64InputStream.readByte());
-
 
199
                        f.setPitch(base64InputStream.readByte());
-
 
200
                        f.setRoll(base64InputStream.readByte());
-
 
201
                        f.setYaw(base64InputStream.readByte());
-
 
202
                        f.setThrottle(base64InputStream.readByte());
-
 
203
                        f.setHeight(base64InputStream.readByte());
-
 
204
                        f.setCommand(base64InputStream.readByte());
-
 
205
                        f.setFrameNum(base64InputStream.readByte());
-
 
206
                        f.setArgument(base64InputStream.readByte());
176
                        result = f;
207
                        result = f;
177
                        break;
208
                        break;
178
                }
209
                }
179
                case 'H': {
210
                case 'H': {
180
                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
211
                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
181
                        f.setLine(getBase64InputStream().readByte());
212
                        f.setLine(base64InputStream.readByte());
182
                        //f.setMaxItem(getDataInputStream().readByte());
213
                        //f.setMaxItem(getDataInputStream().readByte());
183
                        f.setText(getBase64InputStream().readChars(20));
214
                        f.setText(base64InputStream.readChars(20));
184
                        result = f;
215
                        result = f;
185
                        break;
216
                        break;
186
                }
217
                }
187
                case 'L': {
218
                case 'L': {
188
                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
219
                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
189
                        f.setItem(getBase64InputStream().readByte());
220
                        f.setItem(base64InputStream.readByte());
190
                        // f.setMaxItem(getDataInputStream().readByte());
221
                        // f.setMaxItem(getDataInputStream().readByte());
191
                        f.setText(getBase64InputStream().readChars(80));
222
                        f.setText(base64InputStream.readChars(80));
192
                        result = f;
223
                        result = f;
193
                        break;
224
                        break;
194
                }
225
                }
-
 
226
                case 'O': {
-
 
227
                        OSDDataResponseFrame f = new OSDDataResponseFrame(address);
-
 
228
                        f.setVersion(base64InputStream.readByte());
-
 
229
                       
-
 
230
                        GPSPosition pos = new GPSPosition();
-
 
231
                        pos.setLongitude(base64InputStream.readSignedDWord());
-
 
232
                        pos.setLatitude(base64InputStream.readSignedDWord());
-
 
233
                        pos.setAltitude(base64InputStream.readSignedDWord());
-
 
234
                        pos.setStatus(base64InputStream.readByte());
-
 
235
                        f.setCurrentPosition(pos);
-
 
236
                       
-
 
237
                        pos = new GPSPosition();
-
 
238
                        pos.setLongitude(base64InputStream.readSignedDWord());
-
 
239
                        pos.setLatitude(base64InputStream.readSignedDWord());
-
 
240
                        pos.setAltitude(base64InputStream.readSignedDWord());
-
 
241
                        pos.setStatus(base64InputStream.readByte());
-
 
242
                        f.setTargetPosition(pos);
-
 
243
                       
-
 
244
                        GPSDistanceAndBearing rnb = new GPSDistanceAndBearing();
-
 
245
                        rnb.setDistance(base64InputStream.readWord());
-
 
246
                        rnb.setBearing(base64InputStream.readSignedWord());
-
 
247
                        f.setCurrentToTarget(rnb);
-
 
248
 
-
 
249
                        pos = new GPSPosition();
-
 
250
                        pos.setLongitude(base64InputStream.readSignedDWord());
-
 
251
                        pos.setLatitude(base64InputStream.readSignedDWord());
-
 
252
                        pos.setAltitude(base64InputStream.readSignedDWord());
-
 
253
                        pos.setStatus(base64InputStream.readByte());
-
 
254
                        f.setHomePosition(pos);
-
 
255
                       
-
 
256
                        rnb = new GPSDistanceAndBearing();
-
 
257
                        rnb.setDistance(base64InputStream.readWord());
-
 
258
                        rnb.setBearing(base64InputStream.readSignedWord());
-
 
259
                        f.setCurrentToHome(rnb);
-
 
260
                       
-
 
261
                        f.setWaypointIndex(base64InputStream.readByte());
-
 
262
                        f.setWaypointCount(base64InputStream.readByte());
-
 
263
                        f.setNumberOfSatellites(base64InputStream.readByte());
-
 
264
                       
-
 
265
                        f.setHeightByPressure(base64InputStream.readSignedWord());
-
 
266
                        f.setVerticalVelocityByPressure(base64InputStream.readSignedWord());
-
 
267
                        f.setFlightTime(base64InputStream.readWord());
-
 
268
                        f.setBatteryVoltage(base64InputStream.readByte());
-
 
269
                        f.setGroundSpeed(base64InputStream.readWord());
-
 
270
                       
-
 
271
                        f.setHeading(base64InputStream.readSignedWord());
-
 
272
                        f.setCompassHeading(base64InputStream.readSignedWord());
-
 
273
                       
-
 
274
                        f.setPitchAngle(base64InputStream.readSignedByte());
-
 
275
                        f.setRollAngle(base64InputStream.readSignedByte());
-
 
276
                       
-
 
277
                        f.setRcQuality(base64InputStream.readByte());
-
 
278
                        f.setFcFlags(base64InputStream.readByte());
-
 
279
                        f.setNcFlags(base64InputStream.readByte());
-
 
280
                        f.setErrorCode(base64InputStream.readByte());
-
 
281
                        f.setOperatingRadius(base64InputStream.readByte());
-
 
282
                       
-
 
283
                        f.setVerticalVelocityByGPS(base64InputStream.readSignedWord());
-
 
284
                        f.setTargetLoiterTime(base64InputStream.readByte());
-
 
285
                        f.setFcFlags2(base64InputStream.readByte());
-
 
286
                        f.setSetpointForAltitude(base64InputStream.readSignedWord());
-
 
287
                        f.setThrottle(base64InputStream.readByte());
-
 
288
                        f.setCurrent(base64InputStream.readWord());
-
 
289
                        f.setCapacityUsed(base64InputStream.readWord());
-
 
290
                        result = f;
-
 
291
                        break;
-
 
292
}
195
                case 'S': {
293
                case 'S': {
196
                        UniversalWriteParamSetResponseFrame f = new UniversalWriteParamSetResponseFrame(address);
294
                        UniversalWriteParamSetResponseFrame f = new UniversalWriteParamSetResponseFrame(address);
197
                        f.setParameterSetNumber(getBase64InputStream().readByte());
295
                        f.setParameterSetNumber(base64InputStream.readByte());
198
                        result = f;
296
                        result = f;
199
                        break;
297
                        break;
200
                }
298
                }
201
                case 'T': {
299
                case 'T': {
202
                        MotorTestResponseFrame f = new MotorTestResponseFrame(address);
300
                        MotorTestResponseFrame f = new MotorTestResponseFrame(address);
Line 214... Line 312...
214
                        break;
312
                        break;
215
                }
313
                }
216
            */
314
            */
217
                case 'V': {
315
                case 'V': {
218
                        VersionResponseFrame f = new VersionResponseFrame(address);
316
                        VersionResponseFrame f = new VersionResponseFrame(address);
219
                        f.setSWMajor(getBase64InputStream().readByte());
317
                        f.setSWMajor(base64InputStream.readByte());
220
                        f.setSWMinor(getBase64InputStream().readByte());
318
                        f.setSWMinor(base64InputStream.readByte());
221
                        f.setProtoMajor(getBase64InputStream().readByte());
319
                        f.setProtoMajor(base64InputStream.readByte());
222
                        f.setProtoMinor(getBase64InputStream().readByte());
320
                        f.setProtoMinor(base64InputStream.readByte());
223
                        f.setSWPatch(getBase64InputStream().readByte());
321
                        f.setSWPatch(base64InputStream.readByte());
224
                        f.setHardwareErrors(getBase64InputStream().readBytes(5));
322
                        f.setHardwareErrors(base64InputStream.readBytes(5));
225
                        result = f;
323
                        result = f;
226
                        break;
324
                        break;
227
                }
325
                }
228
               
326
               
229
                // This is my own creation. The ID collides with the waypoint one of FC.
327
                // This is my own creation. The ID collides with the waypoint one of FC.
230
                case 'X': {
328
                case 'X': {
231
                        VariablesResponseFrame f = new VariablesResponseFrame(address);
329
                        VariablesResponseFrame f = new VariablesResponseFrame(address);
232
                        f.setVariables(getBase64InputStream().readWords(8));
330
                        f.setVariables(base64InputStream.readWords(8));
233
                        result = f;
331
                        result = f;
234
                        break;
332
                        break;
235
                }
333
                }
236
                case 'w': {
334
                case 'w': {
237
                        SetCompassHeadingResponseFrame f = new SetCompassHeadingResponseFrame(address);
335
                        SetCompassHeadingResponseFrame f = new SetCompassHeadingResponseFrame(address);
Line 242... Line 340...
242
                        ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
340
                        ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];
243
                        ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
341
                        ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];
244
                        ToMk3Mag.CalState = compassCalState;
342
                        ToMk3Mag.CalState = compassCalState;
245
                        */
343
                        */
246
                        // Waste 8 bytes to make CRC match.
344
                        // Waste 8 bytes to make CRC match.
247
                        getBase64InputStream().readBytes(8);
345
                        base64InputStream.readBytes(8);
248
                        result = f;
346
                        result = f;
249
                        break;
347
                        break;
250
                }
348
                }
251
                case 'Q':
349
                case 'Q':
252
                        UniversalReadParamSetResponseFrame f = new UniversalReadParamSetResponseFrame(address);
350
                        UniversalReadParamSetResponseFrame f = new UniversalReadParamSetResponseFrame(address);
253
                        f.setConfigurationSetNumber(getBase64InputStream().readByte());
351
                        f.setConfigurationSetNumber(base64InputStream.readByte());
254
                        f.setConfigurationVersion(getBase64InputStream().readByte());
352
                        f.setConfigurationVersion(base64InputStream.readByte());
255
                        int length = getBase64InputStream().readByte();
353
                        int length = base64InputStream.readByte();
256
                        f.setData(getBase64InputStream().readBytes(length));
354
                        f.setData(base64InputStream.readBytes(length));
257
                        result = f;
355
                        result = f;
258
                        break;
356
                        break;
259
                default:
357
                default:
260
                        result = null;
358
                        result = null;
261
                }
359
                }