0,0 → 1,202 |
package dongfang.mkt.serial; |
|
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.DebugRequestFrame; |
import dongfang.mkt.frames.ExternalControlRequestFrame; |
import dongfang.mkt.frames.LoopbackTestRequestFrame; |
import dongfang.mkt.frames.MotorTestRequestFrame; |
import dongfang.mkt.frames.RequestFrame; |
import dongfang.mkt.frames.ResetRequestFrame; |
import dongfang.mkt.frames.SetCompassHeadingRequestFrame; |
import dongfang.mkt.frames.SingleDisplayRequestFrame; |
import dongfang.mkt.frames.UniversalReadParamSetRequestFrame; |
import dongfang.mkt.frames.UniversalWriteParamSetRequestFrame; |
import dongfang.mkt.frames.VariablesRequestFrame; |
import dongfang.mkt.frames.VersionRequestFrame; |
|
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; |
} |
|
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(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(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(VariablesRequestFrame f) throws IOException { |
writeByte('x'); |
} |
|
public void visit(ExternalControlRequestFrame f) throws IOException { |
writeByte('y'); |
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(LoopbackTestRequestFrame f) throws IOException { |
writeByte('0'); |
base64OutputStream.writeByte(f.getByte()); |
base64OutputStream.writeWord(f.getWord()); |
base64OutputStream.writeChars(f.getChararray()); |
} |
|
public void visit(UniversalReadParamSetRequestFrame f) throws IOException { |
writeByte('q'); |
base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
} |
|
public void visit(UniversalWriteParamSetRequestFrame f) throws IOException { |
writeByte('s'); |
base64OutputStream.writeByte(f.getConfigurationSetNumber()); |
base64OutputStream.writeByte(f.getConfigurationVersionNumber()); |
base64OutputStream.writeBytes(f.getData()); |
} |
|
public void visit(SetCompassHeadingRequestFrame f) throws IOException { |
writeByte('K'); |
} |
} |