WebSVN - Projects - Diff - Rev 1564 and 1568 - /dongfang_FC_rewrite_tool/src/dongfang/mkt/main/OSDLogger.java


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

Rev 1564	Rev 1568
1	package dongfang.mkt.main;	1	package dongfang.mkt.main;
2		2
3	import java.io.BufferedReader;	3	import java.io.BufferedReader;
4	import java.io.FileOutputStream;	4	import java.io.FileOutputStream;
5	import java.io.IOException;	5	import java.io.IOException;
6	import java.io.InputStreamReader;	6	import java.io.InputStreamReader;
7	import java.io.OutputStream;	7	import java.io.OutputStream;
8	import java.io.OutputStreamWriter;	8	import java.io.OutputStreamWriter;
9	import java.io.PrintStream;	9	import java.io.PrintStream;
10	import java.io.Writer;	10	import java.io.Writer;
11	import java.util.ArrayList;	11	import java.util.ArrayList;
12	import java.util.Date;	12	import java.util.Date;
13	import java.util.List;	13	import java.util.List;
14		14
15	import dongfang.mkt.comm.FrameQueue;	15	import dongfang.mkt.comm.FrameQueue;
16	import dongfang.mkt.comm.MKConnection;	16	import dongfang.mkt.comm.MKConnection;
17	import dongfang.mkt.comm.serial.RXTXSerialPort;	17	import dongfang.mkt.comm.serial.RXTXSerialPort;
-		18	import dongfang.mkt.comm.tcp.MKTCPConnection;
18	import dongfang.mkt.frames.OSDDataRequestFrame;	19	import dongfang.mkt.frames.OSDDataRequestFrame;
19	import dongfang.mkt.frames.OSDDataResponseFrame;	20	import dongfang.mkt.frames.OSDDataResponseFrame;
20		21
21	public class OSDLogger {	22	public class OSDLogger {
22	private static final PrintStream STDERR = System.out;	23	private static final PrintStream STDERR = System.out;
23	// private static final FrameFactory ff = MKVersion.getFrameFactory(null);	24	// private static final FrameFactory ff = MKVersion.getFrameFactory(null);
24		25
25	interface Logger {	26	interface Logger {
26	void start(String timestamp) throws IOException;	27	void start(String timestamp) throws IOException;
27	void log(OSDDataResponseFrame f, long timestamp) throws IOException;	28	void log(OSDDataResponseFrame f, long timestamp) throws IOException;
28	void end() throws IOException;	29	void end() throws IOException;
29	}	30	}
30		31
31	static class ConsoleOSDLogger implements Logger {	32	static class ConsoleOSDLogger implements Logger {
32	public void start(String logId) {}	33	public void start(String logId) {}
33		34
34	public void log(OSDDataResponseFrame f, long timestamp) {	35	public void log(OSDDataResponseFrame f, long timestamp) {
35	if (f == null) {	36	if (f == null) {
36	System.out.println("Oops, null response frame.");	37	System.out.println("Oops, null response frame.");
37	return;	38	return;
38	}	39	}
39	System.out.println(f);	40	System.out.println(f);
40	}	41	}
41		42
42	public void end() {}	43	public void end() {}
43	}	44	}
44		45
45	static class XMLOSDLogger implements Logger {	46	static class XMLOSDLogger implements Logger {
46	Writer w;	47	Writer w;
47	String encoding;	48	String encoding;
48		49
49	XMLOSDLogger(OutputStream out, String encoding) throws IOException {	50	XMLOSDLogger(OutputStream out, String encoding) throws IOException {
50	w = new OutputStreamWriter(out, encoding);	51	w = new OutputStreamWriter(out, encoding);
51	this.encoding = encoding;	52	this.encoding = encoding;
52	}	53	}
53		54
54	public void start(String logId) throws IOException {	55	public void start(String logId) throws IOException {
55	w.write("<?xml version=\"1.0\" encoding=\"" + encoding + "\"?>\n");	56	w.write("<?xml version=\"1.0\" encoding=\"" + encoding + "\"?>\n");
56	w.write("<mk-osddata time=\"" + logId + "\">\n");	57	w.write("<mk-osddata time=\"" + logId + "\">\n");
57	}	58	}
58		59
59	public void log(OSDDataResponseFrame f, long timestamp)	60	public void log(OSDDataResponseFrame f, long timestamp)
60	throws IOException {	61	throws IOException {
61	if (f==null)	62	if (f==null)
62	return;	63	return;
63	w.write(" <dataset timestamp=\"" + timestamp + "\" version=\"" + f.getVersion() + "\">\n");	64	w.write(" <dataset timestamp=\"" + timestamp + "\" version=\"" + f.getVersion() + "\">\n");
64	w.write(" <currentPosition " + f.getCurrentPosition().toXML() + "/>\n");	65	w.write(" <currentPosition " + f.getCurrentPosition().toXML() + "/>\n");
65	w.write(" <targetPosition " + f.getTargetPosition().toXML() + " " + f.getCurrentToTarget().toXML() + "/>\n");	66	w.write(" <targetPosition " + f.getTargetPosition().toXML() + " " + f.getCurrentToTarget().toXML() + "/>\n");
66	w.write(" <homePosition " + f.getHomePosition().toXML() + " " + f.getCurrentToHome().toXML() + "/>\n");	67	w.write(" <homePosition " + f.getHomePosition().toXML() + " " + f.getCurrentToHome().toXML() + "/>\n");
67	w.write(" <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + ((double)f.getGroundSpeed())/100 + "\"/>\n");	68	w.write(" <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + f.getGroundSpeed() + "\"/>\n");
68		69
-		70	w.write(" <waypoints index=\"" + f.getWaypointIndex() + "\" count=\"" + f.getWaypointCount() + "\"/>\n");
-		71
69	w.write(" <waypoints index=\"" + f.getWaypointIndex() + "\" count=\"" + f.getWaypointCount() + "\"/>\n");	72	double gpsHeight = f.getCurrentPosition().getAltitude() - f.getHomePosition().getAltitude();
70	w.write(" <height barometric=\"" + f.getHeightByPressure() + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n");	73	w.write(" <height barometric=\"" + f.getHeightByPressure() + "\" gps=\"" + gpsHeight + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n");
71		74
72	w.write(" <status rcQuality=\"" + f.getRcQuality() + "\" fcflags=\"" + f.getFcFlags() + "\" fcflags2=\"" + f.getFcFlags2() + "\" ncflags=\"" + f.getNcFlags() + "\" errorCode=\"" + f.getErrorCode() + "\" numberOfSatellites=\"" + f.getNumberOfSatellites() + "\"/>\n");	75	w.write(" <status rcQuality=\"" + f.getRcQuality() + "\" fcflags=\"" + f.getFcFlags() + "\" fcflags2=\"" + f.getFcFlags2() + "\" ncflags=\"" + f.getNcFlags() + "\" errorCode=\"" + f.getErrorCode() + "\" numberOfSatellites=\"" + f.getNumberOfSatellites() + "\"/>\n");
73	w.write(" <flight time=\"" + f.getFlightTime() + "\" batteryVoltage=\"" + f.getBatteryVoltage() + "\" throttle=\"" + f.getThrottle() + "\" current=\"" + f.getCurrent() + "\" capacityUsed=\"" + f.getCapacityUsed() + "\"/>\n");	76	w.write(" <flight time=\"" + f.getFlightTime() + "\" batteryVoltage=\"" + f.getBatteryVoltage() + "\" throttle=\"" + f.getThrottle() + "\" current=\"" + f.getCurrent() + "\" capacityUsed=\"" + f.getCapacityUsed() + "\"/>\n");
74		77
75	w.write(" <attitude pitch=\"" + f.getPitchAngle() + "\" roll=\"" + f.getRollAngle() + "\"/>\n");	78	w.write(" <attitude pitch=\"" + f.getPitchAngle() + "\" roll=\"" + f.getRollAngle() + "\"/>\n");
76		79
77	/*	80	/*
78	private int operatingRadius;	81	private int operatingRadius;
79	private int targetLoiterTime;	82	private int targetLoiterTime;
80	private int setpointForAltitude; // 16 bit signed.	83	private int setpointForAltitude; // 16 bit signed.
81	*/	84	*/
82		85
83	w.write(" </dataset>\n");	86	w.write(" </dataset>\n");
84	}	87	}
85		88
86	public void end() throws IOException {	89	public void end() throws IOException {
87	w.write("</mk-osddata>\n");	90	w.write("</mk-osddata>\n");
88	w.flush();	91	w.flush();
89	w.close();	92	w.close();
90	}	93	}
91	}	94	}
92		95
93	static class CompositeOSDLogger implements Logger {	96	static class CompositeOSDLogger implements Logger {
94	List<Logger> loggers = new ArrayList<Logger>(2);	97	List<Logger> loggers = new ArrayList<Logger>(2);
95		98
96	public void log(OSDDataResponseFrame f, long timestamp) throws IOException {	99	public void log(OSDDataResponseFrame f, long timestamp) throws IOException {
97	for (Logger l : loggers)	100	for (Logger l : loggers)
98	l.log(f, timestamp);	101	l.log(f, timestamp);
99	}	102	}
100		103
101	public void start(String logId) throws IOException {	104	public void start(String logId) throws IOException {
102	for (Logger l : loggers)	105	for (Logger l : loggers)
103	l.start(logId);	106	l.start(logId);
104	}	107	}
105		108
106	public void end() throws IOException {	109	public void end() throws IOException {
107	for (Logger l : loggers)	110	for (Logger l : loggers)
108	l.end();	111	l.end();
109	}	112	}
110		113
111	void add(Logger l) {	114	void add(Logger l) {
112	loggers.add(l);	115	loggers.add(l);
113	}	116	}
114	}	117	}
115		118
116	private static Logger createLogger(OutputStream out, String encoding) throws IOException {	119	private static Logger createLogger(OutputStream out, String encoding) throws IOException {
117	Logger consoleLogger = new ConsoleOSDLogger();	120	Logger consoleLogger = new ConsoleOSDLogger();
118	XMLOSDLogger xmlLogger = new XMLOSDLogger(out, encoding);	121	XMLOSDLogger xmlLogger = new XMLOSDLogger(out, encoding);
119	CompositeOSDLogger logger = new CompositeOSDLogger();	122	CompositeOSDLogger logger = new CompositeOSDLogger();
120	logger.add(consoleLogger);	123	logger.add(consoleLogger);
121	logger.add(xmlLogger);	124	logger.add(xmlLogger);
122	return logger;	125	return logger;
123	}	126	}
124		127
125	private void prepareLoggers(final FrameQueue q, final Logger logger)	128	private void prepareLoggers(final FrameQueue q, final Logger logger)
126	throws IOException {	129	throws IOException {
127	new Thread() {	130	new Thread() {
128	public void run() {	131	public void run() {
129	}	132	}
130	}.start();	133	}.start();
131	}	134	}
132		135
133	private void osd(final FrameQueue q, final Logger logger, final int interval) throws IOException {	136	private void osd(final FrameQueue q, final Logger logger, final int interval) throws IOException {
134	long timer = 0;	137	long timer = 0;
135	OSDDataRequestFrame f = new OSDDataRequestFrame();	138	OSDDataRequestFrame f = new OSDDataRequestFrame();
136	f.setAutoSendInterval(interval);	139	f.setAutoSendInterval(interval);
137	BufferedReader in = new BufferedReader(new InputStreamReader(System.in));	140	BufferedReader in = new BufferedReader(new InputStreamReader(System.in));
138	Long firsttimestamp = null;	141	Long firsttimestamp = null;
139	while (!in.ready())	142	while (!in.ready())
140	try {	143	try {
141	long now = System.currentTimeMillis();	144	long now = System.currentTimeMillis();
142	if (now - timer > 3000) {	145	if (now - timer > 3000) {
143	timer = now;	146	timer = now;
144	q.sendRequest(f);	147	q.sendRequest(f);
145	}	148	}
146	OSDDataResponseFrame rf = (OSDDataResponseFrame) q.getResponseFor(f, 5000);	149	OSDDataResponseFrame rf = (OSDDataResponseFrame) q.getResponseFor(f, 5000);
147	long timestamp = System.currentTimeMillis();	150	long timestamp = System.currentTimeMillis();
148	if (firsttimestamp == null) {	151	if (firsttimestamp == null) {
149	firsttimestamp = timestamp;	152	firsttimestamp = timestamp;
150	timestamp = 0;	153	timestamp = 0;
151	} else {	154	} else {
152	timestamp -= firsttimestamp;	155	timestamp -= firsttimestamp;
153	}	156	}
154	logger.log(rf, timestamp);	157	logger.log(rf, timestamp);
155	} catch (IOException ex) {	158	} catch (IOException ex) {
156	STDERR.println(ex);	159	STDERR.println(ex);
157	}	160	}
158	}	161	}
159		162
160	public static void main(String[] args) throws IOException {	163	public static void main(String[] args) throws IOException {
161	OSDLogger test = new OSDLogger();	164	OSDLogger test = new OSDLogger();
162		165
163	MKConnection _port = new RXTXSerialPort();	166	MKConnection _port = new MKTCPConnection(); //RXTXSerialPort();
164	_port.init(null);	167	_port.init("8080");
165		168
166	FrameQueue q = new FrameQueue(_port);	169	FrameQueue q = new FrameQueue(_port);
167	String encoding = "iso-8859-1";	170	String encoding = "iso-8859-1";
168		171
169	OutputStream fout = new FileOutputStream("osd.xml");	172	OutputStream fout = new FileOutputStream("osd.xml");
170		173
171	Logger logger = createLogger(fout, encoding);	174	Logger logger = createLogger(fout, encoding);
172	logger.start(new Date().toGMTString());	175	logger.start(new Date().toGMTString());
173	test.osd(q, logger, 20);	176	test.osd(q, logger, 20);
174	logger.end();	177	logger.end();
175	fout.close();	178	fout.close();
176	}	179	}
177	}	180	}
178		181

Subversion Repositories Projects

(root)/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/OSDLogger.java - Rev 1564 → 1568