Subversion Repositories Projects

Rev

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