0,0 → 1,180 |
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.frames.OSDDataRequestFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame.GPSDistanceAndBearing; |
import dongfang.mkt.frames.OSDDataResponseFrame.GPSPosition; |
import dongfang.mkt.io.MKCommPort; |
import dongfang.mkt.io.RXTXSerialPort; |
import dongfang.mkt.serial.FrameQueue; |
|
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(" <height barometric=\"" + f.getHeightByPressure() + "\"/>\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(); |
|
MKCommPort 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(); |
} |
} |