Rev 1611 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1611 | Rev 1613 | ||
---|---|---|---|
1 | package dongfang.mkt.main; |
1 | package dongfang.mkt.main; |
2 | 2 | ||
3 | import java.io.FileInputStream; |
3 | import java.io.FileInputStream; |
4 | import java.io.FileWriter; |
4 | import java.io.FileWriter; |
5 | import java.io.IOException; |
5 | import java.io.IOException; |
6 | import java.io.InputStream; |
6 | import java.io.InputStream; |
7 | 7 | ||
8 | import javax.xml.parsers.DocumentBuilder; |
8 | import javax.xml.parsers.DocumentBuilder; |
9 | import javax.xml.parsers.DocumentBuilderFactory; |
9 | import javax.xml.parsers.DocumentBuilderFactory; |
10 | import javax.xml.parsers.ParserConfigurationException; |
10 | import javax.xml.parsers.ParserConfigurationException; |
11 | import javax.xml.xpath.XPath; |
11 | import javax.xml.xpath.XPath; |
12 | import javax.xml.xpath.XPathConstants; |
12 | import javax.xml.xpath.XPathConstants; |
13 | import javax.xml.xpath.XPathExpressionException; |
13 | import javax.xml.xpath.XPathExpressionException; |
14 | import javax.xml.xpath.XPathFactory; |
14 | import javax.xml.xpath.XPathFactory; |
15 | 15 | ||
16 | import org.w3c.dom.Document; |
16 | import org.w3c.dom.Document; |
17 | import org.w3c.dom.Element; |
17 | import org.w3c.dom.Element; |
18 | import org.w3c.dom.NodeList; |
18 | import org.w3c.dom.NodeList; |
19 | import org.xml.sax.SAXException; |
19 | import org.xml.sax.SAXException; |
20 | 20 | ||
21 | import dongfang.mkt.comm.FrameQueue; |
21 | import dongfang.mkt.comm.FrameQueue; |
22 | import dongfang.mkt.comm.MKConnection; |
22 | import dongfang.mkt.comm.MKConnection; |
23 | import dongfang.mkt.comm.serial.RXTXSerialPort; |
23 | import dongfang.mkt.comm.serial.RXTXSerialPort; |
24 | import dongfang.mkt.configuration.IMUConfiguration; |
24 | import dongfang.mkt.configuration.IMUConfiguration; |
25 | import dongfang.mkt.frames.ReadIMUConfigurationRequestFrame; |
25 | import dongfang.mkt.frames.ReadIMUConfigurationRequestFrame; |
26 | import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
26 | import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
27 | import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame; |
27 | import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame; |
28 | import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame; |
28 | import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame; |
29 | 29 | ||
30 | public class IMUConfigurator { |
30 | public class IMUConfigurator { |
31 | private static void configure(String portIdentifier, WriteIMUConfigurationRequestFrame frame) throws IOException { |
31 | private static void configure(String portIdentifier, WriteIMUConfigurationRequestFrame frame) throws IOException { |
32 | MKConnection port = new RXTXSerialPort(); |
32 | MKConnection port = new RXTXSerialPort(); |
33 | port.init(portIdentifier); |
33 | port.init(portIdentifier); |
34 | FrameQueue q = new FrameQueue(port); |
34 | FrameQueue q = new FrameQueue(port); |
35 | 35 | ||
36 | q.sendRequest(frame); |
36 | q.sendRequest(frame); |
37 | WriteIMUConfigurationResponseFrame r = (WriteIMUConfigurationResponseFrame) q.getResponseFor(frame, 5000); |
37 | WriteIMUConfigurationResponseFrame r = (WriteIMUConfigurationResponseFrame) q.getResponseFor(frame, 5000); |
38 | if (r == null) { |
38 | if (r == null) { |
39 | System.err.println("ERROR. Timeout waiting for response."); |
39 | System.err.println("ERROR. Timeout waiting for response."); |
40 | } else if (!r.wasAccepted()) { |
40 | } else if (!r.wasAccepted()) { |
41 | System.err |
41 | System.err |
42 | .println("ERROR. Parameter set not accepted. Check version against MK firmware EEPROM configuration version."); |
42 | .println("ERROR. Parameter set not accepted. Check version against MK firmware EEPROM configuration version."); |
43 | } else { |
43 | } else { |
44 | System.out.println("Saved IMU configuration."); |
44 | System.out.println("Saved IMU configuration."); |
45 | } |
45 | } |
46 | 46 | ||
47 | q.kill(); |
47 | q.kill(); |
48 | } |
48 | } |
49 | 49 | ||
50 | private static void writeConfiguration(String portIdentifier, String fileName) throws IOException { |
50 | private static void writeConfiguration(String portIdentifier, String fileName) throws IOException { |
51 | System.out.println("Writing IMU configuration from file: " + fileName); |
51 | System.out.println("Writing IMU configuration from file: " + fileName); |
52 | InputStream inputStream = new FileInputStream(fileName); |
52 | InputStream inputStream = new FileInputStream(fileName); |
53 | WriteIMUConfigurationRequestFrame frame = parseXMLIMUConfiguration(inputStream); |
53 | WriteIMUConfigurationRequestFrame frame = parseXMLIMUConfiguration(inputStream); |
54 | configure(portIdentifier, frame); |
54 | configure(portIdentifier, frame); |
55 | } |
55 | } |
56 | 56 | ||
57 | private static IMUConfiguration readConfiguration(String portIdentifier) throws IOException { |
57 | private static IMUConfiguration readConfiguration(String portIdentifier) throws IOException { |
58 | MKConnection port = new RXTXSerialPort(); |
58 | MKConnection port = new RXTXSerialPort(); |
59 | port.init(portIdentifier); |
59 | port.init(portIdentifier); |
60 | FrameQueue q = new FrameQueue(port); |
60 | FrameQueue q = new FrameQueue(port); |
61 | IMUConfiguration cs = null; |
61 | IMUConfiguration cs = null; |
62 | 62 | ||
63 | ReadIMUConfigurationRequestFrame frame = new ReadIMUConfigurationRequestFrame(); |
63 | ReadIMUConfigurationRequestFrame frame = new ReadIMUConfigurationRequestFrame(); |
64 | q.sendRequest(frame); |
64 | q.sendRequest(frame); |
65 | ReadIMUConfigurationResponseFrame r = (ReadIMUConfigurationResponseFrame) q.getResponseFor(frame, 5000); |
65 | ReadIMUConfigurationResponseFrame r = (ReadIMUConfigurationResponseFrame) q.getResponseFor(frame, 5000); |
66 | if (r == null) { |
66 | if (r == null) { |
67 | System.err.println("ERROR. Timeout waiting for response."); |
67 | System.err.println("ERROR. Timeout waiting for response."); |
68 | } else { |
68 | } else { |
69 | int paramSetVersion = r.getConfigurationVersion(); |
69 | int paramSetVersion = r.getConfigurationVersion(); |
70 | int[] data = r.getData(); |
70 | int[] data = r.getData(); |
71 | cs = IMUConfiguration.parseXMLIMUConfiguration(paramSetVersion); |
71 | cs = IMUConfiguration.parseXMLIMUConfiguration(paramSetVersion); |
72 | cs.setData(data); |
72 | cs.setData(data); |
73 | System.out.println(cs.toXML()); |
73 | System.out.println(cs.toXML()); |
74 | } |
74 | } |
75 | q.kill(); |
75 | q.kill(); |
76 | return cs; |
76 | return cs; |
77 | } |
77 | } |
78 | 78 | ||
79 | private static void readConfiguration(String portIdentifier, String fileName) throws IOException { |
79 | private static void readConfiguration(String portIdentifier, String fileName) throws IOException { |
80 | IMUConfiguration cs = readConfiguration(portIdentifier); |
80 | IMUConfiguration cs = readConfiguration(portIdentifier); |
81 | if (cs != null) { |
81 | if (cs != null) { |
82 | FileWriter fw = new FileWriter(fileName); |
82 | FileWriter fw = new FileWriter(fileName); |
83 | fw.write(cs.toXML()); |
83 | fw.write(cs.toXML()); |
84 | fw.close(); |
84 | fw.close(); |
85 | } |
85 | } |
86 | } |
86 | } |
87 | 87 | ||
88 | private static WriteIMUConfigurationRequestFrame parseXMLIMUConfiguration(InputStream input) throws IOException { |
88 | private static WriteIMUConfigurationRequestFrame parseXMLIMUConfiguration(InputStream input) throws IOException { |
89 | DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance(); |
89 | DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance(); |
90 | saxfac.setValidating(false); |
90 | saxfac.setValidating(false); |
91 | try { |
91 | try { |
92 | DocumentBuilder bldr = saxfac.newDocumentBuilder(); |
92 | DocumentBuilder bldr = saxfac.newDocumentBuilder(); |
93 | Document doc = bldr.parse(input); |
93 | Document doc = bldr.parse(input); |
94 | 94 | ||
95 | XPath xpath = XPathFactory.newInstance().newXPath(); |
95 | XPath xpath = XPathFactory.newInstance().newXPath(); |
96 | 96 | ||
97 | String s_eepromVersion = xpath.evaluate( |
97 | String s_eepromVersion = xpath.evaluate( |
98 | "/imuconfiguration/@eepromVersion", doc); |
98 | "/imuconfiguration/@eepromVersion", doc); |
99 | String ss_length = xpath.evaluate("/imuconfiguration/@length", doc); |
99 | String ss_length = xpath.evaluate("/imuconfiguration/@length", doc); |
100 | 100 | ||
101 | int s_length = 0; |
101 | int s_length = 0; |
102 | try { |
102 | try { |
103 | s_length = Integer.parseInt(ss_length); |
103 | s_length = Integer.parseInt(ss_length); |
104 | } catch (NumberFormatException ex) { |
104 | } catch (NumberFormatException ex) { |
105 | // ignore. |
105 | // ignore. |
106 | } |
106 | } |
107 | 107 | ||
108 | int eepromVersion = -1; |
108 | int eepromVersion = -1; |
109 | 109 | ||
110 | try { |
110 | try { |
111 | eepromVersion = Integer.parseInt(s_eepromVersion); |
111 | eepromVersion = Integer.parseInt(s_eepromVersion); |
112 | } catch (NumberFormatException ex) { |
112 | } catch (NumberFormatException ex) { |
113 | System.err |
113 | System.err |
114 | .println("The imuconfiguration element must have an 'eepromVersion' attribute with a numerical value (eg.<parameterset eepromVersion='74'>)"); |
114 | .println("The imuconfiguration element must have an 'eepromVersion' attribute with a numerical value (eg.<imuconfiguration eepromVersion='74'>)"); |
115 | System.exit(-1); |
115 | System.exit(-1); |
116 | } |
116 | } |
117 | 117 | ||
118 | NodeList parameterNodes = (NodeList) xpath.evaluate( |
118 | NodeList parameterNodes = (NodeList) xpath.evaluate( |
119 | "/imuconfiguration/parameter", doc, XPathConstants.NODESET); |
119 | "/imuconfiguration/parameter", doc, XPathConstants.NODESET); |
120 | int[] parameters = new int[parameterNodes.getLength()]; |
120 | int[] parameters = new int[parameterNodes.getLength()]; |
121 | 121 | ||
122 | NodeList allChildNodes = (NodeList) xpath.evaluate("/imuconfiguration/*", doc, XPathConstants.NODESET); |
122 | NodeList allChildNodes = (NodeList) xpath.evaluate("/imuconfiguration/*", doc, XPathConstants.NODESET); |
123 | 123 | ||
124 | if (parameterNodes.getLength() != allChildNodes.getLength()) { |
124 | if (parameterNodes.getLength() != allChildNodes.getLength()) { |
125 | System.err.println("There seems to be a child element of imuconfiguration whose name is not \"parameter\"."); |
125 | System.err.println("There seems to be a child element of imuconfiguration whose name is not \"parameter\"."); |
126 | // System.exit(-1); |
126 | // System.exit(-1); |
127 | } |
127 | } |
128 | 128 | ||
129 | if (s_length >=0 && s_length != parameterNodes.getLength()) { |
129 | if (s_length >=0 && s_length != parameterNodes.getLength()) { |
130 | System.err.println("The number of parameters ("+parameterNodes.getLength()+") does not match the number in the length attribute of the imuconfiguration element ("+s_length+")."); |
130 | System.err.println("The number of parameters ("+parameterNodes.getLength()+") does not match the number in the length attribute of the imuconfiguration element ("+s_length+")."); |
131 | // System.exit(-1); |
131 | // System.exit(-1); |
132 | } |
132 | } |
133 | 133 | ||
134 | for (int i = 0; i < parameterNodes.getLength(); i++) { |
134 | for (int i = 0; i < parameterNodes.getLength(); i++) { |
135 | Element e = (Element) parameterNodes.item(i); |
135 | Element e = (Element) parameterNodes.item(i); |
136 | int value = 0; |
136 | int value = 0; |
137 | if (e.hasAttribute("value")) { |
137 | if (e.hasAttribute("value")) { |
138 | String s_value = e.getAttribute("value"); |
138 | String s_value = e.getAttribute("value"); |
139 | value = Integer.parseInt(s_value); |
139 | value = Integer.parseInt(s_value); |
140 | if (value < 0) |
140 | if (value < 0) |
141 | try { |
141 | try { |
142 | value = Integer.parseInt(s_value); |
142 | value = Integer.parseInt(s_value); |
143 | } catch (NumberFormatException ex) { |
143 | } catch (NumberFormatException ex) { |
144 | throw new NumberFormatException( |
144 | throw new NumberFormatException( |
145 | "Value is not a number and not a variable name."); |
145 | "Value is not a number and not a variable name."); |
146 | } |
146 | } |
147 | } else { |
147 | } else { |
148 | NodeList bitNodes = (NodeList) xpath.evaluate("bit", e, |
148 | NodeList bitNodes = (NodeList) xpath.evaluate("bit", e, |
149 | XPathConstants.NODESET); |
149 | XPathConstants.NODESET); |
150 | for (int j = 0; j < 8; j++) { |
150 | for (int j = 0; j < 8; j++) { |
151 | Element bitElement = (Element) bitNodes.item(j); |
151 | Element bitElement = (Element) bitNodes.item(j); |
152 | if (bitElement != null) { |
152 | if (bitElement != null) { |
153 | String s_bitValue = bitElement |
153 | String s_bitValue = bitElement |
154 | .getAttribute("value"); |
154 | .getAttribute("value"); |
155 | if ("1".equals(s_bitValue)) |
155 | if ("1".equals(s_bitValue)) |
156 | value |= (1 << j); |
156 | value |= (1 << j); |
157 | else if (!"0".equals(s_bitValue)) |
157 | else if (!"0".equals(s_bitValue)) |
158 | throw new NumberFormatException( |
158 | throw new NumberFormatException( |
159 | "Bit value was not 0 or 1."); |
159 | "Bit value was not 0 or 1."); |
160 | } |
160 | } |
161 | } |
161 | } |
162 | } |
162 | } |
163 | parameters[i] = value; |
163 | parameters[i] = value; |
164 | } |
164 | } |
165 | input.close(); |
165 | input.close(); |
166 | return new WriteIMUConfigurationRequestFrame(eepromVersion, parameters); |
166 | return new WriteIMUConfigurationRequestFrame(eepromVersion, parameters); |
167 | } catch (ParserConfigurationException ex) { |
167 | } catch (ParserConfigurationException ex) { |
168 | // Should never happen. |
168 | // Should never happen. |
169 | throw new RuntimeException(ex); |
169 | throw new RuntimeException(ex); |
170 | } catch (SAXException ex) { |
170 | } catch (SAXException ex) { |
171 | System.err.println(ex); |
171 | System.err.println(ex); |
172 | System.err |
172 | System.err |
173 | .println("There is something screwed with your XML document. It is not well-formed and won't parse."); |
173 | .println("There is something screwed with your XML document. It is not well-formed and won't parse."); |
174 | throw new RuntimeException("Parse error."); |
174 | throw new RuntimeException("Parse error."); |
175 | } catch (XPathExpressionException ex) { |
175 | } catch (XPathExpressionException ex) { |
176 | // Should never happen. |
176 | // Should never happen. |
177 | throw new RuntimeException(ex); |
177 | throw new RuntimeException(ex); |
178 | } |
178 | } |
179 | } |
179 | } |
180 | 180 | ||
181 | static void help() { |
181 | static void help() { |
182 | System.err.println("Usage: IMUConfigurator r [filename to write to]"); |
182 | System.err.println("Usage: IMUConfigurator r [filename to write to]"); |
183 | System.err.println("Usage: IMUConfigurator w [filename to read from]"); |
183 | System.err.println("Usage: IMUConfigurator w [filename to read from]"); |
184 | System.exit(-1); |
184 | System.exit(-1); |
185 | } |
185 | } |
186 | 186 | ||
187 | public static void main(String[] args) throws IOException { |
187 | public static void main(String[] args) throws IOException { |
188 | if (!"r".equals(args[0]) && !"w".equals(args[0])) help(); |
188 | if (!"r".equals(args[0]) && !"w".equals(args[0])) help(); |
189 | if ("w".equals(args[0]) && (args.length!=2)) help(); |
189 | if ("w".equals(args[0]) && (args.length!=2)) help(); |
190 | if ("r".equals(args[0]) && (args.length!=2)) help(); |
190 | if ("r".equals(args[0]) && (args.length!=2)) help(); |
191 | 191 | ||
192 | String portIdentifier = null; |
192 | String portIdentifier = null; |
193 | 193 | ||
194 | if ("r".equals(args[0])) { |
194 | if ("r".equals(args[0])) { |
195 | readConfiguration(portIdentifier, args[1]); |
195 | readConfiguration(portIdentifier, args[1]); |
196 | } else { |
196 | } else { |
197 | writeConfiguration(portIdentifier, args[1]); |
197 | writeConfiguration(portIdentifier, args[1]); |
198 | } |
198 | } |
199 | System.exit(0); |
199 | System.exit(0); |
200 | } |
200 | } |
201 | } |
201 | } |