Subversion Repositories Projects

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

                }

        }

}