/dongfang_FC_rewrite_tool/src/dongfang/mkt/RequestFrameVisitor.java |
---|
16,6 → 16,7 |
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.ResetRequestFrame; |
import dongfang.mkt.frames.SetCompassHeadingRequestFrame; |
49,4 → 50,5 |
void visit(CompassHeadingRequestFrame f) throws IOException; |
void visit(WriteMotorMixerRequestFrame f) throws IOException; |
void visit(WriteIMUConfigurationRequestFrame f) throws IOException; |
void visit(ReadRCChannelsRequestFrame f) throws IOException; |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKInputStream.java |
---|
18,14 → 18,15 |
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.ReadParamSetResponseFrame; |
import dongfang.mkt.frames.VersionResponseFrame; |
import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame; |
import dongfang.mkt.frames.WriteMotorMixerResponseFrame; |
import dongfang.mkt.frames.WriteParamSetResponseFrame; |
import dongfang.mkt.frames.ReadVariablesResponseFrame; |
import dongfang.mkt.frames.VersionResponseFrame; |
public class MKInputStream extends InputStream { |
int readByteCnt; |
368,7 → 369,15 |
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()); |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKOutputStream.java |
---|
18,6 → 18,7 |
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; |
261,4 → 262,8 |
base64OutputStream.writeByte(f.getDataLength()); |
base64OutputStream.writeBytes(f.getData()); |
} |
public void visit(ReadRCChannelsRequestFrame f) throws IOException { |
writeByte('p'); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/ReadRCChannelsRequestFrame.java |
---|
0,0 → 1,17 |
package dongfang.mkt.frames; |
import java.io.IOException; |
import dongfang.mkt.RequestFrameVisitor; |
public class ReadRCChannelsRequestFrame extends RequestFrame { |
public ReadRCChannelsRequestFrame() { |
super(FC_ADDRESS); |
} |
@Override |
public void accept(RequestFrameVisitor o) throws IOException { |
o.visit(this); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/ReadRCChannelsResponseFrame.java |
---|
0,0 → 1,41 |
package dongfang.mkt.frames; |
public class ReadRCChannelsResponseFrame extends ResponseFrame { |
private int numberOfChannels; |
private int[] channels; |
public ReadRCChannelsResponseFrame(int address) { |
super(address); |
// TODO Auto-generated constructor stub |
} |
public int getNumberOfChannels() { |
return numberOfChannels; |
} |
public void setNumberOfChannels(int numberOfChannels) { |
this.numberOfChannels = numberOfChannels; |
} |
public int[] getChannels() { |
return channels; |
} |
public void setChannels(int[] channels) { |
this.channels = channels; |
} |
@Override |
public boolean isResponseTo(RequestFrame r) { |
return r instanceof ReadRCChannelsRequestFrame; |
} |
public String toString() { |
StringBuilder sb = new StringBuilder(); |
for (int i=0; i<getNumberOfChannels(); i++) { |
if (i!=0) sb.append(", "); |
sb.append(i + ":" + channels[i]); |
} |
return sb.toString(); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/io/MKCommPort.java |
---|
1,5 → 1,6 |
package dongfang.mkt.io; |
/* |
import java.io.IOException; |
import java.io.InputStream; |
import java.io.OutputStream; |
9,3 → 10,5 |
InputStream getInputStream() throws IOException ; |
OutputStream getOutputStream() throws IOException ; |
} |
*/ |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/io/RXTXSerialPort.java |
---|
9,6 → 9,7 |
import java.io.InputStream; |
import java.io.OutputStream; |
/* |
public class RXTXSerialPort implements MKCommPort { |
private SerialPort serialPort; |
60,3 → 61,4 |
return serialPort.getOutputStream(); |
} |
} |
*/ |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/IMUConfigurator.java |
---|
79,10 → 79,13 |
private static void readConfiguration(String portIdentifier, String fileName) throws IOException { |
IMUConfiguration cs = readConfiguration(portIdentifier); |
if (cs != null) { |
FileWriter fw = new FileWriter(fileName); |
fw.write(cs.toXML()); |
fw.close(); |
if (fileName != null) { |
FileWriter fw = new FileWriter(fileName); |
fw.write(cs.toXML()); |
fw.close(); |
} |
} |
System.out.println(cs.toXML()); |
} |
private static WriteIMUConfigurationRequestFrame parseXMLIMUConfiguration(InputStream input) throws IOException { |
187,12 → 190,12 |
public static void main(String[] args) throws IOException { |
if (!"r".equals(args[0]) && !"w".equals(args[0])) help(); |
if ("w".equals(args[0]) && (args.length!=2)) help(); |
if ("r".equals(args[0]) && (args.length!=2)) help(); |
if ("r".equals(args[0]) && (args.length>2)) help(); |
String portIdentifier = null; |
if ("r".equals(args[0])) { |
readConfiguration(portIdentifier, args[1]); |
readConfiguration(portIdentifier, args.length<2 ? null : args[1]); |
} else { |
writeConfiguration(portIdentifier, args[1]); |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/RCDump.java |
---|
0,0 → 1,81 |
package dongfang.mkt.main; |
import java.io.BufferedReader; |
import java.io.FileOutputStream; |
import java.io.IOException; |
import java.io.InputStreamReader; |
import java.io.OutputStream; |
import java.io.PrintStream; |
import java.util.Date; |
import dongfang.mkt.comm.FrameQueue; |
import dongfang.mkt.comm.MKConnection; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
import dongfang.mkt.frames.ReadRCChannelsRequestFrame; |
import dongfang.mkt.frames.ReadRCChannelsResponseFrame; |
public class RCDump { |
private static final PrintStream STDERR = System.out; |
interface Logger { |
void start(String timestamp) throws IOException; |
void log(ReadRCChannelsResponseFrame f, long timestamp) throws IOException; |
void end() throws IOException; |
} |
static class ConsoleRCLogger implements Logger { |
public void start(String logId) {} |
public void log(ReadRCChannelsResponseFrame f, long timestamp) { |
if (f == null) { |
System.out.println("Oops, null response frame."); |
return; |
} |
System.out.println(f); |
} |
public void end() {} |
} |
private static Logger createLogger() throws IOException { |
Logger consoleLogger = new ConsoleRCLogger(); |
return consoleLogger; |
} |
private void rcDump(final FrameQueue q, final Logger logger) throws IOException { |
long timer = 0; |
ReadRCChannelsRequestFrame f = new ReadRCChannelsRequestFrame(); |
BufferedReader in = new BufferedReader(new InputStreamReader(System.in)); |
Long firsttimestamp = null; |
while (!in.ready()) |
try { |
long now = System.currentTimeMillis(); |
if (now - timer > 500) { |
timer = now; |
q.sendRequest(f); |
ReadRCChannelsResponseFrame rf = (ReadRCChannelsResponseFrame) q.getResponseFor(f, 500); |
long timestamp = System.currentTimeMillis(); |
if (firsttimestamp == null) { |
firsttimestamp = timestamp; |
timestamp = 0; |
} else { |
timestamp -= firsttimestamp; |
} |
logger.log(rf, timestamp); |
} |
} catch (IOException ex) { |
STDERR.println(ex); |
} |
} |
public static void main(String[] args) throws IOException { |
RCDump test = new RCDump(); |
RXTXSerialPort _port = new RXTXSerialPort(); |
_port.init(null); |
FrameQueue q = new FrameQueue(_port); |
Logger logger = createLogger(); |
logger.start(new Date().toGMTString()); |
test.rcDump(q, logger); |
logger.end(); |
} |
} |