Rev 1611 | Rev 1689 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1611 | Rev 1688 | ||
---|---|---|---|
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.OutputStream; |
4 | import java.io.OutputStream; |
5 | 5 | ||
6 | import dongfang.mkt.RequestFrameVisitor; |
6 | import dongfang.mkt.RequestFrameVisitor; |
7 | import dongfang.mkt.frames.AllDisplaysRequestFrame; |
7 | import dongfang.mkt.frames.AllDisplaysRequestFrame; |
8 | import dongfang.mkt.frames.AnalogDebugLabelRequestFrame; |
8 | import dongfang.mkt.frames.AnalogDebugLabelRequestFrame; |
9 | import dongfang.mkt.frames.AttitudeDataRequestFrame; |
9 | import dongfang.mkt.frames.AttitudeDataRequestFrame; |
10 | import dongfang.mkt.frames.ChangeParameterSetRequestFrame; |
10 | import dongfang.mkt.frames.ChangeParameterSetRequestFrame; |
11 | import dongfang.mkt.frames.CompassHeadingRequestFrame; |
11 | import dongfang.mkt.frames.CompassHeadingRequestFrame; |
12 | import dongfang.mkt.frames.DebugRequestFrame; |
12 | import dongfang.mkt.frames.DebugRequestFrame; |
13 | import dongfang.mkt.frames.ExternalControlRequestFrame; |
13 | import dongfang.mkt.frames.ExternalControlRequestFrame; |
14 | import dongfang.mkt.frames.LoopbackTestRequestFrame; |
14 | import dongfang.mkt.frames.LoopbackTestRequestFrame; |
15 | import dongfang.mkt.frames.MotorTestRequestFrame; |
15 | import dongfang.mkt.frames.MotorTestRequestFrame; |
16 | import dongfang.mkt.frames.OSDDataRequestFrame; |
16 | import dongfang.mkt.frames.OSDDataRequestFrame; |
17 | import dongfang.mkt.frames.ReadExternalControlRequestFrame; |
17 | import dongfang.mkt.frames.ReadExternalControlRequestFrame; |
18 | import dongfang.mkt.frames.ReadIMUConfigurationRequestFrame; |
18 | import dongfang.mkt.frames.ReadIMUConfigurationRequestFrame; |
- | 19 | import dongfang.mkt.frames.ReadMotorMixerRequestFrame; |
|
19 | import dongfang.mkt.frames.ReadParamSetRequestFrame; |
20 | import dongfang.mkt.frames.ReadParamSetRequestFrame; |
- | 21 | import dongfang.mkt.frames.ReadVariablesRequestFrame; |
|
20 | import dongfang.mkt.frames.RequestFrame; |
22 | import dongfang.mkt.frames.RequestFrame; |
21 | import dongfang.mkt.frames.ResetRequestFrame; |
23 | import dongfang.mkt.frames.ResetRequestFrame; |
22 | import dongfang.mkt.frames.SetCompassHeadingRequestFrame; |
24 | import dongfang.mkt.frames.SetCompassHeadingRequestFrame; |
23 | import dongfang.mkt.frames.SingleDisplayRequestFrame; |
25 | import dongfang.mkt.frames.SingleDisplayRequestFrame; |
24 | import dongfang.mkt.frames.VariablesRequestFrame; |
- | |
25 | import dongfang.mkt.frames.VersionRequestFrame; |
26 | import dongfang.mkt.frames.VersionRequestFrame; |
26 | import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame; |
27 | import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame; |
- | 28 | import dongfang.mkt.frames.WriteMotorMixerRequestFrame; |
|
27 | import dongfang.mkt.frames.WriteParamSetRequestFrame; |
29 | import dongfang.mkt.frames.WriteParamSetRequestFrame; |
28 | 30 | ||
29 | public class MKOutputStream extends OutputStream implements RequestFrameVisitor { |
31 | public class MKOutputStream extends OutputStream implements RequestFrameVisitor { |
30 | public class MKDataOutputStream { |
32 | public class MKDataOutputStream { |
31 | int[] inbuf = new int[3]; |
33 | int[] inbuf = new int[3]; |
32 | int[] outbuf = new int[4]; |
34 | int[] outbuf = new int[4]; |
33 | int inbufptr = 0; |
35 | int inbufptr = 0; |
34 | 36 | ||
35 | void writeByte(int b) throws IOException { |
37 | void writeByte(int b) throws IOException { |
36 | if(inbufptr == inbuf.length) flush(); |
38 | if(inbufptr == inbuf.length) flush(); |
37 | inbuf[inbufptr++] = b; |
39 | inbuf[inbufptr++] = b; |
38 | } |
40 | } |
39 | 41 | ||
40 | public void writeBytes(int[] bs) throws IOException { |
42 | public void writeBytes(int[] bs) throws IOException { |
41 | for (int i=0; i<bs.length; i++) |
43 | for (int i=0; i<bs.length; i++) |
42 | writeByte(bs[i]); |
44 | writeByte(bs[i]); |
43 | } |
45 | } |
44 | 46 | ||
45 | void writeWord(int w) throws IOException { |
47 | void writeWord(int w) throws IOException { |
46 | writeByte(w & 0xff); |
48 | writeByte(w & 0xff); |
47 | writeByte(w >>> 8); |
49 | writeByte(w >>> 8); |
48 | } |
50 | } |
49 | 51 | ||
50 | void writeChars(char[] s) throws IOException { |
52 | void writeChars(char[] s) throws IOException { |
51 | for (int i=0; i<s.length; i++) { |
53 | for (int i=0; i<s.length; i++) { |
52 | // Here, a 1:1 mapping between byte values and char codes is assumed. |
54 | // Here, a 1:1 mapping between byte values and char codes is assumed. |
53 | // That means we're assuming ISO-8859-1 (= the first 256 code points |
55 | // That means we're assuming ISO-8859-1 (= the first 256 code points |
54 | // of Unicode, which Java uses for chars) |
56 | // of Unicode, which Java uses for chars) |
55 | writeByte(s[i]); |
57 | writeByte(s[i]); |
56 | } |
58 | } |
57 | } |
59 | } |
58 | 60 | ||
59 | void flush() throws IOException { |
61 | void flush() throws IOException { |
60 | if (inbufptr == 0) |
62 | if (inbufptr == 0) |
61 | return; |
63 | return; |
62 | 64 | ||
63 | while(inbufptr < inbuf.length) { |
65 | while(inbufptr < inbuf.length) { |
64 | // add padding .. well just clear it, for tidyness. |
66 | // add padding .. well just clear it, for tidyness. |
65 | inbuf[inbufptr++] = 0; |
67 | inbuf[inbufptr++] = 0; |
66 | } |
68 | } |
67 | 69 | ||
68 | MKOutputStream.this.writeByte((inbuf[0] >>> 2) + '='); |
70 | MKOutputStream.this.writeByte((inbuf[0] >>> 2) + '='); |
69 | MKOutputStream.this.writeByte(( ((inbuf[0] & 0x03) << 4) | (inbuf[1] >>> 4) ) + '='); |
71 | MKOutputStream.this.writeByte(( ((inbuf[0] & 0x03) << 4) | (inbuf[1] >>> 4) ) + '='); |
70 | MKOutputStream.this.writeByte(( ((inbuf[1] & 0x0f) << 2) | (inbuf[2] >>> 6)) + '='); |
72 | MKOutputStream.this.writeByte(( ((inbuf[1] & 0x0f) << 2) | (inbuf[2] >>> 6)) + '='); |
71 | MKOutputStream.this.writeByte(((inbuf[2] & 0x3f) + '=')); |
73 | MKOutputStream.this.writeByte(((inbuf[2] & 0x3f) + '=')); |
72 | 74 | ||
73 | inbufptr = 0; |
75 | inbufptr = 0; |
74 | } |
76 | } |
75 | } |
77 | } |
76 | 78 | ||
77 | final OutputStream os; |
79 | final OutputStream os; |
78 | MKDataOutputStream base64OutputStream = new MKDataOutputStream(); |
80 | MKDataOutputStream base64OutputStream = new MKDataOutputStream(); |
79 | int crc; |
81 | int crc; |
80 | 82 | ||
81 | public MKOutputStream(OutputStream os) { |
83 | public MKOutputStream(OutputStream os) { |
82 | this.os = os; |
84 | this.os = os; |
83 | } |
85 | } |
84 | 86 | ||
85 | @Override |
87 | @Override |
86 | public void write(int b) throws IOException { |
88 | public void write(int b) throws IOException { |
87 | os.write(b); |
89 | os.write(b); |
88 | // System.out.println("Writing: " + b); |
90 | // System.out.println("Writing: " + b); |
89 | } |
91 | } |
90 | 92 | ||
91 | public void writeByte(int b) throws IOException { |
93 | public void writeByte(int b) throws IOException { |
92 | crc += b; |
94 | crc += b; |
93 | write(b); |
95 | write(b); |
94 | } |
96 | } |
95 | 97 | ||
96 | public void write(RequestFrame f) throws IOException { |
98 | public void write(RequestFrame f) throws IOException { |
97 | write(crc = '#'); |
99 | write(crc = '#'); |
98 | 100 | ||
99 | int address = f.getAddress() + 'a'; |
101 | int address = f.getAddress() + 'a'; |
100 | writeByte(address); |
102 | writeByte(address); |
101 | 103 | ||
102 | // Will cause one of the "visit" methods below |
104 | // Will cause one of the "visit" methods below |
103 | // to be called, depending on the type of f. |
105 | // to be called, depending on the type of f. |
104 | f.accept(this); |
106 | f.accept(this); |
105 | base64OutputStream.flush(); |
107 | base64OutputStream.flush(); |
106 | 108 | ||
107 | crc %= 4096; |
109 | crc %= 4096; |
108 | write((crc >>> 6) + '='); |
110 | write((crc >>> 6) + '='); |
109 | write((crc & 0x3f) + '='); |
111 | write((crc & 0x3f) + '='); |
110 | 112 | ||
111 | write('\r'); |
113 | write('\r'); |
112 | } |
114 | } |
113 | 115 | ||
114 | /* |
116 | /* |
115 | public void visit(RequestFrame f) { |
117 | public void visit(RequestFrame f) { |
116 | throw new RuntimeException("Unbound RequestFrame type: " |
118 | throw new RuntimeException("Unbound RequestFrame type: " |
117 | + f.getClass().getSimpleName() |
119 | + f.getClass().getSimpleName() |
118 | + ". Don't know how to output."); |
120 | + ". Don't know how to output."); |
119 | } |
121 | } |
120 | */ |
122 | */ |
121 | 123 | ||
122 | public void visit(AnalogDebugLabelRequestFrame f) throws IOException { |
124 | public void visit(AnalogDebugLabelRequestFrame f) throws IOException { |
123 | writeByte('a'); |
125 | writeByte('a'); |
124 | base64OutputStream.writeByte(f.getChannel()); |
126 | base64OutputStream.writeByte(f.getChannel()); |
125 | } |
127 | } |
126 | 128 | ||
127 | public void visit(AttitudeDataRequestFrame f) throws IOException { |
129 | public void visit(AttitudeDataRequestFrame f) throws IOException { |
128 | writeByte('c'); |
130 | writeByte('c'); |
129 | base64OutputStream.writeByte(f.getAutoSendInterval()); |
131 | base64OutputStream.writeByte(f.getAutoSendInterval()); |
130 | } |
132 | } |
131 | 133 | ||
132 | public void visit(DebugRequestFrame f) throws IOException { |
134 | public void visit(DebugRequestFrame f) throws IOException { |
133 | writeByte('d'); |
135 | writeByte('d'); |
134 | base64OutputStream.writeByte(f.getAutoSendInterval()); |
136 | base64OutputStream.writeByte(f.getAutoSendInterval()); |
135 | } |
137 | } |
136 | 138 | ||
137 | public void visit(ChangeParameterSetRequestFrame f) throws IOException { |
139 | public void visit(ChangeParameterSetRequestFrame f) throws IOException { |
138 | writeByte('f'); |
140 | writeByte('f'); |
139 | base64OutputStream.writeByte(f.getParameterSetNumber()); |
141 | base64OutputStream.writeByte(f.getParameterSetNumber()); |
140 | } |
142 | } |
141 | 143 | ||
142 | public void visit(VersionRequestFrame f) throws IOException { |
144 | public void visit(VersionRequestFrame f) throws IOException { |
143 | writeByte('v'); |
145 | writeByte('v'); |
144 | } |
146 | } |
145 | 147 | ||
146 | public void visit(ResetRequestFrame f) throws IOException { |
148 | public void visit(ResetRequestFrame f) throws IOException { |
147 | writeByte('R'); |
149 | writeByte('R'); |
148 | } |
150 | } |
149 | 151 | ||
150 | public void visit(MotorTestRequestFrame f) throws IOException { |
152 | public void visit(MotorTestRequestFrame f) throws IOException { |
151 | writeByte('t'); |
153 | writeByte('t'); |
152 | base64OutputStream.writeBytes(f.getMotorValues()); |
154 | base64OutputStream.writeBytes(f.getMotorValues()); |
153 | } |
155 | } |
154 | 156 | ||
155 | public void visit(SingleDisplayRequestFrame f) throws IOException { |
157 | public void visit(SingleDisplayRequestFrame f) throws IOException { |
156 | writeByte('l'); |
158 | writeByte('l'); |
157 | base64OutputStream.writeByte(f.getMenuItemCode()); // In 0.74 there is not the all in one mode. |
159 | base64OutputStream.writeByte(f.getMenuItemCode()); // In 0.74 there is not the all in one mode. |
158 | } |
160 | } |
159 | 161 | ||
160 | public void visit(AllDisplaysRequestFrame f) throws IOException { |
162 | public void visit(AllDisplaysRequestFrame f) throws IOException { |
161 | writeByte('h'); |
163 | writeByte('h'); |
162 | base64OutputStream.writeByte(f.getPageOrder().getRemoteKeys()); |
164 | base64OutputStream.writeByte(f.getPageOrder().getRemoteKeys()); |
163 | // mdo.writeByte(f.getAutoSendInterval()); |
165 | // mdo.writeByte(f.getAutoSendInterval()); |
164 | } |
166 | } |
- | 167 | ||
- | 168 | public void visit(ReadMotorMixerRequestFrame f) throws IOException { |
|
- | 169 | writeByte('n'); |
|
- | 170 | } |
|
- | 171 | ||
- | 172 | public void visit(WriteMotorMixerRequestFrame f) throws IOException { |
|
- | 173 | writeByte('m'); |
|
- | 174 | writeByte(f.getConfigurationVersion()); |
|
- | 175 | writeByte(f.getDataLength()); |
|
- | 176 | for(int i=0; i<f.getMatrix().length; i++) { |
|
- | 177 | int[] row = f.getMatrix()[i]; |
|
- | 178 | for(int j=0; j<row.length; j++) { |
|
- | 179 | base64OutputStream.writeByte(row[j]); |
|
- | 180 | } |
|
- | 181 | } |
|
- | 182 | /* |
|
- | 183 | for(int i=0; i<f.getOppositeMotors().length; i++) { |
|
- | 184 | base64OutputStream.writeByte(f.getOppositeMotors()[i]); |
|
- | 185 | } |
|
- | 186 | */ |
|
- | 187 | } |
|
165 | 188 | ||
166 | public void visit(VariablesRequestFrame f) throws IOException { |
189 | public void visit(ReadVariablesRequestFrame f) throws IOException { |
167 | writeByte('x'); |
190 | writeByte('x'); |
168 | } |
191 | } |
169 | 192 | ||
170 | public void visit(ExternalControlRequestFrame f) throws IOException { |
193 | public void visit(ExternalControlRequestFrame f) throws IOException { |
171 | writeByte('b'); |
194 | writeByte('b'); |
172 | base64OutputStream.writeByte(f.getDigital()[0]); |
195 | base64OutputStream.writeByte(f.getDigital()[0]); |
173 | base64OutputStream.writeByte(f.getDigital()[1]); |
196 | base64OutputStream.writeByte(f.getDigital()[1]); |
174 | base64OutputStream.writeByte(f.getRemoteButtons()); |
197 | base64OutputStream.writeByte(f.getRemoteButtons()); |
175 | base64OutputStream.writeByte(f.getPitch()); |
198 | base64OutputStream.writeByte(f.getPitch()); |
176 | base64OutputStream.writeByte(f.getRoll()); |
199 | base64OutputStream.writeByte(f.getRoll()); |
177 | base64OutputStream.writeByte(f.getYaw()); |
200 | base64OutputStream.writeByte(f.getYaw()); |
178 | base64OutputStream.writeByte(f.getThrottle()); |
201 | base64OutputStream.writeByte(f.getThrottle()); |
179 | base64OutputStream.writeByte(f.getHeight()); |
202 | base64OutputStream.writeByte(f.getHeight()); |
180 | base64OutputStream.writeByte(f.getCommand()); |
203 | base64OutputStream.writeByte(f.getCommand()); |
181 | base64OutputStream.writeByte(f.getFrameNum()); |
204 | base64OutputStream.writeByte(f.getFrameNum()); |
182 | base64OutputStream.writeByte(f.getArgument()); |
205 | base64OutputStream.writeByte(f.getArgument()); |
183 | } |
206 | } |
184 | 207 | ||
185 | public void visit(ReadExternalControlRequestFrame f) throws IOException { |
208 | public void visit(ReadExternalControlRequestFrame f) throws IOException { |
186 | writeByte('g'); |
209 | writeByte('g'); |
187 | } |
210 | } |
188 | 211 | ||
189 | public void visit(LoopbackTestRequestFrame f) throws IOException { |
212 | public void visit(LoopbackTestRequestFrame f) throws IOException { |
190 | writeByte('0'); |
213 | writeByte('0'); |
191 | base64OutputStream.writeByte(f.getByte()); |
214 | base64OutputStream.writeByte(f.getByte()); |
192 | base64OutputStream.writeWord(f.getWord()); |
215 | base64OutputStream.writeWord(f.getWord()); |
193 | base64OutputStream.writeChars(f.getChararray()); |
216 | base64OutputStream.writeChars(f.getChararray()); |
194 | } |
217 | } |
195 | 218 | ||
196 | public void visit(ReadParamSetRequestFrame f) throws IOException { |
219 | public void visit(ReadParamSetRequestFrame f) throws IOException { |
197 | writeByte('q'); |
220 | writeByte('q'); |
198 | base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
221 | base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
199 | } |
222 | } |
200 | 223 | ||
201 | public void visit(WriteParamSetRequestFrame f) throws IOException { |
224 | public void visit(WriteParamSetRequestFrame f) throws IOException { |
202 | writeByte('s'); |
225 | writeByte('s'); |
203 | base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
226 | base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
204 | base64OutputStream.writeByte(f.getConfigurationVersionNumber()); |
227 | base64OutputStream.writeByte(f.getConfigurationVersionNumber()); |
205 | base64OutputStream.writeByte(f.getDataLength() + 12); |
228 | base64OutputStream.writeByte(f.getDataLength() + 12); |
206 | base64OutputStream.writeBytes(f.getData()); |
229 | base64OutputStream.writeBytes(f.getData()); |
207 | base64OutputStream.writeChars(f.getName()); |
230 | base64OutputStream.writeChars(f.getName()); |
208 | } |
231 | } |
209 | 232 | ||
210 | public void visit(SetCompassHeadingRequestFrame f) throws IOException { |
233 | public void visit(SetCompassHeadingRequestFrame f) throws IOException { |
211 | writeByte('K'); |
234 | writeByte('K'); |
212 | } |
235 | } |
213 | 236 | ||
214 | public void visit(CompassHeadingRequestFrame f) throws IOException { |
237 | public void visit(CompassHeadingRequestFrame f) throws IOException { |
215 | writeByte('w'); |
238 | writeByte('w'); |
216 | } |
239 | } |
217 | 240 | ||
218 | public void visit(OSDDataRequestFrame f) throws IOException { |
241 | public void visit(OSDDataRequestFrame f) throws IOException { |
219 | writeByte('o'); |
242 | writeByte('o'); |
220 | base64OutputStream.writeByte(f.getAutoSendInterval()); |
243 | base64OutputStream.writeByte(f.getAutoSendInterval()); |
221 | } |
244 | } |
222 | 245 | ||
223 | public void visit(ReadIMUConfigurationRequestFrame f) throws IOException { |
246 | public void visit(ReadIMUConfigurationRequestFrame f) throws IOException { |
224 | writeByte('i'); |
247 | writeByte('i'); |
225 | } |
248 | } |
226 | 249 | ||
227 | public void visit(WriteIMUConfigurationRequestFrame f) throws IOException { |
250 | public void visit(WriteIMUConfigurationRequestFrame f) throws IOException { |
228 | writeByte('j'); |
251 | writeByte('j'); |
229 | base64OutputStream.writeByte(f.getConfigurationVersionNumber()); |
252 | base64OutputStream.writeByte(f.getConfigurationVersionNumber()); |
230 | base64OutputStream.writeByte(f.getDataLength()); |
253 | base64OutputStream.writeByte(f.getDataLength()); |
231 | base64OutputStream.writeBytes(f.getData()); |
254 | base64OutputStream.writeBytes(f.getData()); |
232 | } |
255 | } |
233 | } |
256 | } |
234 | 257 |