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