Subversion Repositories Projects

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

        }

}