Subversion Repositories Projects

Rev

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