Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1560 → Rev 1562

/dongfang_FC_rewrite_tool/src/dongfang/mkt/RequestFrameVisitor.java
6,6 → 6,7
import dongfang.mkt.frames.AnalogDebugLabelRequestFrame;
import dongfang.mkt.frames.AttitudeDataRequestFrame;
import dongfang.mkt.frames.ChangeParameterSetRequestFrame;
import dongfang.mkt.frames.CompassHeadingRequestFrame;
import dongfang.mkt.frames.DebugRequestFrame;
import dongfang.mkt.frames.ExternalControlRequestFrame;
import dongfang.mkt.frames.LoopbackTestRequestFrame;
39,4 → 40,5
void visit(SetCompassHeadingRequestFrame f) throws IOException;
void visit(ReadExternalControlRequestFrame f) throws IOException;
void visit(OSDDataRequestFrame f) throws IOException;
void visit(CompassHeadingRequestFrame f) throws IOException;
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/CompassHeadingRequestFrame.java
0,0 → 1,17
package dongfang.mkt.frames;
 
import java.io.IOException;
 
import dongfang.mkt.RequestFrameVisitor;
 
public class CompassHeadingRequestFrame extends RequestFrame {
 
public CompassHeadingRequestFrame() {
super(MK3MAG_ADDRESS);
}
 
@Override
public void accept(RequestFrameVisitor o) throws IOException {
o.visit(this);
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/CompassHeadingResponseFrame.java
1,6 → 1,11
package dongfang.mkt.frames;
 
public class CompassHeadingResponseFrame extends ResponseFrame {
int[] angle= new int[2];
int[] user = new int[2];
int calcState;
int orientation;
public CompassHeadingResponseFrame(int address) {
super(address);
}
7,6 → 12,6
 
@Override
public boolean isResponseTo(RequestFrame r) {
return false ;//r instanceof CompassHeadingRequestFrame;
return r instanceof CompassHeadingRequestFrame;
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/Frame.java
72,18 → 72,6
public char getId() { return id; }
}
/*
public ResponseFrame getResponseFrameInstance(RESPONSE_IDS id) {
switch (id) {
case ANALOG_DEBUG_LABEL: return new AnalogDebugLabelResponseFrame();
case EXTERNAL_CONTROL: return new ExternalControlResponseFrame();
case DISPLAY1: return new Display1ResponseFrame();
case DISPLAY2: return new Display2ResponseFrame();
case VERSION: return new VersionResponseFrame();
}
}
*/
protected final int address;
protected Frame(int address) {
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/OSDDataResponseFrame.java
31,6 → 31,29
public void setStatus(int status) {
this.status = status;
}
public String toXML() {
double latitude = Math.abs(this.latitude);
String result = "latitude=\"" + latitude/1E7;
if (this.latitude < 0) result+= "S"; else result+="N";
result += "\"";
double longitude= Math.abs(this.longitude);
result += " longitude=\"" + longitude/1E7;
if (this.longitude < 0) result+= "W"; else result+="E";
result += "\"";
return result;
}
 
public String toString() {
double latitude = Math.abs(this.latitude);
String result = "" + latitude/1E7;
if (this.latitude < 0) result+= "S"; else result+="N";
result += " ";
double longitude= Math.abs(this.longitude);
result += "" + longitude/1E7;
if (this.longitude < 0) result+= "W"; else result+="E";
return result;
}
}
public static class GPSDistanceAndBearing {
48,6 → 71,20
public void setBearing(int bearing) {
this.bearing = bearing;
}
public String toXML() {
String result = "distance=\"" + ((double)this.distance)/10;
result += "\" bearing=\"";
result += this.bearing;
result += "\"";
return result;
}
public String toString() {
String result = "" + ((double)this.distance)/10;
result += "m @";
result += this.bearing;
result += "deg";
return result;
}
}
 
private int version;
68,7 → 105,7
private int batteryVoltage; // in 0.1 volts.
 
private int groundSpeed; // in cm/s. 16 bit unsigned.
private int heading; // of movement. 16 bit signed.
private int directionOfFlight; // of movement. 16 bit signed.
private int compassHeading;// 16 bit signed.
private int pitchAngle;
136,8 → 173,8
public int getGroundSpeed() {
return groundSpeed;
}
public int getHeading() {
return heading;
public int getDirectionOfFlight() {
return directionOfFlight;
}
public int getCompassHeading() {
return compassHeading;
226,8 → 263,8
public void setGroundSpeed(int groundSpeed) {
this.groundSpeed = groundSpeed;
}
public void setHeading(int heading) {
this.heading = heading;
public void setDirectionOfFlight(int heading) {
this.directionOfFlight = heading;
}
public void setCompassHeading(int compassHeading) {
this.compassHeading = compassHeading;
274,5 → 311,28
public void setCapacityUsed(int capacityUsed) {
this.capacityUsed = capacityUsed;
}
 
@Override
public boolean isResponseTo(RequestFrame r) {
return r instanceof OSDDataRequestFrame;
}
public String toString() {
StringBuilder result = new StringBuilder();
result.append("OSDData version=" + this.version + "\n");
result.append("currentPosition: " + this.currentPosition + "\n");
result.append("targetPosition: " + this.targetPosition + " (" + this.currentToTarget + ")" + "\n");
result.append("homePosition: " + this.homePosition + " (" + this.currentToHome + ")" + "\n");
result.append("heightByPressure: " + this.heightByPressure + "\n");
result.append("verticalVelocityByPressure: " + this.verticalVelocityByPressure + "\n");
result.append("verticalVelocityByGPS: " + this.verticalVelocityByGPS + "\n");
result.append("direction of flight: " + this.directionOfFlight + "\n");
result.append("compassHeading: " + this.compassHeading + "\n");
result.append("pitchAngle: " + this.pitchAngle + "\n");
result.append("rollAngle: " + this.rollAngle + "\n");
result.append("battery voltage: " + ((double)this.batteryVoltage)/10 + "\n");
result.append("throttle: " + this.throttle + "\n");
return result.toString();
}
}
 
/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]));
/dongfang_FC_rewrite_tool/src/dongfang/mkt/serial/MKInputStream.java
8,6 → 8,7
import dongfang.mkt.frames.AnalogDebugLabelResponseFrame;
import dongfang.mkt.frames.AttitudeDataResponseFrame;
import dongfang.mkt.frames.ChangeParameterSetResponseFrame;
import dongfang.mkt.frames.CompassHeadingResponseFrame;
import dongfang.mkt.frames.ConfirmFrame;
import dongfang.mkt.frames.DebugResponseFrame;
import dongfang.mkt.frames.MotorTestResponseFrame;
98,6 → 99,14
return result;
}
 
public int[] readSignedWords(int length) throws IOException {
int[] result = new int[length];
for (int i = 0; i < length; i++) {
result[i] = readSignedWord();
}
return result;
}
 
public char[] readChars(int length) throws IOException {
char[] result = new char[length];
for (int i = 0; i < length; i++) {
154,6 → 163,7
readByteCnt = 0;
//RESPONSE_IDS id = getResponseType(iid);
ResponseFrame result;
// System.out.println("Received a: " + (char)iid + " from " + address);
switch (iid) {
case 'A': {
AnalogDebugLabelResponseFrame f = new AnalogDebugLabelResponseFrame(address);
215,6 → 225,15
result = f;
break;
}
case 'k' : {
CompassHeadingResponseFrame f = new CompassHeadingResponseFrame(address);
base64InputStream.readSignedWords(2);
base64InputStream.readBytes(2);
base64InputStream.readByte();
base64InputStream.readByte();
result = f;
break;
}
case 'L': {
AllDisplaysResponseFrame f = new AllDisplaysResponseFrame(address);
f.setItem(base64InputStream.readByte());
268,7 → 287,7
f.setBatteryVoltage(base64InputStream.readByte());
f.setGroundSpeed(base64InputStream.readWord());
f.setHeading(base64InputStream.readSignedWord());
f.setDirectionOfFlight(base64InputStream.readSignedWord());
f.setCompassHeading(base64InputStream.readSignedWord());
f.setPitchAngle(base64InputStream.readSignedByte());
355,6 → 374,14
result = f;
break;
default:
int b;
int count = 0;
while((b=read()) != '\r') {
count++;
}
System.err.println("Unknown frame " + (char)iid + " received from " + address);
System.err.println("It appears to have " + (count-2) + " data bytes (encoded)");
System.err.println("(" + (count-2) * 6/8 + " data bytes decoded)");
result = null;
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/serial/MKOutputStream.java
8,6 → 8,7
import dongfang.mkt.frames.AnalogDebugLabelRequestFrame;
import dongfang.mkt.frames.AttitudeDataRequestFrame;
import dongfang.mkt.frames.ChangeParameterSetRequestFrame;
import dongfang.mkt.frames.CompassHeadingRequestFrame;
import dongfang.mkt.frames.DebugRequestFrame;
import dongfang.mkt.frames.ExternalControlRequestFrame;
import dongfang.mkt.frames.LoopbackTestRequestFrame;
63,7 → 64,7
}
 
MKOutputStream.this.writeByte((inbuf[0] >>> 2) + '=');
MKOutputStream.this.writeByte(( ((inbuf[0] & 0x03) << 4) | (inbuf[1] >>> 4) ) + '=');;
MKOutputStream.this.writeByte(( ((inbuf[0] & 0x03) << 4) | (inbuf[1] >>> 4) ) + '=');
MKOutputStream.this.writeByte(( ((inbuf[1] & 0x0f) << 2) | (inbuf[2] >>> 6)) + '=');
MKOutputStream.this.writeByte(((inbuf[2] & 0x3f) + '='));
 
206,6 → 207,10
writeByte('K');
}
 
public void visit(CompassHeadingRequestFrame f) throws IOException {
writeByte('w');
}
 
public void visit(OSDDataRequestFrame f) throws IOException {
writeByte('o');
base64OutputStream.writeByte(f.getAutoSendInterval());