Subversion Repositories Projects

Rev

Rev 1611 | Rev 1689 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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