1 /***************************************************************************** |
1 /***************************************************************************** |
2 * |
2 * |
3 * $Id$ |
3 * $Id$ |
4 * |
4 * |
5 * Copyright (C) 2006-2009 Florian Pose, Ingenieurgemeinschaft IgH |
5 * Copyright (C) 2006-2012 Florian Pose, Ingenieurgemeinschaft IgH |
6 * |
6 * |
7 * This file is part of the IgH EtherCAT Master. |
7 * This file is part of the IgH EtherCAT Master. |
8 * |
8 * |
9 * The IgH EtherCAT Master is free software; you can redistribute it and/or |
9 * The IgH EtherCAT Master is free software; you can redistribute it and/or |
10 * modify it under the terms of the GNU General Public License version 2, as |
10 * modify it under the terms of the GNU General Public License version 2, as |
49 { |
49 { |
50 stringstream str; |
50 stringstream str; |
51 |
51 |
52 str << binaryBaseName << " " << getName() |
52 str << binaryBaseName << " " << getName() |
53 << " [OPTIONS] <OFFSET> <DATA>" << endl |
53 << " [OPTIONS] <OFFSET> <DATA>" << endl |
54 << endl |
54 << endl |
55 << getBriefDescription() << endl |
55 << getBriefDescription() << endl |
56 << endl |
56 << endl |
57 << "This command requires a single slave to be selected." << endl |
57 << "This command requires a single slave to be selected." << endl |
58 << endl |
58 << endl |
59 << "Arguments:" << endl |
59 << "Arguments:" << endl |
60 << " OFFSET is the register address to write to." << endl |
60 << " ADDRESS is the register address to write to." << endl |
61 << " DATA depends on whether a datatype was specified" << endl |
61 << " DATA depends on whether a datatype was specified" << endl |
62 << " with the --type option: If not, DATA must be" << endl |
62 << " with the --type option: If not, DATA must be" << endl |
63 << " either a path to a file with data to write," << endl |
63 << " either a path to a file with data to write," << endl |
64 << " or '-', which means, that data are read from" << endl |
64 << " or '-', which means, that data are read from" << endl |
65 << " stdin. If a datatype was specified, VALUE is" << endl |
65 << " stdin. If a datatype was specified, VALUE is" << endl |
66 << " interpreted respective to the given type." << endl |
66 << " interpreted respective to the given type." << endl |
67 << endl |
67 << endl |
68 << typeInfo() |
68 << typeInfo() |
69 << endl |
69 << endl |
70 << "Command-specific options:" << endl |
70 << "Command-specific options:" << endl |
71 << " --alias -a <alias>" << endl |
71 << " --alias -a <alias>" << endl |
72 << " --position -p <pos> Slave selection. See the help of" << endl |
72 << " --position -p <pos> Slave selection. See the help of" |
73 << " the 'slaves' command." << endl |
73 << endl |
74 << " --type -t <type> Data type (see above)." << endl |
74 << " the 'slaves' command." << endl |
|
75 << " --type -t <type> Data type (see above)." << endl |
|
76 << " --emergency -e Send as emergency request." << endl |
75 << endl |
77 << endl |
76 << numericInfo(); |
78 << numericInfo(); |
77 |
79 |
78 return str.str(); |
80 return str.str(); |
79 } |
81 } |
81 /****************************************************************************/ |
83 /****************************************************************************/ |
82 |
84 |
83 void CommandRegWrite::execute(const StringVector &args) |
85 void CommandRegWrite::execute(const StringVector &args) |
84 { |
86 { |
85 stringstream strOffset, err; |
87 stringstream strOffset, err; |
86 ec_ioctl_slave_reg_t data; |
88 ec_ioctl_slave_reg_t io; |
87 ifstream file; |
89 ifstream file; |
88 SlaveList slaves; |
|
89 |
90 |
90 if (args.size() != 2) { |
91 if (args.size() != 2) { |
91 err << "'" << getName() << "' takes exactly two arguments!"; |
92 err << "'" << getName() << "' takes exactly two arguments!"; |
92 throwInvalidUsageException(err); |
93 throwInvalidUsageException(err); |
93 } |
94 } |
94 |
95 |
95 strOffset << args[0]; |
96 strOffset << args[0]; |
96 strOffset |
97 strOffset |
97 >> resetiosflags(ios::basefield) // guess base from prefix |
98 >> resetiosflags(ios::basefield) // guess base from prefix |
98 >> data.offset; |
99 >> io.address; |
99 if (strOffset.fail()) { |
100 if (strOffset.fail()) { |
100 err << "Invalid offset '" << args[0] << "'!"; |
101 err << "Invalid address '" << args[0] << "'!"; |
101 throwInvalidUsageException(err); |
102 throwInvalidUsageException(err); |
102 } |
103 } |
103 |
104 |
104 if (getDataType().empty()) { |
105 if (getDataType().empty()) { |
105 if (args[1] == "-") { |
106 if (args[1] == "-") { |
106 loadRegData(&data, cin); |
107 loadRegData(&io, cin); |
107 } else { |
108 } else { |
108 file.open(args[1].c_str(), ifstream::in | ifstream::binary); |
109 file.open(args[1].c_str(), ifstream::in | ifstream::binary); |
109 if (file.fail()) { |
110 if (file.fail()) { |
110 err << "Failed to open '" << args[1] << "'!"; |
111 err << "Failed to open '" << args[1] << "'!"; |
111 throwCommandException(err); |
112 throwCommandException(err); |
112 } |
113 } |
113 loadRegData(&data, file); |
114 loadRegData(&io, file); |
114 file.close(); |
115 file.close(); |
115 } |
116 } |
116 } else { |
117 } else { |
117 stringstream strValue; |
118 stringstream strValue; |
118 const DataType *dataType = findDataType(getDataType()); |
119 const DataType *dataType = findDataType(getDataType()); |
121 err << "Invalid data type '" << getDataType() << "'!"; |
122 err << "Invalid data type '" << getDataType() << "'!"; |
122 throwInvalidUsageException(err); |
123 throwInvalidUsageException(err); |
123 } |
124 } |
124 |
125 |
125 if (dataType->byteSize) { |
126 if (dataType->byteSize) { |
126 data.length = dataType->byteSize; |
127 io.size = dataType->byteSize; |
127 } else { |
128 } else { |
128 data.length = 1024; // FIXME |
129 io.size = 1024; // FIXME |
129 } |
130 } |
130 |
131 |
131 data.data = new uint8_t[data.length]; |
132 io.data = new uint8_t[io.size]; |
132 |
133 |
133 try { |
134 try { |
134 data.length = interpretAsType( |
135 io.size = interpretAsType( |
135 dataType, args[1], data.data, data.length); |
136 dataType, args[1], io.data, io.size); |
136 } catch (SizeException &e) { |
137 } catch (SizeException &e) { |
137 delete [] data.data; |
138 delete [] io.data; |
138 throwCommandException(e.what()); |
139 throwCommandException(e.what()); |
139 } catch (ios::failure &e) { |
140 } catch (ios::failure &e) { |
140 delete [] data.data; |
141 delete [] io.data; |
141 err << "Invalid value argument '" << args[1] |
142 err << "Invalid value argument '" << args[1] |
142 << "' for type '" << dataType->name << "'!"; |
143 << "' for type '" << dataType->name << "'!"; |
143 throwInvalidUsageException(err); |
144 throwInvalidUsageException(err); |
144 } |
145 } |
145 } |
146 } |
146 |
147 |
147 if ((uint32_t) data.offset + data.length > 0xffff) { |
148 if ((uint32_t) io.address + io.size > 0xffff) { |
148 err << "Offset and length exceeding 64k!"; |
149 err << "Address and size exceeding 64k!"; |
149 delete [] data.data; |
150 delete [] io.data; |
150 throwInvalidUsageException(err); |
151 throwInvalidUsageException(err); |
151 } |
152 } |
152 |
153 |
153 MasterDevice m(getSingleMasterIndex()); |
154 MasterDevice m(getSingleMasterIndex()); |
154 try { |
155 try { |
155 m.open(MasterDevice::ReadWrite); |
156 m.open(MasterDevice::ReadWrite); |
156 } catch (MasterDeviceException &e) { |
157 } catch (MasterDeviceException &e) { |
157 delete [] data.data; |
158 delete [] io.data; |
158 throw e; |
159 throw e; |
159 } |
160 } |
160 |
161 |
161 slaves = selectedSlaves(m); |
162 if (getEmergency()) { |
162 if (slaves.size() != 1) { |
163 io.slave_position = emergencySlave(); |
163 delete [] data.data; |
164 io.emergency = true; |
164 throwSingleSlaveRequired(slaves.size()); |
165 } |
165 } |
166 else { |
166 data.slave_position = slaves.front().position; |
167 SlaveList slaves = selectedSlaves(m); |
|
168 if (slaves.size() != 1) { |
|
169 delete [] io.data; |
|
170 throwSingleSlaveRequired(slaves.size()); |
|
171 } |
|
172 io.slave_position = slaves.front().position; |
|
173 io.emergency = false; |
|
174 } |
167 |
175 |
168 // send data to master |
176 // send data to master |
169 try { |
177 try { |
170 m.writeReg(&data); |
178 m.writeReg(&io); |
171 } catch (MasterDeviceException &e) { |
179 } catch (MasterDeviceException &e) { |
172 delete [] data.data; |
180 delete [] io.data; |
173 throw e; |
181 throw e; |
174 } |
182 } |
175 |
183 |
176 if (getVerbosity() == Verbose) { |
184 if (getVerbosity() == Verbose) { |
177 cerr << "Register writing finished." << endl; |
185 cerr << "Register writing finished." << endl; |
178 } |
186 } |
179 |
187 |
180 delete [] data.data; |
188 delete [] io.data; |
181 } |
189 } |
182 |
190 |
183 /*****************************************************************************/ |
191 /*****************************************************************************/ |
184 |
192 |
185 void CommandRegWrite::loadRegData( |
193 void CommandRegWrite::loadRegData( |
186 ec_ioctl_slave_reg_t *data, |
194 ec_ioctl_slave_reg_t *io, |
187 const istream &in |
195 const istream &in |
188 ) |
196 ) |
189 { |
197 { |
190 stringstream err; |
198 stringstream err; |
191 ostringstream tmp; |
199 ostringstream tmp; |
199 |
207 |
200 if (contents.size() > 0xffff) { |
208 if (contents.size() > 0xffff) { |
201 err << "Invalid data size " << contents.size() << "!"; |
209 err << "Invalid data size " << contents.size() << "!"; |
202 throwInvalidUsageException(err); |
210 throwInvalidUsageException(err); |
203 } |
211 } |
204 data->length = contents.size(); |
212 io->size = contents.size(); |
205 |
213 |
206 // allocate buffer and read file into buffer |
214 // allocate buffer and read file into buffer |
207 data->data = new uint8_t[data->length]; |
215 io->data = new uint8_t[io->size]; |
208 contents.copy((char *) data->data, contents.size()); |
216 contents.copy((char *) io->data, contents.size()); |
209 } |
217 } |
210 |
218 |
211 /*****************************************************************************/ |
219 /*****************************************************************************/ |