Subversion Repositories Projects

Rev

Rev 1563 | Go to most recent revision | Details | Last modification | View Log | RSS feed

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