Subversion Repositories Projects

Rev

Rev 1539 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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