Subversion Repositories Projects

Rev

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

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