/dongfang_FC_rewrite_tool/src/dongfang/mkt/RequestFrameVisitor.java |
---|
7,7 → 7,6 |
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; |
52,5 → 51,4 |
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,7 → 12,6 |
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; |
123,21 → 122,8 |
} |
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; |
199,9 → 185,10 |
} |
case 'C': { |
AttitudeDataResponseFrame f = new AttitudeDataResponseFrame(address); |
f.setAttitude(base64InputStream.readFloats(3)); |
f.setRates(base64InputStream.readFloats(3)); |
f.setAcc(base64InputStream.readFloats(3)); |
f.setPitch(base64InputStream.readSignedWord()); |
f.setRoll(base64InputStream.readSignedWord()); |
f.setHeading(base64InputStream.readSignedWord()); |
f.setExpansion(base64InputStream.readBytes(8)); |
result = f; |
break; |
} |
214,17 → 201,6 |
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,7 → 9,6 |
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; |
143,10 → 142,6 |
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/DCMMatrixRequestFrame.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/DCMMatrixResponseFrame.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/AttitudeDataResponseFrame.java |
---|
1,9 → 1,12 |
package dongfang.mkt.frames; |
public class AttitudeDataResponseFrame extends ResponseFrame { |
private float[] attitude; |
private float[] rates; |
private float[] acc; |
// 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; |
public AttitudeDataResponseFrame(int address) { |
super(address); |
14,34 → 17,35 |
return r instanceof AttitudeDataRequestFrame; |
} |
public float[] getAttitude() { |
return attitude; |
public int getPitch() { |
return pitch; |
} |
public void setAttitude(float[] attitude) { |
this.attitude = attitude; |
public void setPitch(int pitch) { |
this.pitch = pitch; |
} |
public float[] getRates() { |
return rates; |
public int getRoll() { |
return roll; |
} |
public void setRates(float[] rates) { |
this.rates = rates; |
public void setRoll(int roll) { |
this.roll = roll; |
} |
public float[] getAcc() { |
return acc; |
public int getHeading() { |
return heading; |
} |
public void setAcc(float[] acc) { |
this.acc = acc; |
public void setHeading(int heading) { |
this.heading = heading; |
} |
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 int[] getExpansion() { |
return expansion; |
} |
public void setExpansion(int[] expansion) { |
this.expansion = expansion; |
} |
} |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/DCMMatrixDump.java |
---|
File deleted |
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/IMUDump.java |
---|
File deleted |