Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1562 → Rev 1561

/dongfang_FC_rewrite_tool/src/dongfang/mkt/RequestFrameVisitor.java
6,7 → 6,6
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;
40,5 → 39,4
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
File deleted
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/CompassHeadingResponseFrame.java
1,11 → 1,6
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);
}
12,6 → 7,6
 
@Override
public boolean isResponseTo(RequestFrame r) {
return r instanceof CompassHeadingRequestFrame;
return false ;//r instanceof CompassHeadingRequestFrame;
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/Frame.java
72,6 → 72,18
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,29 → 31,6
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 {
71,20 → 48,6
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;
105,7 → 68,7
private int batteryVoltage; // in 0.1 volts.
 
private int groundSpeed; // in cm/s. 16 bit unsigned.
private int directionOfFlight; // of movement. 16 bit signed.
private int heading; // of movement. 16 bit signed.
private int compassHeading;// 16 bit signed.
private int pitchAngle;
173,8 → 136,8
public int getGroundSpeed() {
return groundSpeed;
}
public int getDirectionOfFlight() {
return directionOfFlight;
public int getHeading() {
return heading;
}
public int getCompassHeading() {
return compassHeading;
263,8 → 226,8
public void setGroundSpeed(int groundSpeed) {
this.groundSpeed = groundSpeed;
}
public void setDirectionOfFlight(int heading) {
this.directionOfFlight = heading;
public void setHeading(int heading) {
this.heading = heading;
}
public void setCompassHeading(int compassHeading) {
this.compassHeading = compassHeading;
311,28 → 274,5
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/OSDLogger.java
File deleted
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/MKDebugLogger.java
43,17 → 43,11
public void start(String logId) {}
 
public void log(DebugResponseFrame f, long timestamp) {
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++) {
for (int i = 0; i < f.getDigital().length; i++) {
System.out.println("Digital " + i + ":\t" + f.getDigital()[i]);
}
 
l = f.getAnalog()==null ? 0 : f.getAnalog().length;
for (int i = 0; i < l; i++) {
for (int i = 0; i < f.getAnalog().length; i++) {
String label = labels[i] == null ? ("Analog " + i) : labels[i];
System.out.println(label + ":\t" + f.getAnalog()[i]);
}
153,7 → 147,7
}
 
int tries = 0;
while (!missing.isEmpty() && tries < 300) {
while (!missing.isEmpty() && tries < 100) {
int i = missing.get(0);
tries++;
AnalogDebugLabelRequestFrame f2 = //ff.createAnalogDebugLabelRequestFrame(Frame.FC_ADDRESS, i);
160,10 → 154,9
new AnalogDebugLabelRequestFrame(Frame.FC_ADDRESS, i);
try {
q.sendRequest(f2);
AnalogDebugLabelResponseFrame rf = (AnalogDebugLabelResponseFrame) q.getResponseFor(f2, 5000);
AnalogDebugLabelResponseFrame rf = (AnalogDebugLabelResponseFrame) q.getResponseFor(f2, 1000);
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/UniversalConfigurator.java
70,7 → 70,7
UniversalReadParamSetRequestFrame frame = new UniversalReadParamSetRequestFrame();
frame.setConfigurationSetNumber(parameterSetNumber);
q.sendRequest(frame);
UniversalReadParamSetResponseFrame r = (UniversalReadParamSetResponseFrame) q.getResponseFor(frame, 5000);
UniversalReadParamSetResponseFrame r = (UniversalReadParamSetResponseFrame) q.getResponseFor(frame, 3000);
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 = null;
String portIdentifier = "COM10";
if ("r".equals(args[0])) {
readConfiguration(portIdentifier, Integer.parseInt(args[1]));
/dongfang_FC_rewrite_tool/src/dongfang/mkt/serial/MKInputStream.java
8,7 → 8,6
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;
99,14 → 98,6
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++) {
163,7 → 154,6
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);
225,15 → 215,6
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());
287,7 → 268,7
f.setBatteryVoltage(base64InputStream.readByte());
f.setGroundSpeed(base64InputStream.readWord());
f.setDirectionOfFlight(base64InputStream.readSignedWord());
f.setHeading(base64InputStream.readSignedWord());
f.setCompassHeading(base64InputStream.readSignedWord());
f.setPitchAngle(base64InputStream.readSignedByte());
374,14 → 355,6
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,7 → 8,6
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;
64,7 → 63,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) + '='));
 
207,10 → 206,6
writeByte('K');
}
 
public void visit(CompassHeadingRequestFrame f) throws IOException {
writeByte('w');
}
 
public void visit(OSDDataRequestFrame f) throws IOException {
writeByte('o');
base64OutputStream.writeByte(f.getAutoSendInterval());