Subversion Repositories Projects

Rev

Rev 1564 | Details | Compare with Previous | 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
 
1563 - 15
import dongfang.mkt.comm.FrameQueue;
16
import dongfang.mkt.comm.MKConnection;
17
import dongfang.mkt.comm.serial.RXTXSerialPort;
1568 - 18
import dongfang.mkt.comm.tcp.MKTCPConnection;
1562 - 19
import dongfang.mkt.frames.OSDDataRequestFrame;
20
import dongfang.mkt.frames.OSDDataResponseFrame;
21
 
22
public class OSDLogger {
23
        private static final PrintStream STDERR = System.out;
24
        // private static final FrameFactory ff = MKVersion.getFrameFactory(null);
25
 
26
        interface Logger {
27
                void start(String timestamp) throws IOException;
28
                void log(OSDDataResponseFrame f, long timestamp) throws IOException;
29
                void end() throws IOException;
30
        }
31
 
32
        static class ConsoleOSDLogger implements Logger {
33
                public void start(String logId) {}
34
 
35
                public void log(OSDDataResponseFrame f, long timestamp) {
36
                        if (f == null) {
37
                                System.out.println("Oops, null response frame.");
38
                                return;
39
                        }
40
                                System.out.println(f);
41
                }
42
 
43
                public void end() {}
44
        }
45
 
46
        static class XMLOSDLogger implements Logger {
47
                Writer w;
48
                String encoding;
49
 
50
                XMLOSDLogger(OutputStream out, String encoding) throws IOException {
51
                        w = new OutputStreamWriter(out, encoding);
52
                        this.encoding = encoding;
53
                }
54
 
55
                public void start(String logId) throws IOException {
56
                        w.write("<?xml version=\"1.0\" encoding=\"" + encoding + "\"?>\n");
57
                        w.write("<mk-osddata time=\"" + logId + "\">\n");
58
                }
59
 
60
                public void log(OSDDataResponseFrame f, long timestamp)
61
                                throws IOException {
62
                        if (f==null)
63
                                return;
64
                        w.write("  <dataset timestamp=\"" + timestamp + "\" version=\"" + f.getVersion() + "\">\n");
65
                        w.write("    <currentPosition " + f.getCurrentPosition().toXML() + "/>\n");
66
                        w.write("    <targetPosition " + f.getTargetPosition().toXML() + " " + f.getCurrentToTarget().toXML() + "/>\n");
67
                        w.write("    <homePosition " + f.getHomePosition().toXML() + " " + f.getCurrentToHome().toXML() + "/>\n");
1568 - 68
                        w.write("    <heading compass=\"" + f.getCompassHeading() + "\" flight=\"" + f.getDirectionOfFlight() + "\" groundSpeed=\"" + f.getGroundSpeed() + "\"/>\n");
1562 - 69
 
70
                        w.write("    <waypoints index=\"" + f.getWaypointIndex() + "\" count=\"" + f.getWaypointCount() + "\"/>\n");
1568 - 71
 
72
                        double gpsHeight = f.getCurrentPosition().getAltitude() - f.getHomePosition().getAltitude();
73
                        w.write("    <height barometric=\"" + f.getHeightByPressure() + "\" gps=\"" + gpsHeight + "\" barometricVerticalVelocity=\"" + f.getVerticalVelocityByPressure() + "\" gpsVerticalVelocity=\"" + f.getVerticalVelocityByGPS() + "\"/>\n");
1562 - 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
 
1568 - 166
                MKConnection _port = new MKTCPConnection(); //RXTXSerialPort();
167
                _port.init("8080");
1562 - 168
 
1563 - 169
                FrameQueue q = new FrameQueue(_port);
1562 - 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
}