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