Subversion Repositories Projects

package dongfang.mkt.serial;

import java.io.IOException;

import java.io.InputStream;

import java.io.OutputStream;

import dongfang.mkt.frames.AllDisplaysResponseFrame;

import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;

import dongfang.mkt.frames.AttitudeDataResponseFrame;

import dongfang.mkt.frames.ChangeParameterSetResponseFrame;

import dongfang.mkt.frames.ConfirmFrame;

import dongfang.mkt.frames.DebugResponseFrame;

import dongfang.mkt.frames.MotorTestResponseFrame;

import dongfang.mkt.frames.ResponseFrame;

import dongfang.mkt.frames.SetCompassHeadingResponseFrame;

import dongfang.mkt.frames.UniversalReadParamSetResponseFrame;

import dongfang.mkt.frames.UniversalWriteParamSetResponseFrame;

import dongfang.mkt.frames.VariablesResponseFrame;

import dongfang.mkt.frames.VersionResponseFrame;

import dongfang.mkt.main.UniversalConfigurator;

public class MKInputStream extends InputStream {

        int readByteCnt;

        class MKDataInputStream {

                int[] inbuf = new int[4];

                int[] outbuf = new int[3];

                int outbufptr = outbuf.length; // reset to "buffer empty"

                private boolean decode() throws IOException {

                        for (int i = 0; i < 4; i++) {

                                int raw = MKInputStream.this.readByte();

                                int in = raw - '=';

                                if (in < 0 || in > 63)

                                        return false;

                                        // throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!");

                                inbuf[i] = in;

                                readByteCnt++;

                        }

                        outbuf[0] = (inbuf[0] << 2) | (inbuf[1] >>> 4);

                        outbuf[1] = ((inbuf[1] & 0x0f) << 4) | (inbuf[2] >>> 2);

                        outbuf[2] = ((inbuf[2] & 0x03) << 6) | inbuf[3];

                        outbufptr = 0;

                        return true;

                }

                public void reset() {

                        outbufptr = outbuf.length; // reset to "buffer empty"

                }

                public int readByte() throws IOException {

                        if (outbufptr > 2 && !decode())

                                        throw new IOException("Out of range data received where frame data expected. Probably the frame was shorter than expected (" + readByteCnt + ")!");            

                        return outbuf[outbufptr++];

                }

                public int readWord() throws IOException {

                        int byte2 = readByte();

                        int byte1 = readByte();

                        return (byte1 << 8) | byte2;

                }

                public int readSignedWord() throws IOException {

                        int word = readWord();

                        if (word > 32767)

                                word = word - 65536;

                        return word;

                }

                public int[] readBytes(int length) throws IOException {

                        int[] result = new int[length];

                        for (int i = 0; i < length; i++) {

                                result[i] = readByte();

                        }

                        return result;

                }

                public int[] readWords(int length) throws IOException {

                        int[] result = new int[length];

                        for (int i = 0; i < length; i++) {

                                result[i] = readWord();

                        }

                        return result;

                }

                public char[] readChars(int length) throws IOException {

                        char[] result = new char[length];

                        for (int i = 0; i < 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)

                                result[i] = (char) readByte();

                        }

                        return result;

                }

        }

        MKDataInputStream dis = new MKDataInputStream();

        OutputStream nonPacketSpillway = null; //System.err;

        final InputStream is;

        int crc;

        public MKInputStream(InputStream is) {

                this.is = is;

        }

        @Override

        public int read() throws IOException {

                int i;

                while ((i=is.read()) == -1);

                // System.out.print("Received: " + i + " (as char: " + (char)i + ")\n");

                return i;

        }

        public int readByte() throws IOException {

                int _byte = read();

                if (_byte < 0)

                        throw new IOException("End of Stream!");

                crc += _byte;

                return _byte;

        }

        public MKDataInputStream getBase64InputStream() {

                return dis;

        }

        public ResponseFrame getNextFrame() throws IOException {

                int c;

                while ((c = read()) != '#') {

                        // throw it on some scrap-text buffer.

                        if (nonPacketSpillway != null)

                                nonPacketSpillway.write(c);

                }

                crc = '#';

                dis.reset();

                int address = readByte() - 'a';

                int iid = readByte();

                readByteCnt = 0;

                //RESPONSE_IDS id = getResponseType(iid);

                ResponseFrame result;

                switch (iid) {

                case 'A': {

                        AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address);

                        f.setChannel(getBase64InputStream().readByte());

                        f.setLabel(getBase64InputStream().readChars(16));

                        result = f;

                        break;

                }

                case 'B': {

                        ConfirmFrame f = new ConfirmFrame(address);

                        f.setFrameNum(getBase64InputStream().readByte());

                        result = f;

                        break;

                }

                case 'C': {

                        AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address);

                        f.setPitch(getBase64InputStream().readSignedWord());

                        f.setRoll(getBase64InputStream().readSignedWord());

                        f.setHeading(getBase64InputStream().readSignedWord());

                        f.setExpansion(getBase64InputStream().readBytes(8));

                        result = f;

                        break;

                }

                case 'D': {

                        DebugResponseFrame f = new DebugResponseFrame(address);

                        for (int i=0; i<2; i++)

                                f.setDigital(i, getBase64InputStream().readByte());

                        for (int i=0; i<32; i++)

                                f.setAnalog(i, getBase64InputStream().readSignedWord());

                        result = f;

                        break;

                }

                case 'F': {

                        ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);

                        f.setParameterSetNumber(getBase64InputStream().readByte());

                        result = f;

                        break;

                }

                case 'H': {

                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);

                        f.setLine(getBase64InputStream().readByte());

                        //f.setMaxItem(getDataInputStream().readByte());

                        f.setText(getBase64InputStream().readChars(20));

                        result = f;

                        break;

                }

                case 'L': {

                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);

                        f.setItem(getBase64InputStream().readByte());

                        // f.setMaxItem(getDataInputStream().readByte());

                        f.setText(getBase64InputStream().readChars(80));

                        result = f;

                        break;

                }

                case 'S': {

                        UniversalWriteParamSetResponseFrame f = new UniversalWriteParamSetResponseFrame(address);

                        f.setParameterSetNumber(getBase64InputStream().readByte());

                        result = f;

                        break;

                }

                case 'T': {

                        MotorTestResponseFrame f = new MotorTestResponseFrame(address);

                        result = f;

                        break;

                }

                /*

                 * We have a collision with the 'x' token: Also used for VariablesRequest.

                case 'x': {

                        LoopbackTestResponseFrame f = new LoopbackTestResponseFrame(address);

                        f.setByte(getDataInputStream().readByte());

                        f.setWord(getDataInputStream().readWord());

                        f.setChararray(getDataInputStream().readChars(8));

                        result = f;

                        break;

                }

            */

                case 'V': {

                        VersionResponseFrame f = new VersionResponseFrame(address);

                        f.setSWMajor(getBase64InputStream().readByte());

                        f.setSWMinor(getBase64InputStream().readByte());

                        f.setProtoMajor(getBase64InputStream().readByte());

                        f.setProtoMinor(getBase64InputStream().readByte());

                        f.setSWPatch(getBase64InputStream().readByte());

                        f.setHardwareErrors(getBase64InputStream().readBytes(5));

                        result = f;

                        break;

                }

                // This is my own creation. The ID collides with the waypoint one of FC.

                case 'X': {

                        VariablesResponseFrame f = new VariablesResponseFrame(address);

                        f.setVariables(getBase64InputStream().readWords(8));

                        result = f;

                        break;

                }

                case 'w': {

                        SetCompassHeadingResponseFrame f = new SetCompassHeadingResponseFrame(address);

                        // do stuff.

                        /*

                        ToMk3Mag.Attitude[0] = (int16_t)((10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg

                        ToMk3Mag.Attitude[1] = (int16_t)((10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL); // approx. 0.1 deg

                        ToMk3Mag.UserParam[0] = dynamicParams.UserParams[0];

                        ToMk3Mag.UserParam[1] = dynamicParams.UserParams[1];

                        ToMk3Mag.CalState = compassCalState;

                        */

                        // Waste 8 bytes to make CRC match.

                        getBase64InputStream().readBytes(8);

                        result = f;

                        break;

                }

                case 'Q':

                        UniversalReadParamSetResponseFrame f = new UniversalReadParamSetResponseFrame(address);

                        f.setConfigurationSetNumber(getBase64InputStream().readByte());

                        f.setConfigurationVersion(getBase64InputStream().readByte());

                        int length = getBase64InputStream().readByte();

                        f.setData(getBase64InputStream().readBytes(length));

                        result = f;

                        break;

                default:

                        result = null;

                }

                int receivedCRC = (read() - '=') << 6;

                receivedCRC += (read() - '=');

                crc %= 4096;

                if (receivedCRC != crc) {

                        /// System.err.println("Expected CRC: " + crc + ", got CRC: " + receivedCRC);

                        throw new IOException("CRC mismatch! Calculated crc: " + (int)crc + "; received check crc: " + receivedCRC + ", difference: " + Math.abs(crc - receivedCRC));

                }

                if (read() != '\r') {

                        throw new IOException("CR at end of frame missing");

                }

                return result;

        }

}