Subversion Repositories Projects

Rev

Rev 1688 | Rev 1690 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

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.ReadIMUConfigurationRequestFrame;
import dongfang.mkt.frames.ReadMotorMixerRequestFrame;
import dongfang.mkt.frames.ReadParamSetRequestFrame;
import dongfang.mkt.frames.ReadVariablesRequestFrame;
import dongfang.mkt.frames.RequestFrame;
import dongfang.mkt.frames.ResetRequestFrame;
import dongfang.mkt.frames.SetCompassHeadingRequestFrame;
import dongfang.mkt.frames.SingleDisplayRequestFrame;
import dongfang.mkt.frames.VersionRequestFrame;
import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame;
import dongfang.mkt.frames.WriteMotorMixerRequestFrame;
import dongfang.mkt.frames.WriteParamSetRequestFrame;

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;
                }

                void writeSignedByte(int b) throws IOException {
                        if (b<0) b+= 256;
                        writeByte(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(ReadMotorMixerRequestFrame f) throws IOException {
                writeByte('n');
        }

        public void visit(WriteMotorMixerRequestFrame f) throws IOException {
                writeByte('m');
                base64OutputStream.writeByte(f.getMotorMixerVersionNumber());
                base64OutputStream.writeByte(f.getDataLength());
                base64OutputStream.writeChars(f.getName());
                for(int i=0; i<f.getMatrix().length; i++) {
                        int[] row = f.getMatrix()[i];
                        for(int j=0; j<row.length; j++) {
                                base64OutputStream.writeSignedByte(row[j]);
                                //System.out.print(row[j] + " ");
                        }
                        //System.out.println();
                }
                /*
                for(int i=0; i<f.getOppositeMotors().length; i++) {
                        base64OutputStream.writeByte(f.getOppositeMotors()[i]);
                }
                */

        }

        public void visit(ReadVariablesRequestFrame 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(ReadParamSetRequestFrame f) throws IOException {
                writeByte('q');
                base64OutputStream.writeByte(f.getConfigurationSetNumber());
        }

        public void visit(WriteParamSetRequestFrame f) throws IOException {
                writeByte('s');
                base64OutputStream.writeByte(f.getConfigurationSetNumber());
                base64OutputStream.writeByte(f.getConfigurationVersionNumber());
                base64OutputStream.writeByte(f.getDataLength() + 12);
                base64OutputStream.writeBytes(f.getData());
                base64OutputStream.writeChars(f.getName());
        }
       
        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());
        }

        public void visit(ReadIMUConfigurationRequestFrame f) throws IOException {
                writeByte('i');
        }

        public void visit(WriteIMUConfigurationRequestFrame f) throws IOException {
                writeByte('j');
                base64OutputStream.writeByte(f.getConfigurationVersionNumber());
                base64OutputStream.writeByte(f.getDataLength());
                base64OutputStream.writeBytes(f.getData());
        }
}