Subversion Repositories Projects

Rev

Rev 1688 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1688 Rev 1689
1 
package dongfang.mkt.configuration;
 1 
package dongfang.mkt.configuration;
2 
 
 - 
 
3 
import java.io.File;
 2 
 
4 
import java.io.IOException;
 3 
import java.io.IOException;
5 
import java.io.InputStream;
 4 
import java.io.InputStream;
6 
 
 5 
 
7 
import javax.xml.parsers.DocumentBuilder;
 6 
import javax.xml.parsers.DocumentBuilder;
8 
import javax.xml.parsers.DocumentBuilderFactory;
 7 
import javax.xml.parsers.DocumentBuilderFactory;
9 
import javax.xml.parsers.ParserConfigurationException;
 8 
import javax.xml.parsers.ParserConfigurationException;
10 
import javax.xml.xpath.XPath;
 9 
import javax.xml.xpath.XPath;
11 
import javax.xml.xpath.XPathConstants;
 10 
import javax.xml.xpath.XPathConstants;
12 
import javax.xml.xpath.XPathExpressionException;
 11 
import javax.xml.xpath.XPathExpressionException;
13 
import javax.xml.xpath.XPathFactory;
 12 
import javax.xml.xpath.XPathFactory;
14 
 
 13 
 
15 
import org.w3c.dom.Document;
 14 
import org.w3c.dom.Document;
16 
import org.w3c.dom.Element;
 15 
import org.w3c.dom.Element;
17 
import org.w3c.dom.NodeList;
 16 
import org.w3c.dom.NodeList;
18 
import org.xml.sax.SAXException;
 17 
import org.xml.sax.SAXException;
19 
 
 - 
 
20 
import dongfang.mkt.frames.WriteParamSetRequestFrame;
 - 
 
21 
 
 18 
 
22 
public class MotorMixer {
 19 
public class MotorMixer {
23 
        public static final int EEPROMVERSION = 11;
 20 
        public static final int EEPROMVERSION = 1;
24 
       
 21 
       
25 
        public static final int MAX_MOTORS = 12;
 22 
        public static final int MAX_MOTORS = 12;
26 
        public static final int MIX_PITCH                   = 0;
 23 
        public static final int MIX_PITCH                   = 0;
27 
        public static final int MIX_ROLL                        = 1;
 24 
        public static final int MIX_ROLL                        = 1;
28 
        public static final int MIX_THROTTLE            = 2;
 25 
        public static final int MIX_THROTTLE            = 2;
29 
        public static final int MIX_YAW                         = 3;
 26 
        public static final int MIX_YAW                         = 3;
30 
        public static final int MIX_OPPOSITE_MOTOR      = 4;
 27 
        public static final int MIX_OPPOSITE_MOTOR      = 4;
31 
 
 - 
 
32 
        private int getByteCount() {
 - 
 
33 
                return MAX_MOTORS * 4 + MAX_MOTORS;
 - 
 
34 
        }
 - 
 
35 
       
 28 
       
36 
        void rowToXML(int[] row, StringBuilder out) {
 29 
        void rowToXML(int[] row, StringBuilder out) {
37 
                out.append("<motor throttlePart=\"" + row[MIX_THROTTLE] + "\" pitchPart=\"" + row[MIX_PITCH] + "\" rollPart=\"" + row[MIX_ROLL] + "\" yawPart=\"" + row[MIX_YAW] + "\" oppositeMotor=\"" + row[MIX_OPPOSITE_MOTOR] + "\"/>");
 30 
                out.append("<motor throttlePart=\"" + row[MIX_THROTTLE] + "\" pitchPart=\"" + row[MIX_PITCH] + "\" rollPart=\"" + row[MIX_ROLL] + "\" yawPart=\"" + row[MIX_YAW] + "\" oppositeMotor=\"" + row[MIX_OPPOSITE_MOTOR] + "\" />");
38 
        }
 31 
        }
- 
 
 32 
 
- 
 
 33 
        private String name;
39 
               
 34 
       
40 
        private int[][] matrix = new int[MAX_MOTORS][];
 35 
        private int[][] matrix = new int[MAX_MOTORS][];
41 
 
 36 
 
42 
        public void setMatrix(int[][] matrix) {
 37 
        public void setMatrix(int[][] matrix) {
43 
                this.matrix = matrix;
 38 
                this.matrix = matrix;
44 
        }
 39 
        }
- 
 
 40 
 
- 
 
 41 
        public String getName() {
- 
 
 42 
                return name;
- 
 
 43 
        }
- 
 
 44 
 
- 
 
 45 
        public void setName(String name) {
- 
 
 46 
                this.name = name;
- 
 
 47 
        }
45 
 
 48 
 
46 
        public int[][] toBinary() {
 49 
        public int[][] toBinary() {
47 
                return matrix;
 50 
                return matrix;
48 
        }
 51 
        }
49 
       
 52 
       
50 
        public String toXML() {
 53 
        public String toXML() {
51 
                StringBuilder result = new StringBuilder();
 54 
                StringBuilder result = new StringBuilder();
52 
                result.append("<motorMixer eepromVersion=\"" + EEPROMVERSION + "\" length=\"" + getByteCount() + "\">\n");
 55 
                result.append("<motorMixer eepromVersion=\"" + EEPROMVERSION + "\" name=\"" + name + "\">\n");
53 
                for (int i = 0; i < MAX_MOTORS; i++) {
 56 
                for (int i = 0; i < MAX_MOTORS; i++) {
54 
                        int[] row = matrix[i];
 57 
                        int[] row = matrix[i];
55 
                        result.append("  ");
 58 
                        result.append("  ");
56 
                        this.rowToXML(row, result);
 59 
                        this.rowToXML(row, result);
57 
                        result.append("\n");
 60 
                        result.append("\n");
58 
                }
 61 
                }
59 
                result.append("</motorMixer>\n");
 62 
                result.append("</motorMixer>\n");
60 
                return result.toString();
 63 
                return result.toString();
61 
        }
 64 
        }
62 
       
 65 
       
63 
        private void parseXMLParameterSet(InputStream input) throws IOException {
 66 
        public void parseXML(InputStream input) throws IOException {
64 
                DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance();
 67 
                DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance();
65 
                saxfac.setValidating(false);
 68 
                saxfac.setValidating(false);
66 
                try {
 69 
                try {
67 
                        DocumentBuilder bldr = saxfac.newDocumentBuilder();
 70 
                        DocumentBuilder bldr = saxfac.newDocumentBuilder();
68 
                        Document doc = bldr.parse(input);
 71 
                        Document doc = bldr.parse(input);
69 
 
 72 
 
70 
                        XPath xpath = XPathFactory.newInstance().newXPath();
 73 
                        XPath xpath = XPathFactory.newInstance().newXPath();
71 
                        String s_eepromVersion = xpath.evaluate("/motorMixer/@eepromVersion", doc);
 74 
                        String s_eepromVersion = xpath.evaluate("/motorMixer/@eepromVersion", doc);
72 
                        int eepromVersion;
 75 
                        int eepromVersion;
73 
 
 76 
 
74 
                        try {
 77 
                        try {
75 
                                eepromVersion = Integer.parseInt(s_eepromVersion);
 78 
                                eepromVersion = Integer.parseInt(s_eepromVersion);
76 
                        } catch (NumberFormatException ex) {
 79 
                        } catch (NumberFormatException ex) {
77 
                                System.err
 80 
                                System.err
78 
                                                .println("The motorMixer element must have an 'eepromVersion' attribute with a numerical value (eg.<motorMixer eepromVersion='1'>)");
 81 
                                                .println("The motorMixer element must have an 'eepromVersion' attribute with a numerical value (eg.<motorMixer eepromVersion='1'>)");
79 
                                System.exit(-1);
 82 
                                System.exit(-1);
80 
                        }
 83 
                        }
- 
 
 84 
 
- 
 
 85 
                        this.name = xpath.evaluate("/motorMixer/@name", doc);
81 
 
 86 
                       
82 
                        NodeList rowNodes = (NodeList) xpath.evaluate(
 87 
                        NodeList rowNodes = (NodeList) xpath.evaluate(
83 
                                        "/motorMixer/motor", doc, XPathConstants.NODESET);
 88 
                                        "/motorMixer/motor", doc, XPathConstants.NODESET);
84 
 
 89 
 
85 
                        for (int i = 0; i < rowNodes.getLength(); i++) {
 90 
                        for (int i = 0; i < rowNodes.getLength(); i++) {
86 
                                Element e = (Element) rowNodes.item(i);
 91 
                                Element e = (Element) rowNodes.item(i);
87 
                                int[] row = new int[5];
 92 
                                int[] row = new int[5];
88 
                               
 93 
                               
89 
                                String s_value = e.getAttribute("throttle");
 94 
                                String s_value = e.getAttribute("throttlePart");
90 
                                row[MIX_THROTTLE] = Integer.parseInt(s_value);
 95 
                                row[MIX_THROTTLE] = Integer.parseInt(s_value);
91 
 
 96 
 
92 
                                s_value = e.getAttribute("pitch");
 97 
                                s_value = e.getAttribute("pitchPart");
93 
                                row[MIX_PITCH] = Integer.parseInt(s_value);
 98 
                                row[MIX_PITCH] = Integer.parseInt(s_value);
94 
 
 99 
 
95 
                                s_value = e.getAttribute("roll");
 100 
                                s_value = e.getAttribute("rollPart");
96 
                                row[MIX_ROLL] = Integer.parseInt(s_value);
 101 
                                row[MIX_ROLL] = Integer.parseInt(s_value);
97 
 
 102 
 
98 
                                s_value = e.getAttribute("yaw");
 103 
                                s_value = e.getAttribute("yawPart");
99 
                                row[MIX_YAW] = Integer.parseInt(s_value);
 104 
                                row[MIX_YAW] = Integer.parseInt(s_value);
100 
 
 105 
 
101 
                                s_value = e.getAttribute("oppositeMotor");
 106 
                                s_value = e.getAttribute("oppositeMotor");
102 
                                row[MIX_OPPOSITE_MOTOR] = Integer.parseInt(s_value);
 107 
                                row[MIX_OPPOSITE_MOTOR] = Integer.parseInt(s_value);
103 
 
 108 
 
104 
                                matrix[i] = row;
 109 
                                matrix[i] = row;
105 
                        }
 110 
                        }
106 
                        input.close();
 111 
                        input.close();
107 
                } catch (ParserConfigurationException ex) {
 112 
                } catch (ParserConfigurationException ex) {
108 
                        // Should never happen.
 113 
                        // Should never happen.
109 
                        throw new RuntimeException(ex);
 114 
                        throw new RuntimeException(ex);
110 
                } catch (SAXException ex) {
 115 
                } catch (SAXException ex) {
111 
                        System.err.println(ex);
 116 
                        System.err.println(ex);
112 
                        System.err
 117 
                        System.err
113 
                                        .println("There is something screwed with your XML document. It is not well-formed and won't parse.");
 118 
                                        .println("There is something screwed with your XML document. It is not well-formed and won't parse.");
114 
                        throw new RuntimeException("Parse error.");
 119 
                        throw new RuntimeException("Parse error.");
115 
                } catch (XPathExpressionException ex) {
 120 
                } catch (XPathExpressionException ex) {
116 
                        // Should never happen.
 121 
                        // Should never happen.
117 
                        throw new RuntimeException(ex);
 122 
                        throw new RuntimeException(ex);
118 
                }
 123 
                }
119 
        }
 124 
        }
120 
}
 125 
}