Subversion Repositories Projects

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.OutputStreamWriter;

import java.io.PrintStream;

import java.io.Writer;

import java.util.ArrayList;

import java.util.Date;

import java.util.List;

import dongfang.mkt.comm.FrameQueue;

import dongfang.mkt.comm.MKConnection;

import dongfang.mkt.comm.serial.RXTXSerialPort;

import dongfang.mkt.comm.tcp.MKTCPConnection;

import dongfang.mkt.frames.OSDDataRequestFrame;

import dongfang.mkt.frames.OSDDataResponseFrame;

public class OSDLogger {

        private static final PrintStream STDERR = System.out;

        // private static final FrameFactory ff = MKVersion.getFrameFactory(null);

        interface Logger {

                void start(String timestamp) throws IOException;

                void log(OSDDataResponseFrame f, long timestamp) throws IOException;

                void end() throws IOException;

        }

        static class ConsoleOSDLogger implements Logger {

                public void start(String logId) {}

                public void log(OSDDataResponseFrame f, long timestamp) {

                        if (f == null) {

                                System.out.println("Oops, null response frame.");

                                return;

                        }

                                System.out.println(f);

                }

                public void end() {}

        }

        static class XMLOSDLogger implements Logger {

                Writer w;

                String encoding;

                XMLOSDLogger(OutputStream out, String encoding) throws IOException {

                        w = new OutputStreamWriter(out, encoding);

                        this.encoding = encoding;

                }

                public void start(String logId) throws IOException {

                        w.write("<?xml version=\"1.0\" encoding=\"" + encoding + "\"?>\n");

                        w.write("<mk-osddata time=\"" + logId + "\">\n");

                }

                public void log(OSDDataResponseFrame f, long timestamp)

                                throws IOException {

                        if (f==null)

                                return;

                        w.write("  <dataset timestamp=\"" + timestamp + "\" version=\"" + f.getVersion() + "\">\n");

                        w.write("    <currentPosition " + f.getCurrentPosition().toXML() + "/>\n");

                        w.write("    <targetPosition " + f.getTargetPosition().toXML() + " " + f.getCurrentToTarget().toXML() + "/>\n");

                        w.write("    <homePosition " + f.getHomePosition().toXML() + " " + f.getCurrentToHome().toXML() + "/>\n");

                        w.write("    <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + f.getGroundSpeed() + "\"/>\n");

                        w.write("    <waypoints index=\"" + f.getWaypointIndex() + "\" count=\"" + f.getWaypointCount() + "\"/>\n");

                        double gpsHeight = f.getCurrentPosition().getAltitude() - f.getHomePosition().getAltitude();

                        w.write("    <height barometric=\"" + f.getHeightByPressure() + "\" gps=\"" + gpsHeight + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n");

                        w.write("    <status rcQuality=\"" + f.getRcQuality() + "\" fcflags=\"" + f.getFcFlags() + "\" fcflags2=\"" + f.getFcFlags2() + "\" ncflags=\"" + f.getNcFlags() + "\" errorCode=\"" + f.getErrorCode() + "\" numberOfSatellites=\"" + f.getNumberOfSatellites() + "\"/>\n");

                        w.write("    <flight time=\"" + f.getFlightTime() + "\" batteryVoltage=\"" + f.getBatteryVoltage() + "\" throttle=\"" + f.getThrottle() + "\" current=\"" + f.getCurrent() + "\" capacityUsed=\"" + f.getCapacityUsed() + "\"/>\n");

                        w.write("    <attitude pitch=\"" + f.getPitchAngle() + "\" roll=\"" + f.getRollAngle() + "\"/>\n");

                        /*

        private int operatingRadius;

        private int targetLoiterTime;

        private int setpointForAltitude;        // 16 bit signed.

                         */

                        w.write("  </dataset>\n");

                }

                public void end() throws IOException {

                        w.write("</mk-osddata>\n");

                        w.flush();

                        w.close();

                }

        }

        static class CompositeOSDLogger implements Logger {

                List<Logger> loggers = new ArrayList<Logger>(2);

                public void log(OSDDataResponseFrame f, long timestamp) throws IOException {

                        for (Logger l : loggers)

                                l.log(f, timestamp);

                }

                public void start(String logId) throws IOException {

                        for (Logger l : loggers)

                                l.start(logId);

                }

                public void end() throws IOException {

                        for (Logger l : loggers)

                                l.end();

                }

                void add(Logger l) {

                        loggers.add(l);

                }

        }

        private static Logger createLogger(OutputStream out, String encoding) throws IOException {

                Logger consoleLogger = new ConsoleOSDLogger();

                XMLOSDLogger xmlLogger = new XMLOSDLogger(out, encoding);

                CompositeOSDLogger logger = new CompositeOSDLogger();

                logger.add(consoleLogger);

                logger.add(xmlLogger);

                return logger;

        }

        private void prepareLoggers(final FrameQueue q, final Logger logger)

                        throws IOException {

                new Thread() {

                        public void run() {

                        }

                }.start();

        }

        private void osd(final FrameQueue q, final Logger logger, final int interval) throws IOException {

                long timer = 0;

                OSDDataRequestFrame f = new OSDDataRequestFrame();

                f.setAutoSendInterval(interval);

                BufferedReader in = new BufferedReader(new InputStreamReader(System.in));

                Long firsttimestamp = null;

                while (!in.ready())

                        try {

                                long now = System.currentTimeMillis();

                                if (now - timer > 3000) {

                                        timer = now;

                                        q.sendRequest(f);

                                }

                                OSDDataResponseFrame rf = (OSDDataResponseFrame) q.getResponseFor(f, 5000);

                                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 {

                OSDLogger test = new OSDLogger();

                MKConnection _port = new MKTCPConnection(); //RXTXSerialPort();

                _port.init("8080");

                FrameQueue q = new FrameQueue(_port);

                String encoding = "iso-8859-1";

                OutputStream fout = new FileOutputStream("osd.xml");

                Logger logger = createLogger(fout, encoding);

                logger.start(new Date().toGMTString());

                test.osd(q, logger, 20);

                logger.end();

                fout.close();

        }

}