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.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.ProfilerLabelResponseFrame;
import dongfang.mkt.frames.ProfilerResponseFrame;
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 'F': {
                        ProfilerLabelResponseFrame f = new ProfilerLabelResponseFrame(address);
                        f.setChannel(base64InputStream.readByte());
                        f.setLabel(base64InputStream.readChars(16));
                        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 'U': {
                        ProfilerResponseFrame f = new ProfilerResponseFrame(address);
                        f.setTotalHits(base64InputStream.readSignedDWord());
                        for (int i=0; i<16; i++)
                                f.setActivity(i, base64InputStream.readWord());
                        result = f;
                        break;
                }
                case 'V': {
                        VersionResponseFrame f = new VersionResponseFrame(address);
                        f.setSWMajor(base64InputStream.readByte());
                        f.setSWMinor(base64InputStream.readByte());
                        f.setSWPatch(base64InputStream.readByte());
                        f.setProtoMajor(base64InputStream.readByte());
                        f.setProtoMinor(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;
        }
}