/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/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; |
29,14 → 26,12 |
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] + "\"/>"); |
} |
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/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/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; |
} |
37,15 → 50,7 |
} |
*/ |
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/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,10 → 73,18 |
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) { |
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]); |
} |