Subversion Repositories Projects

Rev

Rev 1688 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1688 Rev 1689
Line 1... Line 1...
1
package dongfang.mkt.configuration;
1
package dongfang.mkt.configuration;
2
 
2
 
3
import java.io.File;
-
 
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;
Line 15... Line 14...
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
 
18
 
20
import dongfang.mkt.frames.WriteParamSetRequestFrame;
-
 
21
 
-
 
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
        }
39
               
32
 
-
 
33
        private String name;
-
 
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
        }
45
 
40
 
-
 
41
        public String getName() {
-
 
42
                return name;
-
 
43
        }
-
 
44
 
-
 
45
        public void setName(String name) {
-
 
46
                this.name = name;
-
 
47
        }
-
 
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);
Line 77... Line 80...
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
                        }
81
 
84
 
-
 
85
                        this.name = xpath.evaluate("/motorMixer/@name", doc);
-
 
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