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;

	}

}