Subversion Repositories Projects

Rev

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