| 0,0 → 1,201 |
|---|
| package dongfang.mkt.main; |
| |
| import java.io.FileInputStream; |
| import java.io.FileWriter; |
| 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.comm.FrameQueue; |
| import dongfang.mkt.comm.MKConnection; |
| import dongfang.mkt.comm.serial.RXTXSerialPort; |
| import dongfang.mkt.configuration.IMUConfiguration; |
| import dongfang.mkt.frames.ReadIMUConfigurationRequestFrame; |
| import dongfang.mkt.frames.ReadIMUConfigurationResponseFrame; |
| import dongfang.mkt.frames.WriteIMUConfigurationRequestFrame; |
| import dongfang.mkt.frames.WriteIMUConfigurationResponseFrame; |
| |
| public class IMUConfigurator { |
| private static void configure(String portIdentifier, WriteIMUConfigurationRequestFrame frame) throws IOException { |
| MKConnection port = new RXTXSerialPort(); |
| port.init(portIdentifier); |
| FrameQueue q = new FrameQueue(port); |
| |
| q.sendRequest(frame); |
| WriteIMUConfigurationResponseFrame r = (WriteIMUConfigurationResponseFrame) q.getResponseFor(frame, 5000); |
| if (r == null) { |
| System.err.println("ERROR. Timeout waiting for response."); |
| } else if (!r.wasAccepted()) { |
| System.err |
| .println("ERROR. Parameter set not accepted. Check version against MK firmware EEPROM configuration version."); |
| } else { |
| System.out.println("Saved IMU configuration."); |
| } |
| |
| q.kill(); |
| } |
| |
| private static void writeConfiguration(String portIdentifier, String fileName) throws IOException { |
| System.out.println("Writing IMU configuration from file: " + fileName); |
| InputStream inputStream = new FileInputStream(fileName); |
| WriteIMUConfigurationRequestFrame frame = parseXMLIMUConfiguration(inputStream); |
| configure(portIdentifier, frame); |
| } |
| |
| private static IMUConfiguration readConfiguration(String portIdentifier) throws IOException { |
| MKConnection port = new RXTXSerialPort(); |
| port.init(portIdentifier); |
| FrameQueue q = new FrameQueue(port); |
| IMUConfiguration cs = null; |
| |
| ReadIMUConfigurationRequestFrame frame = new ReadIMUConfigurationRequestFrame(); |
| q.sendRequest(frame); |
| ReadIMUConfigurationResponseFrame r = (ReadIMUConfigurationResponseFrame) q.getResponseFor(frame, 5000); |
| if (r == null) { |
| System.err.println("ERROR. Timeout waiting for response."); |
| } else { |
| int paramSetVersion = r.getConfigurationVersion(); |
| int[] data = r.getData(); |
| cs = IMUConfiguration.parseXMLIMUConfiguration(paramSetVersion); |
| cs.setData(data); |
| System.out.println(cs.toXML()); |
| } |
| q.kill(); |
| return cs; |
| } |
| |
| private static void readConfiguration(String portIdentifier, String fileName) throws IOException { |
| IMUConfiguration cs = readConfiguration(portIdentifier); |
| if (cs != null) { |
| FileWriter fw = new FileWriter(fileName); |
| fw.write(cs.toXML()); |
| fw.close(); |
| } |
| } |
| |
| private static WriteIMUConfigurationRequestFrame parseXMLIMUConfiguration(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( |
| "/imuconfiguration/@eepromVersion", doc); |
| String ss_length = xpath.evaluate("/imuconfiguration/@length", doc); |
| |
| int s_length = 0; |
| try { |
| s_length = Integer.parseInt(ss_length); |
| } catch (NumberFormatException ex) { |
| // ignore. |
| } |
| |
| int eepromVersion = -1; |
| |
| try { |
| eepromVersion = Integer.parseInt(s_eepromVersion); |
| } catch (NumberFormatException ex) { |
| System.err |
| .println("The imuconfiguration element must have an 'eepromVersion' attribute with a numerical value (eg.<parameterset eepromVersion='74'>)"); |
| System.exit(-1); |
| } |
| |
| NodeList parameterNodes = (NodeList) xpath.evaluate( |
| "/imuconfiguration/parameter", doc, XPathConstants.NODESET); |
| int[] parameters = new int[parameterNodes.getLength()]; |
| |
| NodeList allChildNodes = (NodeList) xpath.evaluate("/imuconfiguration/*", doc, XPathConstants.NODESET); |
| |
| if (parameterNodes.getLength() != allChildNodes.getLength()) { |
| System.err.println("There seems to be a child element of imuconfiguration whose name is not \"parameter\"."); |
| // System.exit(-1); |
| } |
| |
| if (s_length >=0 && s_length != parameterNodes.getLength()) { |
| System.err.println("The number of parameters ("+parameterNodes.getLength()+") does not match the number in the length attribute of the imuconfiguration element ("+s_length+")."); |
| // System.exit(-1); |
| } |
| |
| for (int i = 0; i < parameterNodes.getLength(); i++) { |
| Element e = (Element) parameterNodes.item(i); |
| int value = 0; |
| if (e.hasAttribute("value")) { |
| String s_value = e.getAttribute("value"); |
| value = Integer.parseInt(s_value); |
| if (value < 0) |
| try { |
| value = Integer.parseInt(s_value); |
| } catch (NumberFormatException ex) { |
| throw new NumberFormatException( |
| "Value is not a number and not a variable name."); |
| } |
| } else { |
| NodeList bitNodes = (NodeList) xpath.evaluate("bit", e, |
| XPathConstants.NODESET); |
| for (int j = 0; j < 8; j++) { |
| Element bitElement = (Element) bitNodes.item(j); |
| if (bitElement != null) { |
| String s_bitValue = bitElement |
| .getAttribute("value"); |
| if ("1".equals(s_bitValue)) |
| value |= (1 << j); |
| else if (!"0".equals(s_bitValue)) |
| throw new NumberFormatException( |
| "Bit value was not 0 or 1."); |
| } |
| } |
| } |
| parameters[i] = value; |
| } |
| input.close(); |
| return new WriteIMUConfigurationRequestFrame(eepromVersion, parameters); |
| } 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); |
| } |
| } |
| |
| static void help() { |
| System.err.println("Usage: IMUConfigurator r [filename to write to]"); |
| System.err.println("Usage: IMUConfigurator w [filename to read from]"); |
| System.exit(-1); |
| } |
| |
| public static void main(String[] args) throws IOException { |
| if (!"r".equals(args[0]) && !"w".equals(args[0])) help(); |
| if ("w".equals(args[0]) && (args.length!=2)) help(); |
| if ("r".equals(args[0]) && (args.length!=2)) help(); |
| |
| String portIdentifier = null; |
| |
| if ("r".equals(args[0])) { |
| readConfiguration(portIdentifier, args[1]); |
| } else { |
| writeConfiguration(portIdentifier, args[1]); |
| } |
| System.exit(0); |
| } |
| } |