Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1561 → Rev 1562

/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/MKDebugLogger.java
43,11 → 43,17
public void start(String logId) {}
 
public void log(DebugResponseFrame f, long timestamp) {
for (int i = 0; i < f.getDigital().length; i++) {
if (f == null) {
System.out.println("Oops, null response frame.");
return;
}
int l = f.getDigital()==null ? 0 : f.getDigital().length;
for (int i = 0; i < l; i++) {
System.out.println("Digital " + i + ":\t" + f.getDigital()[i]);
}
 
for (int i = 0; i < f.getAnalog().length; i++) {
l = f.getAnalog()==null ? 0 : f.getAnalog().length;
for (int i = 0; i < l; i++) {
String label = labels[i] == null ? ("Analog " + i) : labels[i];
System.out.println(label + ":\t" + f.getAnalog()[i]);
}
147,7 → 153,7
}
 
int tries = 0;
while (!missing.isEmpty() && tries < 100) {
while (!missing.isEmpty() && tries < 300) {
int i = missing.get(0);
tries++;
AnalogDebugLabelRequestFrame f2 = //ff.createAnalogDebugLabelRequestFrame(Frame.FC_ADDRESS, i);
154,9 → 160,10
new AnalogDebugLabelRequestFrame(Frame.FC_ADDRESS, i);
try {
q.sendRequest(f2);
AnalogDebugLabelResponseFrame rf = (AnalogDebugLabelResponseFrame) q.getResponseFor(f2, 1000);
AnalogDebugLabelResponseFrame rf = (AnalogDebugLabelResponseFrame) q.getResponseFor(f2, 5000);
if (rf != null) {
logger.setLabel(i, rf.getLabelAsString());
missing.remove(0);
} else {
String unknown = "analog " + i;
logger.setLabel(i, unknown);
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/OSDLogger.java
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();
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/UniversalConfigurator.java
70,7 → 70,7
UniversalReadParamSetRequestFrame frame = new UniversalReadParamSetRequestFrame();
frame.setConfigurationSetNumber(parameterSetNumber);
q.sendRequest(frame);
UniversalReadParamSetResponseFrame r = (UniversalReadParamSetResponseFrame) q.getResponseFor(frame, 3000);
UniversalReadParamSetResponseFrame r = (UniversalReadParamSetResponseFrame) q.getResponseFor(frame, 5000);
if (r == null) {
System.err.println("ERROR. Timeout waiting for response.");
} else {
244,7 → 244,7
if ("w".equals(args[0]) && (args.length!=3 && args.length!=4)) help();
if ("r".equals(args[0]) && (args.length!=3 && args.length!=4)) help();
 
String portIdentifier = "COM10";
String portIdentifier = null;
if ("r".equals(args[0])) {
readConfiguration(portIdentifier, Integer.parseInt(args[1]));