Subversion Repositories Projects

package dongfang.mkt.comm;

import java.io.IOException;

import java.io.InputStream;

import java.io.OutputStream;

import dongfang.mkt.datatype.GPSBearingAndRange;

import dongfang.mkt.datatype.GPSPosition;

import dongfang.mkt.frames.AllDisplaysResponseFrame;

import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;

import dongfang.mkt.frames.AttitudeDataResponseFrame;

import dongfang.mkt.frames.ChangeParameterSetResponseFrame;

import dongfang.mkt.frames.CompassHeadingResponseFrame;

import dongfang.mkt.frames.ConfirmFrame;

import dongfang.mkt.frames.DCMMatrixResponseFrame;

import dongfang.mkt.frames.DebugResponseFrame;

import dongfang.mkt.frames.MotorTestResponseFrame;

import dongfang.mkt.frames.OSDDataResponseFrame;

import dongfang.mkt.frames.ReadExternalControlResponseFrame;

import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame;

import dongfang.mkt.frames.ReadMotorMixerResponseFrame;

import dongfang.mkt.frames.ReadParamSetResponseFrame;

import dongfang.mkt.frames.ReadRCChannelsResponseFrame;

import dongfang.mkt.frames.ReadVariablesResponseFrame;

import dongfang.mkt.frames.ResponseFrame;

import dongfang.mkt.frames.SetCompassHeadingResponseFrame;

import dongfang.mkt.frames.VersionResponseFrame;

import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame;

import dongfang.mkt.frames.WriteMotorMixerResponseFrame;

import dongfang.mkt.frames.WriteParamSetResponseFrame;

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 readSignedByte() throws IOException {

                        byte result = (byte)readByte();

                        return result;

                }

                public int readWord() throws IOException {

                        int byte0 = readByte();

                        int byte1 = readByte();

                        return (byte1 << 8) | byte0;

                }

                public int readSignedWord() throws IOException {

                        int word = readWord();

                        if (word > 32767)

                                word = word - 65536;

                        return word;

                }

                public int readSignedDWord() throws IOException {

                        int byte0 = readByte();

                        int byte1 = readByte();

                        int byte2 = readByte();

                        int byte3 = readByte();

                        return (byte3 << 24) | (byte2 << 16) | (byte1 << 8) | byte0;

                }

                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 int[] readSignedWords(int length) throws IOException {

                        int[] result = new int[length];

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

                                result[i] = readSignedWord();

                        }

                        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;

                }

                public float readFloat() throws IOException {

                        int asInt = readByte() | (readByte()<<8) | (readByte()<<16) | (readByte()<<24);

                        return Float.intBitsToFloat(asInt);

                }

                public float[] readFloats(int length) throws IOException {

                        float[] result = new float[length];

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

                                result[i] = readFloat();

                        }

                        return result;

                }

}

        MKDataInputStream base64InputStream = 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 base64InputStream;

        }

        public ResponseFrame getNextFrame() throws IOException {

                int c;

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

                        // throw it on some scrap-text buffer.

                        if (nonPacketSpillway != null)

                                nonPacketSpillway.write(c);

                }

                crc = '#';

                base64InputStream.reset();

                int address = readByte() - 'a';

                int iid = readByte();

                readByteCnt = 0;

                //RESPONSE_IDS id = getResponseType(iid);

                ResponseFrame result;

//              System.out.println("Received a: " + (char)iid + " from " + address);

                switch (iid) {

                case 'A': {

                        AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address);

                        f.setChannel(base64InputStream.readByte());

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

                        result = f;

                        break;

                }

                case 'B': {

                        ConfirmFrame f = new ConfirmFrame(address);

                        f.setFrameNum(base64InputStream.readByte());

                        result = f;

                        break;

                }

                case 'C': {

                        AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address);

                        f.setAttitude(base64InputStream.readFloats(3));

                        f.setRates(base64InputStream.readFloats(3));

                        f.setAcc(base64InputStream.readFloats(3));

                        result = f;

                        break;

                }

                case 'D': {

                        DebugResponseFrame f = new DebugResponseFrame(address);

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

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

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

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

                        result = f;

                        break;

                }

                case 'E': {

                        DCMMatrixResponseFrame f= new DCMMatrixResponseFrame(address);

                        float[][] matrix = new float[3][];

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

                                float[] row = base64InputStream.readFloats(3);

                                matrix[i] = row;

                        }

                        f.setMatrix(matrix);

                        result = f;

                        break;

                }

                case 'F': {

                        ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address);

                        f.setParameterSetNumber(base64InputStream.readByte());

                        result = f;

                        break;

                }

                case 'G': {

                        ReadExternalControlResponseFrame f = new ReadExternalControlResponseFrame(address);

                        f.setDigital(base64InputStream.readBytes(2));

                        f.setRemoteButtons(base64InputStream.readByte());

                        f.setPitch(base64InputStream.readByte());

                        f.setRoll(base64InputStream.readByte());

                        f.setYaw(base64InputStream.readByte());

                        f.setThrottle(base64InputStream.readByte());

                        f.setHeight(base64InputStream.readByte());

                        f.setCommand(base64InputStream.readByte());

                        f.setFrameNum(base64InputStream.readByte());

                        f.setArgument(base64InputStream.readByte());

                        result = f;

                        break;

                }

                case 'H': {

                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);

                        f.setLine(base64InputStream.readByte());

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

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

                        result = f;

                        break;

                }

                case 'I': {

                        ReadIMUConfigurationResponseFrame f = new ReadIMUConfigurationResponseFrame(address);

                        f.setConfigurationVersion(base64InputStream.readByte());

                        int length = base64InputStream.readByte();

                        f.setConfigurationSetLength(length);

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

                        result = f;

                        break;

                }

                case 'J': {

                        WriteIMUConfigurationResponseFrame f = new WriteIMUConfigurationResponseFrame(address);

                        f.setWasAccepted(base64InputStream.readByte()==1);

                        result = f;

                        break;

                }

                case 'k' : {

                        CompassHeadingResponseFrame f = new CompassHeadingResponseFrame(address);

                        base64InputStream.readSignedWords(2);

                        base64InputStream.readBytes(2);

                        base64InputStream.readByte();

                        base64InputStream.readByte();

                        result = f;

                        break;

                }

                case 'L': {

                        AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);

                        f.setItem(base64InputStream.readByte());

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

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

                        result = f;

                        break;

                }

                case 'M': {

                        WriteMotorMixerResponseFrame f = new WriteMotorMixerResponseFrame(address);

                        f.setWasAccepted(base64InputStream.readByte() != 0);

                        result = f;

                        break;

                }

                case 'N': {

                        int numMotors = 12;

                        ReadMotorMixerResponseFrame f = new ReadMotorMixerResponseFrame();

                        f.setConfigurationVersion(base64InputStream.readByte());

                        int length = base64InputStream.readByte();

                        f.setDataLength(length);

                        int testLength = 12; // the name is first!

                        char[] name = base64InputStream.readChars(12);

                        f.setName(name);

                        int[][] matrix = new int[numMotors][];

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

                                int[] row = new int[5];

                                matrix[i] = row;

                                for(int j=0; j<5; j++) {

                                        row[j] = base64InputStream.readSignedByte();

                                        testLength++;

                                }

                        }

                        if (length != testLength)

                                throw new IOException("Length of motor mixer data was not as expected (" + testLength + " vs. " + length + ").");

                        /*

                        int[] opposite = new int[numMotors];

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

                                opposite[i] = base64InputStream.readByte();

                        }

                        */

                        f.setMatrix(matrix);

                        result = f;

                        break;

                }

                case 'O': {

                        OSDDataResponseFrame f = new OSDDataResponseFrame(address);

                        f.setVersion(base64InputStream.readByte());

                        GPSPosition pos = new GPSPosition();

                        pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7);

                        pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7);

                        pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3);

                        pos.setStatus(base64InputStream.readByte());

                        f.setCurrentPosition(pos);

                        pos = new GPSPosition();

                        pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7);

                        pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7);

                        pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3);

                        pos.setStatus(base64InputStream.readByte());

                        f.setTargetPosition(pos);

                        GPSBearingAndRange rnb = new GPSBearingAndRange();

                        rnb.setDistance(((double)base64InputStream.readWord())/10.0);

                        rnb.setBearing(base64InputStream.readSignedWord());

                        f.setCurrentToTarget(rnb);

                        pos = new GPSPosition();

                        pos.setLongitude(((double)base64InputStream.readSignedDWord())/1E7);

                        pos.setLatitude(((double)base64InputStream.readSignedDWord())/1E7);

                        pos.setAltitude(((double)base64InputStream.readSignedDWord())/1E3);

                        pos.setStatus(base64InputStream.readByte());

                        f.setHomePosition(pos);

                        rnb = new GPSBearingAndRange();

                        rnb.setDistance(((double)base64InputStream.readWord())/10.0);

                        rnb.setBearing(base64InputStream.readSignedWord());

                        f.setCurrentToHome(rnb);

                        f.setWaypointIndex(base64InputStream.readByte());

                        f.setWaypointCount(base64InputStream.readByte());

                        f.setNumberOfSatellites(base64InputStream.readByte());

                        // This stunt is a metric unit conversion: The height was supposed (H&I) to be in integral 5cm steps.

                        // However there is error factor in the measurement of 24% too much.

                        // h[m] = h[int] * 0.05 / 1.24 = h[int]

                        f._setHeightByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24);

                        f.setVerticalVelocityByPressure(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24); // clueless!

                        f.setFlightTime(base64InputStream.readWord());

                        f.setBatteryVoltage(base64InputStream.readByte());

                        f.setGroundSpeed(((double)base64InputStream.readWord()) / 1E2);

                        f.setDirectionOfFlight(base64InputStream.readSignedWord());

                        f.setCompassHeading(base64InputStream.readSignedWord());

                        f.setPitchAngle(base64InputStream.readSignedByte());

                        f.setRollAngle(base64InputStream.readSignedByte());

                        f.setRcQuality(base64InputStream.readByte());

                        f.setFcFlags(base64InputStream.readByte());

                        f.setNcFlags(base64InputStream.readByte());

                        f.setErrorCode(base64InputStream.readByte());

                        f.setOperatingRadius(base64InputStream.readByte());

                        f.setVerticalVelocityByGPS(((double)base64InputStream.readSignedWord()) / 1E2);

                        f.setTargetLoiterTime(base64InputStream.readByte());

                        f.setFcFlags2(base64InputStream.readByte());

                        f.setSetpointForAltitude(((double)base64InputStream.readSignedWord()) * 0.05 / 1.24);

                        f.setThrottle(base64InputStream.readByte());

                        f.setCurrent(base64InputStream.readWord());

                        f.setCapacityUsed(base64InputStream.readWord());

                        result = f;

                        break;

                }

                case 'P': {

                        ReadRCChannelsResponseFrame f = new ReadRCChannelsResponseFrame(address);

                        int numberOfChannels = base64InputStream.readByte();

                        f.setNumberOfChannels(numberOfChannels);

                        f.setChannels(base64InputStream.readSignedWords(numberOfChannels));

                        result = f;

                        break;

                }

                case 'S': {

                        WriteParamSetResponseFrame f = new WriteParamSetResponseFrame(address);

                        f.setParameterSetNumber(base64InputStream.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(base64InputStream.readByte());

                        f.setSWMinor(base64InputStream.readByte());

                        f.setProtoMajor(base64InputStream.readByte());

                        f.setProtoMinor(base64InputStream.readByte());

                        f.setSWPatch(base64InputStream.readByte());

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

                        result = f;

                        break;

                }

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

                case 'X': {

                        ReadVariablesResponseFrame f = new ReadVariablesResponseFrame(address);

                        f.setVariables(base64InputStream.readSignedWords(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.

                        base64InputStream.readBytes(8);

                        result = f;

                        break;

                }

                case 'Q':

                        ReadParamSetResponseFrame f = new ReadParamSetResponseFrame(address);

                        f.setConfigurationSetNumber(base64InputStream.readByte());

                        f.setConfigurationVersion(base64InputStream.readByte());

                        int length = base64InputStream.readByte();

                        f.setConfigurationSetLength(length);

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

                        result = f;

                        break;

                default:

                        int count = 0;

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

                                count++;

                        }

                        System.err.println("Unknown frame " + (char)iid + " received from " + address);

                        System.err.println("It appears to have " + (count-2) + " data bytes (encoded)");

                        System.err.println("(" + (count-2) * 6/8 + " data bytes decoded)");

                        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;

        }

}