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.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=\"" + ((double)f.getGroundSpeed())/100 + "\"/>\n");

                        w.write("    <waypoints index=\"" + f.getWaypointIndex() + "\" count=\"" + f.getWaypointCount() + "\"/>\n");
                        w.write("    <height barometric=\"" + f.getHeightByPressure() + "\" 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 RXTXSerialPort();
                _port.init(null);

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