Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 1688 → Rev 1689

/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/configuration/MotorMixer.java
1,6 → 1,5
package dongfang.mkt.configuration;
 
import java.io.File;
import java.io.IOException;
import java.io.InputStream;
 
17,10 → 16,8
import org.w3c.dom.NodeList;
import org.xml.sax.SAXException;
 
import dongfang.mkt.frames.WriteParamSetRequestFrame;
 
public class MotorMixer {
public static final int EEPROMVERSION = 11;
public static final int EEPROMVERSION = 1;
public static final int MAX_MOTORS = 12;
public static final int MIX_PITCH = 0;
28,15 → 25,13
public static final int MIX_THROTTLE = 2;
public static final int MIX_YAW = 3;
public static final int MIX_OPPOSITE_MOTOR = 4;
 
private int getByteCount() {
return MAX_MOTORS * 4 + MAX_MOTORS;
}
void rowToXML(int[] row, StringBuilder out) {
out.append("<motor throttlePart=\"" + row[MIX_THROTTLE] + "\" pitchPart=\"" + row[MIX_PITCH] + "\" rollPart=\"" + row[MIX_ROLL] + "\" yawPart=\"" + row[MIX_YAW] + "\" oppositeMotor=\"" + row[MIX_OPPOSITE_MOTOR] + "\"/>");
out.append("<motor throttlePart=\"" + row[MIX_THROTTLE] + "\" pitchPart=\"" + row[MIX_PITCH] + "\" rollPart=\"" + row[MIX_ROLL] + "\" yawPart=\"" + row[MIX_YAW] + "\" oppositeMotor=\"" + row[MIX_OPPOSITE_MOTOR] + "\" />");
}
 
private String name;
private int[][] matrix = new int[MAX_MOTORS][];
 
public void setMatrix(int[][] matrix) {
43,6 → 38,14
this.matrix = matrix;
}
 
public String getName() {
return name;
}
 
public void setName(String name) {
this.name = name;
}
 
public int[][] toBinary() {
return matrix;
}
49,7 → 52,7
public String toXML() {
StringBuilder result = new StringBuilder();
result.append("<motorMixer eepromVersion=\"" + EEPROMVERSION + "\" length=\"" + getByteCount() + "\">\n");
result.append("<motorMixer eepromVersion=\"" + EEPROMVERSION + "\" name=\"" + name + "\">\n");
for (int i = 0; i < MAX_MOTORS; i++) {
int[] row = matrix[i];
result.append(" ");
60,7 → 63,7
return result.toString();
}
private void parseXMLParameterSet(InputStream input) throws IOException {
public void parseXML(InputStream input) throws IOException {
DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance();
saxfac.setValidating(false);
try {
79,6 → 82,8
System.exit(-1);
}
 
this.name = xpath.evaluate("/motorMixer/@name", doc);
NodeList rowNodes = (NodeList) xpath.evaluate(
"/motorMixer/motor", doc, XPathConstants.NODESET);
 
86,16 → 91,16
Element e = (Element) rowNodes.item(i);
int[] row = new int[5];
String s_value = e.getAttribute("throttle");
String s_value = e.getAttribute("throttlePart");
row[MIX_THROTTLE] = Integer.parseInt(s_value);
 
s_value = e.getAttribute("pitch");
s_value = e.getAttribute("pitchPart");
row[MIX_PITCH] = Integer.parseInt(s_value);
 
s_value = e.getAttribute("roll");
s_value = e.getAttribute("rollPart");
row[MIX_ROLL] = Integer.parseInt(s_value);
 
s_value = e.getAttribute("yaw");
s_value = e.getAttribute("yawPart");
row[MIX_YAW] = Integer.parseInt(s_value);
 
s_value = e.getAttribute("oppositeMotor");
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKInputStream.java
264,6 → 264,8
case 'M': {
WriteMotorMixerResponseFrame f = new WriteMotorMixerResponseFrame(address);
f.setWasAccepted(base64InputStream.readByte() != 0);
result = f;
break;
}
case 'N': {
int numMotors = 12;
271,18 → 273,21
f.setConfigurationVersion(base64InputStream.readByte());
int length = base64InputStream.readByte();
f.setDataLength(length);
int testLength = 0;
int testLength = 12; // the name is first!
char[] name = base64InputStream.readChars(12);
f.setName(name);
int[][] matrix = new int[numMotors][];
for(int i=0; i<numMotors; i++) {
int[] row = new int[4];
for(int j=0; j<4; j++) {
matrix[i][j] = base64InputStream.readByte();
int[] row = new int[5];
matrix[i] = row;
for(int j=0; j<5; j++) {
row[j] = base64InputStream.readSignedByte();
testLength++;
}
}
if (length != testLength)
throw new IOException("Length of motor mixer data was not as expected.");
throw new IOException("Length of motor mixer data was not as expected (" + testLength + " vs. " + length + ").");
/*
int[] opposite = new int[numMotors];
for(int i=0; i<numMotors; i++) {
289,10 → 294,9
opposite[i] = base64InputStream.readByte();
}
*/
f.setMatrix(matrix);
 
result = f;
break;
}
case 'O': {
OSDDataResponseFrame f = new OSDDataResponseFrame(address);
/dongfang_FC_rewrite_tool/src/dongfang/mkt/comm/MKOutputStream.java
39,6 → 39,11
inbuf[inbufptr++] = b;
}
 
void writeSignedByte(int b) throws IOException {
if (b<0) b+= 256;
writeByte(b);
}
 
public void writeBytes(int[] bs) throws IOException {
for (int i=0; i<bs.length; i++)
writeByte(bs[i]);
171,13 → 176,16
 
public void visit(WriteMotorMixerRequestFrame f) throws IOException {
writeByte('m');
writeByte(f.getConfigurationVersion());
writeByte(f.getDataLength());
base64OutputStream.writeByte(f.getMotorMixerVersionNumber());
base64OutputStream.writeByte(f.getDataLength());
base64OutputStream.writeChars(f.getName());
for(int i=0; i<f.getMatrix().length; i++) {
int[] row = f.getMatrix()[i];
for(int j=0; j<row.length; j++) {
base64OutputStream.writeByte(row[j]);
base64OutputStream.writeSignedByte(row[j]);
//System.out.print(row[j] + " ");
}
//System.out.println();
}
/*
for(int i=0; i<f.getOppositeMotors().length; i++) {
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/WriteMotorMixerRequestFrame.java
6,14 → 6,15
 
 
public class WriteMotorMixerRequestFrame extends RequestFrame {
private char[] name;
private int[][] matrix;
// private int[] oppositeMotors;
private int motorMixerVersionNumber;
private int configurationVersion;
public WriteMotorMixerRequestFrame(int motorMixerVersionNumber, int[][] matrix /*, int[] oppositeMotors*/) {
public WriteMotorMixerRequestFrame(int motorMixerVersionNumber, char[] name, int[][] matrix /*, int[] oppositeMotors*/) {
super(FC_ADDRESS);
this.motorMixerVersionNumber = motorMixerVersionNumber;
this.name = name;
this.matrix = matrix;
// this.oppositeMotors = oppositeMotors;
}
27,6 → 28,18
return motorMixerVersionNumber;
}
 
public void setMotorMixerVersionNumber(int motorMixerVersionNumber) {
this.motorMixerVersionNumber = motorMixerVersionNumber;
}
 
public char[] getName() {
return name;
}
 
public void setName(char[] name) {
this.name = name;
}
 
public int[][] getMatrix() {
return matrix;
}
36,16 → 49,8
return oppositeMotors;
}
*/
 
public int getConfigurationVersion() {
return configurationVersion;
}
 
public void setConfigurationVersion(int configurationVersion) {
this.configurationVersion = configurationVersion;
}
public int getDataLength() {
return matrix.length * 5; // return matrix.length * 4 + oppositeMotors.length;*/
return 12 + matrix.length * 5; // return matrix.length * 4 + oppositeMotors.length;*/
}
}
/dongfang_FC_rewrite_tool/src/dongfang/mkt/frames/ReadMotorMixerResponseFrame.java
1,6 → 1,7
package dongfang.mkt.frames;
 
public class ReadMotorMixerResponseFrame extends ResponseFrame {
private char[] name;
private int[][] matrix;
// private int[] oppositeMotors;
private int configurationVersion;
10,6 → 11,14
super(FC_ADDRESS);
}
 
public char[] getName() {
return name;
}
 
public void setName(char[] name) {
this.name = name;
}
 
@Override
public boolean isResponseTo(RequestFrame r) {
return r instanceof ReadMotorMixerRequestFrame;
/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/MotorMixerConfigurator.java
53,8 → 53,11
System.out.println("Writing motor mixer from file: " + fileName);
InputStream inputStream = new FileInputStream(fileName);
MotorMixer mm = new MotorMixer();
mm.parseXMLMotorMixer(inputStream);
WriteMotorMixerRequestFrame frame = new WriteMotorMixerRequestFrame(1, mm.toBinary());
mm.parseXML(inputStream);
char[] name = new char[12];
mm.getName().getChars(0, mm.getName().length(), name, 0);
WriteMotorMixerRequestFrame frame = new WriteMotorMixerRequestFrame(1, name, mm.toBinary());
 
configure(portIdentifier, frame);
}
 
70,9 → 73,17
if (r == null) {
System.err.println("ERROR. Timeout waiting for response.");
} else {
int[][] matrix = r.getMatrix();
mm.setMatrix(matrix);
System.out.println(mm.toXML());
mm.setMatrix(r.getMatrix());
char[] name = r.getName();
int firstNulPos = name.length;
for (int i=0; i<name.length; i++) {
if (name[i] == 0) {
firstNulPos = i;
break;
}
}
String s_name = new String(name, 0, firstNulPos);
mm.setName(s_name);
}
q.kill();
return mm;
81,27 → 92,30
private static void readMotorMixer(String portIdentifier, String fileName) throws IOException {
MotorMixer mm = readMotorMixer(portIdentifier);
if (mm != null) {
FileWriter fw = new FileWriter(fileName);
fw.write(mm.toXML());
fw.close();
if (fileName != null) {
FileWriter fw = new FileWriter(fileName);
fw.write(mm.toXML());
fw.close();
}
System.out.println(mm.toXML());
}
}
 
static void help() {
System.err.println("Usage: MotorMixerConfigurator r [filename to write to]");
System.err.println("Usage: MotorMixerConfigurator r <filename to write to>");
System.err.println("Usage: MotorMixerConfigurator w [filename to read from]");
System.exit(-1);
}
public static void main(String[] args) throws IOException {
if (args.length < 1) help();
if (!"r".equals(args[0]) && !"w".equals(args[0])) help();
if ("w".equals(args[0]) && (args.length!=2)) help();
if ("r".equals(args[0]) && (args.length!=2)) help();
if ("w".equals(args[0]) && args.length < 2) help();
 
String portIdentifier = null;
if ("r".equals(args[0])) {
readMotorMixer(portIdentifier, args[1]);
readMotorMixer(portIdentifier, args.length < 2 ? null : args[1]);
} else {
writeConfiguration(portIdentifier, args[1]);
}