WebSVN - Projects - Diff - Rev 1613 and 1690 - /dongfang_FC_rewrite_tool/src/dongfang/mkt/main/IMUConfigurator.java


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) {
                        if (fileName != null) {
                                FileWriter fw = new FileWriter(fileName);
                                fw.write(cs.toXML());
                                fw.close();
                        }
                }
                System.out.println(cs.toXML());
        }
 
        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.length<2 ? null : args[1]);
                } else {
                        writeConfiguration(portIdentifier, args[1]);
                }
                System.exit(0);
        }
}

Rev 1613	Rev 1690
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	if (fileName != null) {
82	FileWriter fw = new FileWriter(fileName);	83	FileWriter fw = new FileWriter(fileName);
83	fw.write(cs.toXML());	84	fw.write(cs.toXML());
84	fw.close();	85	fw.close();
-		86	}
85	}	87	}
-		88	System.out.println(cs.toXML());
86	}	89	}
87		90
88	private static WriteIMUConfigurationRequestFrame parseXMLIMUConfiguration(InputStream input) throws IOException {	91	private static WriteIMUConfigurationRequestFrame parseXMLIMUConfiguration(InputStream input) throws IOException {
89	DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance();	92	DocumentBuilderFactory saxfac = DocumentBuilderFactory.newInstance();
90	saxfac.setValidating(false);	93	saxfac.setValidating(false);
91	try {	94	try {
92	DocumentBuilder bldr = saxfac.newDocumentBuilder();	95	DocumentBuilder bldr = saxfac.newDocumentBuilder();
93	Document doc = bldr.parse(input);	96	Document doc = bldr.parse(input);
94		97
95	XPath xpath = XPathFactory.newInstance().newXPath();	98	XPath xpath = XPathFactory.newInstance().newXPath();
96		99
97	String s_eepromVersion = xpath.evaluate(	100	String s_eepromVersion = xpath.evaluate(
98	"/imuconfiguration/@eepromVersion", doc);	101	"/imuconfiguration/@eepromVersion", doc);
99	String ss_length = xpath.evaluate("/imuconfiguration/@length", doc);	102	String ss_length = xpath.evaluate("/imuconfiguration/@length", doc);
100		103
101	int s_length = 0;	104	int s_length = 0;
102	try {	105	try {
103	s_length = Integer.parseInt(ss_length);	106	s_length = Integer.parseInt(ss_length);
104	} catch (NumberFormatException ex) {	107	} catch (NumberFormatException ex) {
105	// ignore.	108	// ignore.
106	}	109	}
107		110
108	int eepromVersion = -1;	111	int eepromVersion = -1;
109		112
110	try {	113	try {
111	eepromVersion = Integer.parseInt(s_eepromVersion);	114	eepromVersion = Integer.parseInt(s_eepromVersion);
112	} catch (NumberFormatException ex) {	115	} catch (NumberFormatException ex) {
113	System.err	116	System.err
114	.println("The imuconfiguration element must have an 'eepromVersion' attribute with a numerical value (eg.<imuconfiguration eepromVersion='74'>)");	117	.println("The imuconfiguration element must have an 'eepromVersion' attribute with a numerical value (eg.<imuconfiguration eepromVersion='74'>)");
115	System.exit(-1);	118	System.exit(-1);
116	}	119	}
117		120
118	NodeList parameterNodes = (NodeList) xpath.evaluate(	121	NodeList parameterNodes = (NodeList) xpath.evaluate(
119	"/imuconfiguration/parameter", doc, XPathConstants.NODESET);	122	"/imuconfiguration/parameter", doc, XPathConstants.NODESET);
120	int[] parameters = new int[parameterNodes.getLength()];	123	int[] parameters = new int[parameterNodes.getLength()];
121		124
122	NodeList allChildNodes = (NodeList) xpath.evaluate("/imuconfiguration/*", doc, XPathConstants.NODESET);	125	NodeList allChildNodes = (NodeList) xpath.evaluate("/imuconfiguration/*", doc, XPathConstants.NODESET);
123		126
124	if (parameterNodes.getLength() != allChildNodes.getLength()) {	127	if (parameterNodes.getLength() != allChildNodes.getLength()) {
125	System.err.println("There seems to be a child element of imuconfiguration whose name is not \"parameter\".");	128	System.err.println("There seems to be a child element of imuconfiguration whose name is not \"parameter\".");
126	// System.exit(-1);	129	// System.exit(-1);
127	}	130	}
128		131
129	if (s_length >=0 && s_length != parameterNodes.getLength()) {	132	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+").");	133	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);	134	// System.exit(-1);
132	}	135	}
133		136
134	for (int i = 0; i < parameterNodes.getLength(); i++) {	137	for (int i = 0; i < parameterNodes.getLength(); i++) {
135	Element e = (Element) parameterNodes.item(i);	138	Element e = (Element) parameterNodes.item(i);
136	int value = 0;	139	int value = 0;
137	if (e.hasAttribute("value")) {	140	if (e.hasAttribute("value")) {
138	String s_value = e.getAttribute("value");	141	String s_value = e.getAttribute("value");
139	value = Integer.parseInt(s_value);	142	value = Integer.parseInt(s_value);
140	if (value < 0)	143	if (value < 0)
141	try {	144	try {
142	value = Integer.parseInt(s_value);	145	value = Integer.parseInt(s_value);
143	} catch (NumberFormatException ex) {	146	} catch (NumberFormatException ex) {
144	throw new NumberFormatException(	147	throw new NumberFormatException(
145	"Value is not a number and not a variable name.");	148	"Value is not a number and not a variable name.");
146	}	149	}
147	} else {	150	} else {
148	NodeList bitNodes = (NodeList) xpath.evaluate("bit", e,	151	NodeList bitNodes = (NodeList) xpath.evaluate("bit", e,
149	XPathConstants.NODESET);	152	XPathConstants.NODESET);
150	for (int j = 0; j < 8; j++) {	153	for (int j = 0; j < 8; j++) {
151	Element bitElement = (Element) bitNodes.item(j);	154	Element bitElement = (Element) bitNodes.item(j);
152	if (bitElement != null) {	155	if (bitElement != null) {
153	String s_bitValue = bitElement	156	String s_bitValue = bitElement
154	.getAttribute("value");	157	.getAttribute("value");
155	if ("1".equals(s_bitValue))	158	if ("1".equals(s_bitValue))
156	value \|= (1 << j);	159	value \|= (1 << j);
157	else if (!"0".equals(s_bitValue))	160	else if (!"0".equals(s_bitValue))
158	throw new NumberFormatException(	161	throw new NumberFormatException(
159	"Bit value was not 0 or 1.");	162	"Bit value was not 0 or 1.");
160	}	163	}
161	}	164	}
162	}	165	}
163	parameters[i] = value;	166	parameters[i] = value;
164	}	167	}
165	input.close();	168	input.close();
166	return new WriteIMUConfigurationRequestFrame(eepromVersion, parameters);	169	return new WriteIMUConfigurationRequestFrame(eepromVersion, parameters);
167	} catch (ParserConfigurationException ex) {	170	} catch (ParserConfigurationException ex) {
168	// Should never happen.	171	// Should never happen.
169	throw new RuntimeException(ex);	172	throw new RuntimeException(ex);
170	} catch (SAXException ex) {	173	} catch (SAXException ex) {
171	System.err.println(ex);	174	System.err.println(ex);
172	System.err	175	System.err
173	.println("There is something screwed with your XML document. It is not well-formed and won't parse.");	176	.println("There is something screwed with your XML document. It is not well-formed and won't parse.");
174	throw new RuntimeException("Parse error.");	177	throw new RuntimeException("Parse error.");
175	} catch (XPathExpressionException ex) {	178	} catch (XPathExpressionException ex) {
176	// Should never happen.	179	// Should never happen.
177	throw new RuntimeException(ex);	180	throw new RuntimeException(ex);
178	}	181	}
179	}	182	}
180		183
181	static void help() {	184	static void help() {
182	System.err.println("Usage: IMUConfigurator r [filename to write to]");	185	System.err.println("Usage: IMUConfigurator r [filename to write to]");
183	System.err.println("Usage: IMUConfigurator w [filename to read from]");	186	System.err.println("Usage: IMUConfigurator w [filename to read from]");
184	System.exit(-1);	187	System.exit(-1);
185	}	188	}
186		189
187	public static void main(String[] args) throws IOException {	190	public static void main(String[] args) throws IOException {
188	if (!"r".equals(args[0]) && !"w".equals(args[0])) help();	191	if (!"r".equals(args[0]) && !"w".equals(args[0])) help();
189	if ("w".equals(args[0]) && (args.length!=2)) help();	192	if ("w".equals(args[0]) && (args.length!=2)) help();
190	if ("r".equals(args[0]) && (args.length!=2)) help();	193	if ("r".equals(args[0]) && (args.length>2)) help();
191		194
192	String portIdentifier = null;	195	String portIdentifier = null;
193		196
194	if ("r".equals(args[0])) {	197	if ("r".equals(args[0])) {
195	readConfiguration(portIdentifier, args[1]);	198	readConfiguration(portIdentifier, args.length<2 ? null : args[1]);
196	} else {	199	} else {
197	writeConfiguration(portIdentifier, args[1]);	200	writeConfiguration(portIdentifier, args[1]);
198	}	201	}
199	System.exit(0);	202	System.exit(0);
200	}	203	}
201	}	204	}

Subversion Repositories Projects

(root)/dongfang_FC_rewrite_tool/src/dongfang/mkt/main/IMUConfigurator.java @ 1806 - Rev 1613 → 1690