Subversion Repositories Projects

Rev

Rev 1696 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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