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();
}
}