/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()); |