Subversion Repositories Projects

Rev

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