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