Subversion Repositories Projects

Rev

Rev 1611 | Go to most recent revision | Blame | Compare with Previous | Last modification | View Log | RSS feed

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.<imuconfiguration 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);
        }
}