Subversion Repositories Projects

Rev

Rev 1695 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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