Rev 1695 | 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.DCMMatrixRequestFrame;
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.ProfilerLabelRequestFrame;
import dongfang.mkt.frames.ProfilerRequestFrame;
import dongfang.mkt.frames.ReadExternalControlRequestFrame;
import dongfang.mkt.frames.ReadIMUConfigurationRequestFrame;
import dongfang.mkt.frames.ReadMotorMixerRequestFrame;
import dongfang.mkt.frames.ReadParamSetRequestFrame;
import dongfang.mkt.frames.ReadRCChannelsRequestFrame;
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(ProfilerLabelRequestFrame f) throws IOException {
writeByte('f');
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(ProfilerRequestFrame f) throws IOException {
writeByte('u');
base64OutputStream.writeByte(f.getAutoSendInterval());
}
public void visit(DCMMatrixRequestFrame f) throws IOException {
writeByte('e');
}
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());
}
public void visit(ReadRCChannelsRequestFrame f) throws IOException {
writeByte('p');
}
}