Subversion Repositories Projects

Rev

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

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