| 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); |
| } |
| } |
| } |