/dongfang_FC_rewrite_tool/configsets/FC2.0_Quadcopter_imu.xml |
---|
File deleted |
/dongfang_FC_rewrite_tool/configsets/FC2.0_Quadcopter.xml |
---|
File deleted |
/dongfang_FC_rewrite_tool/configsets/ENC-03/configset.xml |
---|
11,15 → 11,15 |
</parameter> |
<parameter name="levelCorrectionPitch" value="128"/> |
<parameter name="levelCorrectionRoll" value="128"/> |
<parameter name="gyroP" value="160"/> |
<parameter name="gyroP" value="120"/> |
<parameter name="gyroI" value="80"/> |
<parameter name="gyroD" value="4"/> |
<parameter name="attitudeControl" value="0"/> |
<parameter name="stickP" value="4"/> |
<parameter name="stickP" value="6"/> |
<parameter name="stickD" value="4"/> |
<parameter name="stickYawP" value="8"/> |
<parameter name="stickThrottleD" value="4"/> |
<parameter name="minThrottle" value="8"/> |
<parameter name="minThrottle" value="20"/> |
<parameter name="maxThrottle" value="230"/> |
<parameter name="externalControl" value="0"/> |
<parameter name="motorSmoothing" value="0"/> |
/dongfang_FC_rewrite_tool/configsets/ENC-03/imu.xml |
---|
1,9 → 1,9 |
<imuconfiguration eepromVersion="1" length="12" name="ENC-03/FC1.3"> |
<imuconfiguration eepromVersion="0" length="11" name="ENC-03/FC1.3"> |
<parameter name="gyroQuadrant" value="0" /> |
<parameter name="accQuadrant" value="4" /> |
<parameter name="imuReversedFlags"> |
<bit name="IMU_REVERSE_GYRO_PR" value="0" /> |
<bit name="IMU_REVERSE_GYRO_YAW" value="0" /> |
<bit name="IMU_REVERSE_GYRO_YAW" value="1" /> |
<bit name="IMU_REVERSE_ACC_XY" value="1" /> |
<bit name="IMU_REVERSE_ACC_Z" value="0" /> |
<bit name="Unused" value="0" /> |
14,11 → 14,10 |
<parameter name="gyroPIDFilterConstant" value="1" /> |
<parameter name="gyroDWindowLength" value="8" /> |
<parameter name="gyroDFilterConstant" value="0" /> |
<parameter name="accFilterConstant" value="15" /> |
<parameter name="zerothOrderCorrection" value="10" /> |
<parameter name="rateTolerance" value="100" /> |
<parameter name="zerothOrderCorrection" value="80" /> |
<parameter name="rateTolerance" value="240" /> |
<parameter name="gyroActivityDamping" value="64" /> |
<parameter name="driftCompDivider" value="2" /> |
/dongfang_FC_rewrite_tool/configsets/FC2.0/FC2.0_Quadcopter.xml |
---|
0,0 → 1,154 |
<parameterset eepromVersion="3" length="73" name="hundeprut"> |
<parameter name="bitConfig"> |
<bit name="CFG_SIMPLE_HEIGHT_CONTROL" value="0" /> |
<bit name="CFG_SIMPLE_HC_HOLD_SWITCH" value="0" /> |
<bit name="CFG_HEADING_HOLD" value="1" /> |
<bit name="CFG_COMPASS_ENABLED" value="0" /> |
<bit name="CFG_UNUSED" value="0" /> |
<bit name="CFG_NAVI_ENABLED" value="0" /> |
<bit name="CFG_AXIS_COUPLING_ENABLED" value="0" /> |
<bit name="CFG_GYRO_SATURATION_PREVENTION" value="1" /> |
</parameter> |
<parameter name="levelCorrectionPitch" value="128" /> |
<parameter name="levelCorrectionRoll" value="128" /> |
<parameter name="gyroP" value="80" /> |
<parameter name="gyroI" value="60" /> |
<parameter name="gyroD" value="15" /> |
<parameter name="attitudeControl" value="0" /> |
<parameter name="stickP" value="6" /> |
<parameter name="stickD" value="2" /> |
<parameter name="stickYawP" value="8" /> |
<parameter name="stickThrottleD" value="4" /> |
<parameter name="minThrottle" value="20" /> |
<parameter name="maxThrottle" value="220" /> |
<parameter name="externalControl" value="0" /> |
<parameter name="motorSmoothing" value="0" /> |
<parameter name="dynamicStability" value="50" /> |
<parameter name="IFactor" value="30" /> |
<parameter name="yawIFactor" value="60" /> |
<parameter name="compassMode" value="0" /> |
<parameter name="compassYawCorrection" value="20" /> |
<parameter name="compassBendingReturnSpeed" value="16" /> |
<parameter name="compassP" value="50" /> |
<parameter name="batteryVoltageWarning" value="100" /> |
<parameter name="emergencyThrottle" value="40" /> |
<parameter name="emergencyFlightDuration" value="30" /> |
<parameter name="airpressureFilterConstant" value="8" /> |
<parameter name="airpressureWindowLength" value="0" /> |
<parameter name="airpressureDWindowLength" value="24" /> |
<parameter name="airpressureAccZCorrection" value="184" /> <!-- 184 for my 2.0 board. --> |
<parameter name="heightP" value="110" /> |
<parameter name="heightI" value="90" /> |
<parameter name="heightD" value="25" /> <!-- 30 --> |
<parameter name="heightSetting" value="var0" /> |
<parameter name="heightControlMaxIntegralIn" value="125" /> |
<parameter name="heightControlMaxIntegralOut" value="75" /> |
<parameter name="heightMaxThrottleChange" value="75" /> |
<parameter name="heightControlTestOscPeriod" value="0" /> |
<parameter name="heightControlTestOscAmplitude" value="0" /> |
<parameter name="servoCount" value="7" /> |
<parameter name="servoManualMaxSpeed" value="10" /> |
<parameter name="pitchServoControl" value="var1" /> |
<parameter name="pitchServoStabilization" value="45" /> |
<parameter name="pitchServoMinValue" value="20" /> |
<parameter name="pitchServoMaxValue" value="235" /> |
<parameter name="pitchServoFlags"> |
<bit name="reverseStabilization" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
</parameter> |
<parameter name="rollServoControl" value="var2" /> |
<parameter name="rollServoStabilization" value="120" /> |
<parameter name="rollServoMinValue" value="20" /> |
<parameter name="rollServoMaxValue" value="235" /> |
<parameter name="rollServoFlags"> |
<bit name="reverseStabilization" value="1" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
</parameter> |
<parameter name="output0BitMask"> |
<bit name="t0" value="1" /> |
<bit name="t1" value="1" /> |
<bit name="t2" value="1" /> |
<bit name="t3" value="1" /> |
<bit name="t4" value="1" /> |
<bit name="t5" value="1" /> |
<bit name="t6" value="0" /> |
<bit name="t7" value="0" /> |
</parameter> |
<parameter name="output0Timing" value="15" /> |
<parameter name="output1BitMask"> |
<bit name="t0" value="1" /> |
<bit name="t1" value="1" /> |
<bit name="t2" value="1" /> |
<bit name="t3" value="1" /> |
<bit name="t4" value="1" /> |
<bit name="t5" value="1" /> |
<bit name="t6" value="0" /> |
<bit name="t7" value="0" /> |
</parameter> |
<parameter name="output1Timing" value="14" /> |
<parameter name="outputDebugMask"> |
<bit name="DEBUG_MAINLOOP_TIMER" value="0" /> |
<bit name="DEBUG_HEIGHT_DIFF" value="0" /> |
<bit name="DEBUG_HOVERTHROTTLE" value="0" /> |
<bit name="DEBUG_ACC0THORDER" value="0" /> |
<bit name="DEBUG_COMPASS" value="0" /> |
<bit name="DEBUG_PRESSURERANGE" value="0" /> |
<bit name="DEBUG_CLIP" value="0" /> |
<bit name="DEBUG_SENSORLIMIT" value="0" /> |
</parameter> |
<parameter name="outputFlags"> |
<bit name="INVERT_OUTPUT0" value="0" /> |
<bit name="INVERT_OUTPUT1" value="0" /> |
<bit name="FLASH_OUTPUT0_BEEPING" value="1" /> |
<bit name="FLASH_OUTPUT1_BEEPING" value="1" /> |
<bit name="USE_ONBOARD_LEDS" value="1" /> |
<bit name="TEST_OFF" value="0" /> |
<bit name="TEST_ON" value="0" /> |
<bit name="UNUSED" value="0" /> |
</parameter> |
<parameter name="naviMode" value="0" /> |
<parameter name="naviStickThreshold" value="50" /> |
<parameter name="naviStickLimit" value="80" /> |
<parameter name="GPSMinimumSatellites" value="6" /> |
<parameter name="naviP" value="30" /> |
<parameter name="naviI" value="120" /> |
<parameter name="naviD" value="65" /> |
<parameter name="naviTestOscPeriod" value="30" /> |
<parameter name=" naviTestOscAmplitude" value="30" /> |
<parameter name="userparam0" value="0" /> |
<parameter name="userparam1" value="0" /> |
<parameter name="userparam2" value="0" /> |
<parameter name="userparam3" value="0" /> |
<parameter name="userparam4" value="0" /> |
<parameter name="userparam5" value="0" /> |
<parameter name="userparam6" value="0" /> |
<parameter name="userparam7" value="0" /> |
</parameterset> |
/dongfang_FC_rewrite_tool/configsets/FC2.0/imu.xml |
---|
0,0 → 1,25 |
<imuconfiguration eepromVersion="0" length="11" > |
<parameter name="gyroQuadrant" value="0" /> |
<parameter name="accQuadrant" value="4" /> |
<parameter name="imuReversedFlags" type="bitset"> |
<bit name="IMU_REVERSE_GYRO_PR" value="0"/> |
<bit name="IMU_REVERSE_GYRO_YAW" value="0" /> |
<bit name="IMU_REVERSE_ACC_XY" value="1" /> |
<bit name="IMU_REVERSE_ACC_Z" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
<bit name="Unused" value="0" /> |
</parameter> |
<parameter name="gyroPIDFilterConstant" value="4" /> |
<parameter name="gyroDWindowLength" value="3" /> |
<parameter name="accFilterConstant" value="10" /> |
<parameter name="zerothOrderCorrection" value="2" /> |
<parameter name="rateTolerance" value="120" /> |
<parameter name="gyroActivityDamping" value="64" /> |
<parameter name="driftCompDivider" value="2" /> |
<parameter name="driftCompLimit" value="2" /> |
</imuconfiguration> |
/dongfang_FC_rewrite_tool/configsets/motorMixers/quadro+.xml |
---|
File deleted |
/dongfang_FC_rewrite_tool/configsets/motorMixers/quadro-+.xml |
---|
0,0 → 1,14 |
<motorMixer eepromVersion="1" name="Quadro +"> |
<motor throttlePart="64" pitchPart="64" rollPart="0" yawPart="64" oppositeMotor="1" /> |
<motor throttlePart="64" pitchPart="-64" rollPart="0" yawPart="64" oppositeMotor="0" /> |
<motor throttlePart="64" pitchPart="0" rollPart="-64" yawPart="-64" oppositeMotor="3" /> |
<motor throttlePart="64" pitchPart="0" rollPart="64" yawPart="-64" oppositeMotor="2" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
</motorMixer> |
/dongfang_FC_rewrite_tool/configsets/motorMixers/quadro-x-rot90.xml |
---|
0,0 → 1,15 |
<motorMixer eepromVersion="1" name="Quadro X"> |
<motor throttlePart="64" pitchPart="64" rollPart="-64" yawPart="-64" oppositeMotor="2" /> |
<motor throttlePart="64" pitchPart="-64" rollPart="64" yawPart="64" oppositeMotor="1" /> |
<motor throttlePart="64" pitchPart="-64" rollPart="-64" yawPart="-64" oppositeMotor="3" /> |
<motor throttlePart="64" pitchPart="64" rollPart="64" yawPart="64" oppositeMotor="0" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
</motorMixer> |
/dongfang_FC_rewrite_tool/configsets/motorMixers/quadro-x.xml |
---|
0,0 → 1,14 |
<motorMixer eepromVersion="1" name="Quadro X"> |
<motor throttlePart="64" pitchPart="64" rollPart="64" yawPart="64" oppositeMotor="1" /> |
<motor throttlePart="64" pitchPart="-64" rollPart="-64" yawPart="64" oppositeMotor="0" /> |
<motor throttlePart="64" pitchPart="64" rollPart="-64" yawPart="-64" oppositeMotor="3" /> |
<motor throttlePart="64" pitchPart="-64" rollPart="64" yawPart="-64" oppositeMotor="2" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
<motor throttlePart="0" pitchPart="0" rollPart="0" yawPart="0" oppositeMotor="-1" /> |
</motorMixer> |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/RequestFrameVisitor.java |
---|
7,6 → 7,7 |
import dongfang.mkt.frames.AttitudeDataRequestFrame; |
import dongfang.mkt.frames.ChangeParameterSetRequestFrame; |
import dongfang.mkt.frames.CompassHeadingRequestFrame; |
import dongfang.mkt.frames.DCMMatrixRequestFrame; |
import dongfang.mkt.frames.DebugRequestFrame; |
import dongfang.mkt.frames.ExternalControlRequestFrame; |
import dongfang.mkt.frames.LoopbackTestRequestFrame; |
51,4 → 52,5 |
void visit(WriteMotorMixerRequestFrame f) throws IOException; |
void visit(WriteIMUConfigurationRequestFrame f) throws IOException; |
void visit(ReadRCChannelsRequestFrame f) throws IOException; |
void visit(DCMMatrixRequestFrame f) throws IOException; |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKInputStream.java |
---|
12,6 → 12,7 |
import dongfang.mkt.frames.ChangeParameterSetResponseFrame; |
import dongfang.mkt.frames.CompassHeadingResponseFrame; |
import dongfang.mkt.frames.ConfirmFrame; |
import dongfang.mkt.frames.DCMMatrixResponseFrame; |
import dongfang.mkt.frames.DebugResponseFrame; |
import dongfang.mkt.frames.MotorTestResponseFrame; |
import dongfang.mkt.frames.OSDDataResponseFrame; |
122,8 → 123,21 |
} |
return result; |
} |
} |
public float readFloat() throws IOException { |
int asInt = readByte() | (readByte()<<8) | (readByte()<<16) | (readByte()<<24); |
return Float.intBitsToFloat(asInt); |
} |
public float[] readFloats(int length) throws IOException { |
float[] result = new float[length]; |
for (int i = 0; i < length; i++) { |
result[i] = readFloat(); |
} |
return result; |
} |
} |
MKDataInputStream base64InputStream = new MKDataInputStream(); |
OutputStream nonPacketSpillway = null; //System.err; |
185,10 → 199,9 |
} |
case 'C': { |
AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
f.setPitch(base64InputStream.readSignedWord()); |
f.setRoll(base64InputStream.readSignedWord()); |
f.setHeading(base64InputStream.readSignedWord()); |
f.setExpansion(base64InputStream.readBytes(8)); |
f.setAttitude(base64InputStream.readFloats(3)); |
f.setRates(base64InputStream.readFloats(3)); |
f.setAcc(base64InputStream.readFloats(3)); |
result = f; |
break; |
} |
201,6 → 214,17 |
result = f; |
break; |
} |
case 'E': { |
DCMMatrixResponseFrame f= new DCMMatrixResponseFrame(address); |
float[][] matrix = new float[3][]; |
for (int i=0; i<3; i++) { |
float[] row = base64InputStream.readFloats(3); |
matrix[i] = row; |
} |
f.setMatrix(matrix); |
result = f; |
break; |
} |
case 'F': { |
ChangeParameterSetResponseFrame f = new ChangeParameterSetResponseFrame(address); |
f.setParameterSetNumber(base64InputStream.readByte()); |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKOutputStream.java |
---|
9,6 → 9,7 |
import dongfang.mkt.frames.AttitudeDataRequestFrame; |
import dongfang.mkt.frames.ChangeParameterSetRequestFrame; |
import dongfang.mkt.frames.CompassHeadingRequestFrame; |
import dongfang.mkt.frames.DCMMatrixRequestFrame; |
import dongfang.mkt.frames.DebugRequestFrame; |
import dongfang.mkt.frames.ExternalControlRequestFrame; |
import dongfang.mkt.frames.LoopbackTestRequestFrame; |
142,6 → 143,10 |
base64OutputStream.writeByte(f.getAutoSendInterval()); |
} |
public void visit(DCMMatrixRequestFrame f) throws IOException { |
writeByte('e'); |
} |
public void visit(ChangeParameterSetRequestFrame f) throws IOException { |
writeByte('f'); |
base64OutputStream.writeByte(f.getParameterSetNumber()); |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/AttitudeDataResponseFrame.java |
---|
1,12 → 1,9 |
package dongfang.mkt.frames; |
public class AttitudeDataResponseFrame extends ResponseFrame { |
// signed int Winkel[3]; // nick, roll, compass in 0,1� |
// signed char reserve[8]; |
private int pitch; |
private int roll; |
private int heading; |
private int[] expansion; |
private float[] attitude; |
private float[] rates; |
private float[] acc; |
public AttitudeDataResponseFrame(int address) { |
super(address); |
17,35 → 14,34 |
return r instanceof AttitudeDataRequestFrame; |
} |
public int getPitch() { |
return pitch; |
public float[] getAttitude() { |
return attitude; |
} |
public void setPitch(int pitch) { |
this.pitch = pitch; |
public void setAttitude(float[] attitude) { |
this.attitude = attitude; |
} |
public int getRoll() { |
return roll; |
public float[] getRates() { |
return rates; |
} |
public void setRoll(int roll) { |
this.roll = roll; |
public void setRates(float[] rates) { |
this.rates = rates; |
} |
public int getHeading() { |
return heading; |
public float[] getAcc() { |
return acc; |
} |
public void setHeading(int heading) { |
this.heading = heading; |
public void setAcc(float[] acc) { |
this.acc = acc; |
} |
public int[] getExpansion() { |
return expansion; |
public String toString() { |
String result = "pitch: " + attitude[0] + ", roll: " + attitude[1] + ", yaw: " + attitude[2]; |
result += ", pitchRate: " + rates[0] + ", rollRate: " + rates[1] + ", yawRate: " + rates[2]; |
result += ", X: " + acc[0] + ", Y: " + acc[1] + ", Z: " + acc[2]; |
return result; |
} |
public void setExpansion(int[] expansion) { |
this.expansion = expansion; |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/DCMMatrixRequestFrame.java |
---|
0,0 → 1,17 |
package dongfang.mkt.frames; |
import java.io.IOException; |
import dongfang.mkt.RequestFrameVisitor; |
public class DCMMatrixRequestFrame extends RequestFrame { |
public DCMMatrixRequestFrame() { |
super(FC_ADDRESS); |
} |
public void accept(RequestFrameVisitor o) throws IOException { |
o.visit(this); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/DCMMatrixResponseFrame.java |
---|
0,0 → 1,42 |
package dongfang.mkt.frames; |
import java.util.Formatter; |
import java.util.Locale; |
public class DCMMatrixResponseFrame extends ResponseFrame { |
private float[][] matrix; |
public DCMMatrixResponseFrame(int address) { |
super(address); |
} |
@Override |
public boolean isResponseTo(RequestFrame r) { |
return r instanceof DCMMatrixRequestFrame; |
} |
public float[][] getMatrix() { |
return matrix; |
} |
public void setMatrix(float[][] matrix) { |
this.matrix = matrix; |
} |
public String toString() { |
StringBuilder result = new StringBuilder(); |
Formatter fmt = new Formatter(result, Locale.US); |
fmt.format("%4.4f", matrix[0][0] + " "); |
fmt.format("%4.4f", matrix[0][1] + " "); |
fmt.format("%4.4f", matrix[0][2] + "\n"); |
fmt.format("%4.4f", matrix[1][0] + " "); |
fmt.format("%4.4f", matrix[1][1] + " "); |
fmt.format("%4.4f", matrix[1][2] + "\n"); |
fmt.format("%4.4f", matrix[2][0] + " "); |
fmt.format("%4.4f", matrix[2][1] + " "); |
fmt.format("%4.4f", matrix[2][2]); |
return result.toString(); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/DCMMatrixDump.java |
---|
0,0 → 1,78 |
package dongfang.mkt.main; |
import java.io.BufferedReader; |
import java.io.IOException; |
import java.io.InputStreamReader; |
import java.io.PrintStream; |
import java.util.Date; |
import dongfang.mkt.comm.FrameQueue; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
import dongfang.mkt.frames.DCMMatrixResponseFrame; |
import dongfang.mkt.frames.ReadRCChannelsRequestFrame; |
public class DCMMatrixDump { |
private static final PrintStream STDERR = System.out; |
interface Logger { |
void start(String timestamp) throws IOException; |
void log(DCMMatrixResponseFrame f, long timestamp) throws IOException; |
void end() throws IOException; |
} |
static class ConsoleRCLogger implements Logger { |
public void start(String logId) {} |
public void log(DCMMatrixResponseFrame f, long timestamp) { |
if (f == null) { |
System.out.println("Oops, null response frame."); |
return; |
} |
System.out.println(f); |
} |
public void end() {} |
} |
private static Logger createLogger() throws IOException { |
Logger consoleLogger = new ConsoleRCLogger(); |
return consoleLogger; |
} |
private void dcmMatrixDump(final FrameQueue q, final Logger logger) throws IOException { |
long timer = 0; |
ReadRCChannelsRequestFrame f = new ReadRCChannelsRequestFrame(); |
BufferedReader in = new BufferedReader(new InputStreamReader(System.in)); |
Long firsttimestamp = null; |
while (!in.ready()) |
try { |
long now = System.currentTimeMillis(); |
if (now - timer > 500) { |
timer = now; |
q.sendRequest(f); |
DCMMatrixResponseFrame rf = (DCMMatrixResponseFrame) q.getResponseFor(f, 500); |
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 { |
DCMMatrixDump test = new DCMMatrixDump(); |
RXTXSerialPort _port = new RXTXSerialPort(); |
_port.init(null); |
FrameQueue q = new FrameQueue(_port); |
Logger logger = createLogger(); |
logger.start(new Date().toGMTString()); |
test.dcmMatrixDump(q, logger); |
logger.end(); |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/IMUDump.java |
---|
0,0 → 1,81 |
package dongfang.mkt.main; |
import java.io.BufferedReader; |
import java.io.IOException; |
import java.io.InputStreamReader; |
import java.io.PrintStream; |
import java.util.Date; |
import dongfang.mkt.comm.FrameQueue; |
import dongfang.mkt.comm.serial.RXTXSerialPort; |
import dongfang.mkt.frames.AttitudeDataRequestFrame; |
import dongfang.mkt.frames.AttitudeDataResponseFrame; |
import dongfang.mkt.frames.Frame; |
import dongfang.mkt.frames.ReadRCChannelsRequestFrame; |
import dongfang.mkt.frames.ReadRCChannelsResponseFrame; |
public class IMUDump { |
private static final PrintStream STDERR = System.out; |
interface Logger { |
void start(String timestamp) throws IOException; |
void log(AttitudeDataResponseFrame f, long timestamp) throws IOException; |
void end() throws IOException; |
} |
static class ConsoleAttitudeLogger implements Logger { |
public void start(String logId) {} |
public void log(AttitudeDataResponseFrame f, long timestamp) { |
if (f == null) { |
System.out.println("Oops, null response frame."); |
return; |
} |
System.out.println(f); |
} |
public void end() {} |
} |
private static Logger createLogger() throws IOException { |
Logger consoleLogger = new ConsoleAttitudeLogger(); |
return consoleLogger; |
} |
private void imuDump(final FrameQueue q, final Logger logger) throws IOException { |
long timer = 0; |
AttitudeDataRequestFrame f = new AttitudeDataRequestFrame(100); |
BufferedReader in = new BufferedReader(new InputStreamReader(System.in)); |
Long firsttimestamp = null; |
while (!in.ready()) |
try { |
long now = System.currentTimeMillis(); |
if (now - timer > 500) { |
timer = now; |
q.sendRequest(f); |
AttitudeDataResponseFrame rf = (AttitudeDataResponseFrame) q.getResponseFor(f, 500); |
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 { |
IMUDump test = new IMUDump(); |
RXTXSerialPort _port = new RXTXSerialPort(); |
_port.init(null); |
FrameQueue q = new FrameQueue(_port); |
Logger logger = createLogger(); |
logger.start(new Date().toGMTString()); |
test.imuDump(q, logger); |
logger.end(); |
} |
} |