Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1694 → Rev 1695

/dongfang_FC_rewrite_tool/configsets/FC2.0_Quadcopter.xml
File deleted
/dongfang_FC_rewrite_tool/configsets/FC2.0_Quadcopter_imu.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
New file
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
New file
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-x-rot90.xml
New file
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
New file
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/configsets/motorMixers/quadro-+.xml
New file
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/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
New file
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
New file
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/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/main/DCMMatrixDump.java
New file
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
New file
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();
}
}