| 0,0 → 1,218 |
|---|
| package dongfang.mkt.comm; |
| |
| import java.io.IOException; |
| import java.io.OutputStream; |
| |
| import dongfang.mkt.RequestFrameVisitor; |
| import dongfang.mkt.frames.AllDisplaysRequestFrame; |
| import dongfang.mkt.frames.AnalogDebugLabelRequestFrame; |
| import dongfang.mkt.frames.AttitudeDataRequestFrame; |
| import dongfang.mkt.frames.ChangeParameterSetRequestFrame; |
| import dongfang.mkt.frames.CompassHeadingRequestFrame; |
| import dongfang.mkt.frames.DebugRequestFrame; |
| import dongfang.mkt.frames.ExternalControlRequestFrame; |
| import dongfang.mkt.frames.LoopbackTestRequestFrame; |
| import dongfang.mkt.frames.MotorTestRequestFrame; |
| import dongfang.mkt.frames.OSDDataRequestFrame; |
| import dongfang.mkt.frames.ReadExternalControlRequestFrame; |
| import dongfang.mkt.frames.RequestFrame; |
| import dongfang.mkt.frames.ResetRequestFrame; |
| import dongfang.mkt.frames.SetCompassHeadingRequestFrame; |
| import dongfang.mkt.frames.SingleDisplayRequestFrame; |
| import dongfang.mkt.frames.UniversalReadParamSetRequestFrame; |
| import dongfang.mkt.frames.UniversalWriteParamSetRequestFrame; |
| import dongfang.mkt.frames.VariablesRequestFrame; |
| import dongfang.mkt.frames.VersionRequestFrame; |
| |
| public class MKOutputStream extends OutputStream implements RequestFrameVisitor { |
| public class MKDataOutputStream { |
| int[] inbuf = new int[3]; |
| int[] outbuf = new int[4]; |
| int inbufptr = 0; |
| |
| void writeByte(int b) throws IOException { |
| if(inbufptr == inbuf.length) flush(); |
| inbuf[inbufptr++] = b; |
| } |
| |
| public void writeBytes(int[] bs) throws IOException { |
| for (int i=0; i<bs.length; i++) |
| writeByte(bs[i]); |
| } |
| |
| void writeWord(int w) throws IOException { |
| writeByte(w & 0xff); |
| writeByte(w >>> 8); |
| } |
| |
| void writeChars(char[] s) throws IOException { |
| for (int i=0; i<s.length; i++) { |
| // Here, a 1:1 mapping between byte values and char codes is assumed. |
| // That means we're assuming ISO-8859-1 (= the first 256 code points |
| // of Unicode, which Java uses for chars) |
| writeByte(s[i]); |
| } |
| } |
| |
| void flush() throws IOException { |
| if (inbufptr == 0) |
| return; |
| |
| while(inbufptr < inbuf.length) { |
| // add padding .. well just clear it, for tidyness. |
| inbuf[inbufptr++] = 0; |
| } |
| |
| MKOutputStream.this.writeByte((inbuf[0] >>> 2) + '='); |
| MKOutputStream.this.writeByte(( ((inbuf[0] & 0x03) << 4) | (inbuf[1] >>> 4) ) + '='); |
| MKOutputStream.this.writeByte(( ((inbuf[1] & 0x0f) << 2) | (inbuf[2] >>> 6)) + '='); |
| MKOutputStream.this.writeByte(((inbuf[2] & 0x3f) + '=')); |
| |
| inbufptr = 0; |
| } |
| } |
| |
| final OutputStream os; |
| MKDataOutputStream base64OutputStream = new MKDataOutputStream(); |
| int crc; |
| |
| public MKOutputStream(OutputStream os) { |
| this.os = os; |
| } |
| |
| @Override |
| public void write(int b) throws IOException { |
| os.write(b); |
| // System.out.println("Writing: " + b); |
| } |
| |
| public void writeByte(int b) throws IOException { |
| crc += b; |
| write(b); |
| } |
| |
| public void write(RequestFrame f) throws IOException { |
| write(crc = '#'); |
| |
| int address = f.getAddress() + 'a'; |
| writeByte(address); |
| |
| // Will cause one of the "visit" methods below |
| // to be called, depending on the type of f. |
| f.accept(this); |
| base64OutputStream.flush(); |
| |
| crc %= 4096; |
| write((crc >>> 6) + '='); |
| write((crc & 0x3f) + '='); |
| |
| write('\r'); |
| } |
| |
| /* |
| public void visit(RequestFrame f) { |
| throw new RuntimeException("Unbound RequestFrame type: " |
| + f.getClass().getSimpleName() |
| + ". Don't know how to output."); |
| } |
| */ |
| |
| public void visit(AnalogDebugLabelRequestFrame f) throws IOException { |
| writeByte('a'); |
| base64OutputStream.writeByte(f.getChannel()); |
| } |
| |
| public void visit(AttitudeDataRequestFrame f) throws IOException { |
| writeByte('c'); |
| base64OutputStream.writeByte(f.getAutoSendInterval()); |
| } |
| |
| public void visit(DebugRequestFrame f) throws IOException { |
| writeByte('d'); |
| base64OutputStream.writeByte(f.getAutoSendInterval()); |
| } |
| |
| public void visit(ChangeParameterSetRequestFrame f) throws IOException { |
| writeByte('f'); |
| base64OutputStream.writeByte(f.getParameterSetNumber()); |
| } |
| |
| public void visit(VersionRequestFrame f) throws IOException { |
| writeByte('v'); |
| } |
| |
| public void visit(ResetRequestFrame f) throws IOException { |
| writeByte('R'); |
| } |
| |
| public void visit(MotorTestRequestFrame f) throws IOException { |
| writeByte('t'); |
| base64OutputStream.writeBytes(f.getMotorValues()); |
| } |
| |
| public void visit(SingleDisplayRequestFrame f) throws IOException { |
| writeByte('l'); |
| base64OutputStream.writeByte(f.getMenuItemCode()); // In 0.74 there is not the all in one mode. |
| } |
| |
| public void visit(AllDisplaysRequestFrame f) throws IOException { |
| writeByte('h'); |
| base64OutputStream.writeByte(f.getPageOrder().getRemoteKeys()); |
| // mdo.writeByte(f.getAutoSendInterval()); |
| } |
| |
| public void visit(VariablesRequestFrame f) throws IOException { |
| writeByte('x'); |
| } |
| |
| public void visit(ExternalControlRequestFrame f) throws IOException { |
| writeByte('b'); |
| base64OutputStream.writeByte(f.getDigital()[0]); |
| base64OutputStream.writeByte(f.getDigital()[1]); |
| base64OutputStream.writeByte(f.getRemoteButtons()); |
| base64OutputStream.writeByte(f.getPitch()); |
| base64OutputStream.writeByte(f.getRoll()); |
| base64OutputStream.writeByte(f.getYaw()); |
| base64OutputStream.writeByte(f.getThrottle()); |
| base64OutputStream.writeByte(f.getHeight()); |
| base64OutputStream.writeByte(f.getCommand()); |
| base64OutputStream.writeByte(f.getFrameNum()); |
| base64OutputStream.writeByte(f.getArgument()); |
| } |
| |
| public void visit(ReadExternalControlRequestFrame f) throws IOException { |
| writeByte('g'); |
| } |
| |
| public void visit(LoopbackTestRequestFrame f) throws IOException { |
| writeByte('0'); |
| base64OutputStream.writeByte(f.getByte()); |
| base64OutputStream.writeWord(f.getWord()); |
| base64OutputStream.writeChars(f.getChararray()); |
| } |
| |
| public void visit(UniversalReadParamSetRequestFrame f) throws IOException { |
| writeByte('q'); |
| base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
| } |
| |
| public void visit(UniversalWriteParamSetRequestFrame f) throws IOException { |
| writeByte('s'); |
| base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
| base64OutputStream.writeByte(f.getConfigurationVersionNumber()); |
| base64OutputStream.writeBytes(f.getData()); |
| } |
| |
| public void visit(SetCompassHeadingRequestFrame f) throws IOException { |
| writeByte('K'); |
| } |
| |
| public void visit(CompassHeadingRequestFrame f) throws IOException { |
| writeByte('w'); |
| } |
| |
| public void visit(OSDDataRequestFrame f) throws IOException { |
| writeByte('o'); |
| base64OutputStream.writeByte(f.getAutoSendInterval()); |
| } |
| } |