Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1694 → Rev 1695

/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/IMUDump.java
0,0 → 1,81
package dongfang.mkt.main;
 
import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStreamReader;
import java.io.PrintStream;
import java.util.Date;
 
import dongfang.mkt.comm.FrameQueue;
import dongfang.mkt.comm.serial.RXTXSerialPort;
import dongfang.mkt.frames.AttitudeDataRequestFrame;
import dongfang.mkt.frames.AttitudeDataResponseFrame;
import dongfang.mkt.frames.Frame;
import dongfang.mkt.frames.ReadRCChannelsRequestFrame;
import dongfang.mkt.frames.ReadRCChannelsResponseFrame;
 
public class IMUDump {
private static final PrintStream STDERR = System.out;
 
interface Logger {
void start(String timestamp) throws IOException;
void log(AttitudeDataResponseFrame f, long timestamp) throws IOException;
void end() throws IOException;
}
 
static class ConsoleAttitudeLogger implements Logger {
public void start(String logId) {}
 
public void log(AttitudeDataResponseFrame 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 ConsoleAttitudeLogger();
return consoleLogger;
}
 
private void imuDump(final FrameQueue q, final Logger logger) throws IOException {
long timer = 0;
AttitudeDataRequestFrame f = new AttitudeDataRequestFrame(100);
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);
AttitudeDataResponseFrame rf = (AttitudeDataResponseFrame) 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 {
IMUDump test = new IMUDump();
RXTXSerialPort _port = new RXTXSerialPort();
_port.init(null);
FrameQueue q = new FrameQueue(_port);
Logger logger = createLogger();
logger.start(new Date().toGMTString());
test.imuDump(q, logger);
logger.end();
}
}