Subversion Repositories Projects

Rev

Blame | Last modification | View Log | RSS feed

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