22 |
22 |
23 /****************************************************************************/ |
23 /****************************************************************************/ |
24 |
24 |
25 void command_upload(void) |
25 void command_upload(void) |
26 { |
26 { |
27 stringstream strIndex, strSubIndex; |
27 stringstream err, strIndex, strSubIndex; |
28 int sval; |
28 int sval; |
29 ec_ioctl_slave_sdo_upload_t data; |
29 ec_ioctl_slave_sdo_upload_t data; |
30 unsigned int uval; |
30 unsigned int uval; |
31 const CoEDataType *dataType = NULL; |
31 const CoEDataType *dataType = NULL; |
32 |
32 |
33 if (slavePosition < 0) { |
33 if (slavePosition < 0) { |
34 stringstream err; |
34 err << "'" << commandName << "' requires a slave! " |
35 err << "'sdo_upload' requires a slave! Please specify --slave."; |
35 << "Please specify --slave."; |
36 throw MasterDeviceException(err.str()); |
36 throw InvalidUsageException(err); |
37 } |
37 } |
38 data.slave_position = slavePosition; |
38 data.slave_position = slavePosition; |
39 |
39 |
40 if (commandArgs.size() != 2) { |
40 if (commandArgs.size() != 2) { |
41 stringstream err; |
41 err << "'" << commandName << "' takes two arguments!"; |
42 err << "'sdo_upload' takes two arguments!"; |
42 throw InvalidUsageException(err); |
43 throw MasterDeviceException(err.str()); |
|
44 } |
43 } |
45 |
44 |
46 strIndex << commandArgs[0]; |
45 strIndex << commandArgs[0]; |
47 strIndex |
46 strIndex |
48 >> resetiosflags(ios::basefield) // guess base from prefix |
47 >> resetiosflags(ios::basefield) // guess base from prefix |
49 >> data.sdo_index; |
48 >> data.sdo_index; |
50 if (strIndex.fail()) { |
49 if (strIndex.fail()) { |
51 stringstream err; |
|
52 err << "Invalid Sdo index '" << commandArgs[0] << "'!"; |
50 err << "Invalid Sdo index '" << commandArgs[0] << "'!"; |
53 throw MasterDeviceException(err.str()); |
51 throw InvalidUsageException(err); |
54 } |
52 } |
55 |
53 |
56 strSubIndex << commandArgs[1]; |
54 strSubIndex << commandArgs[1]; |
57 strSubIndex |
55 strSubIndex |
58 >> resetiosflags(ios::basefield) // guess base from prefix |
56 >> resetiosflags(ios::basefield) // guess base from prefix |
59 >> uval; |
57 >> uval; |
60 if (strSubIndex.fail() || uval > 0xff) { |
58 if (strSubIndex.fail() || uval > 0xff) { |
61 stringstream err; |
|
62 err << "Invalid Sdo subindex '" << commandArgs[1] << "'!"; |
59 err << "Invalid Sdo subindex '" << commandArgs[1] << "'!"; |
63 throw MasterDeviceException(err.str()); |
60 throw InvalidUsageException(err); |
64 } |
61 } |
65 data.sdo_entry_subindex = uval; |
62 data.sdo_entry_subindex = uval; |
66 |
63 |
67 if (dataTypeStr != "") { // data type specified |
64 if (dataTypeStr != "") { // data type specified |
68 if (!(dataType = findDataType(dataTypeStr))) { |
65 if (!(dataType = findDataType(dataTypeStr))) { |
69 stringstream err; |
|
70 err << "Invalid data type '" << dataTypeStr << "'!"; |
66 err << "Invalid data type '" << dataTypeStr << "'!"; |
71 throw MasterDeviceException(err.str()); |
67 throw InvalidUsageException(err); |
72 } |
68 } |
73 } else { // no data type specified: fetch from dictionary |
69 } else { // no data type specified: fetch from dictionary |
74 ec_ioctl_slave_sdo_entry_t entry; |
70 ec_ioctl_slave_sdo_entry_t entry; |
75 |
71 |
76 masterDev.open(MasterDevice::Read); |
72 masterDev.open(MasterDevice::Read); |
77 |
73 |
78 try { |
74 try { |
79 masterDev.getSdoEntry(&entry, slavePosition, |
75 masterDev.getSdoEntry(&entry, slavePosition, |
80 data.sdo_index, data.sdo_entry_subindex); |
76 data.sdo_index, data.sdo_entry_subindex); |
81 } catch (MasterDeviceException &e) { |
77 } catch (MasterDeviceException &e) { |
82 stringstream err; |
|
83 err << "Failed to determine Sdo entry data type. " |
78 err << "Failed to determine Sdo entry data type. " |
84 << "Please specify --type."; |
79 << "Please specify --type."; |
85 throw ExecutionFailureException(err); |
80 throw CommandException(err); |
86 } |
81 } |
87 if (!(dataType = findDataType(entry.data_type))) { |
82 if (!(dataType = findDataType(entry.data_type))) { |
88 stringstream err; |
|
89 err << "Pdo entry has unknown data type 0x" |
83 err << "Pdo entry has unknown data type 0x" |
90 << hex << setfill('0') << setw(4) << entry.data_type << "!" |
84 << hex << setfill('0') << setw(4) << entry.data_type << "!" |
91 << " Please specify --type."; |
85 << " Please specify --type."; |
92 throw ExecutionFailureException(err); |
86 throw CommandException(err); |
93 } |
87 } |
94 } |
88 } |
95 |
89 |
96 if (dataType->byteSize) { |
90 if (dataType->byteSize) { |
97 data.target_size = dataType->byteSize; |
91 data.target_size = dataType->byteSize; |