0,0 → 1,120 |
package dongfang.mkt.configuration; |
|
import java.io.File; |
import java.io.IOException; |
import java.io.InputStream; |
|
import javax.xml.parsers.DocumentBuilder; |
import javax.xml.parsers.DocumentBuilderFactory; |
import javax.xml.parsers.ParserConfigurationException; |
import javax.xml.xpath.XPath; |
import javax.xml.xpath.XPathConstants; |
import javax.xml.xpath.XPathExpressionException; |
import javax.xml.xpath.XPathFactory; |
|
import org.w3c.dom.Document; |
import org.w3c.dom.Element; |
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 MAX_MOTORS = 12; |
public static final int MIX_PITCH = 0; |
public static final int MIX_ROLL = 1; |
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] + "\"/>"); |
} |
|
private int[][] matrix = new int[MAX_MOTORS][]; |
|
public void setMatrix(int[][] matrix) { |
this.matrix = matrix; |
} |
|
public int[][] toBinary() { |
return matrix; |
} |
|
public String toXML() { |
StringBuilder result = new StringBuilder(); |
result.append("<motorMixer eepromVersion=\"" + EEPROMVERSION + "\" length=\"" + getByteCount() + "\">\n"); |
for (int i = 0; i < MAX_MOTORS; i++) { |
int[] row = matrix[i]; |
result.append(" "); |
this.rowToXML(row, result); |
result.append("\n"); |
} |
result.append("</motorMixer>\n"); |
return result.toString(); |
} |
|
private void parseXMLParameterSet(InputStream input) throws IOException { |
DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance(); |
saxfac.setValidating(false); |
try { |
DocumentBuilder bldr = saxfac.newDocumentBuilder(); |
Document doc = bldr.parse(input); |
|
XPath xpath = XPathFactory.newInstance().newXPath(); |
String s_eepromVersion = xpath.evaluate("/motorMixer/@eepromVersion", doc); |
int eepromVersion; |
|
try { |
eepromVersion = Integer.parseInt(s_eepromVersion); |
} catch (NumberFormatException ex) { |
System.err |
.println("The motorMixer element must have an 'eepromVersion' attribute with a numerical value (eg.<motorMixer eepromVersion='1'>)"); |
System.exit(-1); |
} |
|
NodeList rowNodes = (NodeList) xpath.evaluate( |
"/motorMixer/motor", doc, XPathConstants.NODESET); |
|
for (int i = 0; i < rowNodes.getLength(); i++) { |
Element e = (Element) rowNodes.item(i); |
int[] row = new int[5]; |
|
String s_value = e.getAttribute("throttle"); |
row[MIX_THROTTLE] = Integer.parseInt(s_value); |
|
s_value = e.getAttribute("pitch"); |
row[MIX_PITCH] = Integer.parseInt(s_value); |
|
s_value = e.getAttribute("roll"); |
row[MIX_ROLL] = Integer.parseInt(s_value); |
|
s_value = e.getAttribute("yaw"); |
row[MIX_YAW] = Integer.parseInt(s_value); |
|
s_value = e.getAttribute("oppositeMotor"); |
row[MIX_OPPOSITE_MOTOR] = Integer.parseInt(s_value); |
|
matrix[i] = row; |
} |
input.close(); |
} catch (ParserConfigurationException ex) { |
// Should never happen. |
throw new RuntimeException(ex); |
} catch (SAXException ex) { |
System.err.println(ex); |
System.err |
.println("There is something screwed with your XML document. It is not well-formed and won't parse."); |
throw new RuntimeException("Parse error."); |
} catch (XPathExpressionException ex) { |
// Should never happen. |
throw new RuntimeException(ex); |
} |
} |
} |