OpcUaCanOpen
CANopen OPC-UA server
CanSDOObject.cpp
Go to the documentation of this file.
1 #include <opcua_basedatavariabletype.h>
2 #include "uabasenodes.h"
3 #include "CanSDOObject.h"
4 #include "CanSDOItem.h"
5 #include "CanNodeObject.h"
6 #include "CanOpenData.h"
7 #include "CANopen.h"
8 #include <uadatetime.h>
9 #include "uasemaphore.h"
10 #include "UaCanTrace.h"
11 #include <boost/lexical_cast.hpp>
12 
13 
14 namespace CanOpen
15 {
16  using boost::lexical_cast;
17  using boost::bad_lexical_cast;
18  /*
19  * Constructor of CanSDOObject
20  * @par CanObject using to pass message to bus
21  * @conf SDO xml entry
22  * @blink
23  */
24  CanSDOObject::CanSDOObject(pUserDeviceStruct *par,
25  SDO *conf,UaNode *blink,int code) : CanOpenObject( par, conf,blink,code)
26  {
27  const char *p = conf->index().data();
28 
29  m_ind = (OpcUa_UInt16)strtol(p,NULL,16);
30  m_buffer.resize(8);
31  CanNodeObject *cno = static_cast<CanNodeObject *>(par->getDevice());
32  m_cobId = cno->getCanNodeId();
33 
34  m_iCount = 0;
35  m_iMaxCount = 1;
36  }
37 
38  OpcUa_BuiltInType CanSDOObject::getItemType()
39  {
40  return m_pSDOItem->getItemType();
41  }
42 
43  OpcUa_Byte CanSDOObject::getSubIndex()
44  {
45  return m_pSDOItem->getSubIndex();
46  }
47 
48 /*
49  CanSDOObject::CanSDOObject(UserDataBase *par,
50  SDOITEM_TYPE2 *conf,UaNode *blink,int code) : CanOpenObject( par, conf,blink,code)
51  {
52  char *p;
53  m_ind = (OpcUa_UInt16)strtol(conf->index().c_data(),&p,16);
54  m_buffer.resize(8);
55 // CanNodeObject *cno = ((UserDataBase_ptr<CanNodeObject> *)par)->getDevice();
56  CanNodeObject *cno = (CanNodeObject *)par;
57  m_cobId = cno->getCanNodeId();
58 
59  m_iCount = 0;
60  m_iMaxCount = 1;
61  }
62  */
63  /*
64  * Constructor of CanSDOObject
65  * @index of SDO message
66  */
67  /*
68  CanSDOObject::CanSDOObject(OpcUa_UInt16 index) : CanOpenObject(0,0,0,Ba_SDOType)
69  , m_waitSdo(0,1)
70  {
71  m_ind = index;
72  m_buffer.resize(8);
73  }
74  */
78  void CanSDOObject::pass(const CanMsgStruct *cms)
79  {
80 
81  m_udt = UaDateTime::fromTime_t(cms->c_time.tv_sec); // write time
82  m_udt.addMilliSecs(cms->c_time.tv_usec/1000); // in millisecond
83 
84  for(int i = 0; i < cms->c_dlc; i++)
85  m_buffer[i] = cms->c_data[i]; // put data
86  messageCame(); // inform that message came
87  }
88 
89 
90  CanSDOObject::~CanSDOObject(void)
91  {
92  }
93 
97  bool CanSDOObject::isMsgForObject(const CanMsgStruct *cms)
98  {
99  short index = cms->c_data[2];
100  index = (index << 8) + cms->c_data[1];
101  return ((cms->c_id == CANOPEN_SDOSERVER_COBID + getCobId()) && (m_ind == index) && (getSubIndex() == cms->c_data[3]));
102  }
103 
104  UaStatus CanSDOObject::getDeviceMessage(OpcUa_UInt32 code)
105  {
106  OpcUa_ReferenceParameter(code);
107  UaStatus ret = OpcUa_Good;
108 
109  lock();
110 
111  ret = getCanBus()->sendMessage(CANOPEN_SDOCLIENT_COBID + getCobId(), 8,(unsigned char *)(m_buffer.data()));
112  unlock();
113  if (ret.isGood()) {
114  ret = waitOperation();
115  }
116  return ret;
117  }
118  UaStatus CanSDOObject::sendDeviceMessage(OpcUa_UInt32 code, UaDataValue *value)
119  {
120  OpcUa_ReferenceParameter(code);
121  OpcUa_ReferenceParameter(value);
122  UaStatus ret = OpcUa_Good;
123  lock();
124  ret = getCanBus()->sendMessage(CANOPEN_SDOCLIENT_COBID + this->getCobId(),8,(unsigned char *)m_buffer.data());
125  unlock();
126  if (ret.isGood()) {
127  ret = waitOperation();
128  }
129  return ret;
130  }
131 
132  UaStatus CanSDOObject::connectCode(OpcUa_UInt32 code,::xsd::cxx::tree::type *conf,UaNode *blink)
133  {
134  OpcUa_ReferenceParameter(code);
135  OpcUa_ReferenceParameter(conf);
136  OpcUa_ReferenceParameter(blink);
137 
138  //UaDataValue udt;
139  //UaVariant val;
140  //UaDateTime sdt = UaDateTime::now();
141  //char *end;
142 
143  //SDO *sdob = (SDO *)conf;
144 
145  //if (code == BA_SDOOBJECT_INDEX) {
146 
147  // m_ind = strtoul(sdob->index().c_str(),&end,16);
148  // val.setString(UaString(sdob->index().c_str()));
149  //
150  // udt.setDataValue(val, OpcUa_False, OpcUa_Good, sdt, sdt);
151  // ((OpcUa::BaseDataVariableType *)blink)->setValue(0, udt, OpcUa_False);
152  // return OpcUa_Good;
153  //}
154  //else return OpcUa_Bad;
155  return OpcUa_Good;
156  }
157 
158  UaVariant CanSDOObject::getPropertyValue(OpcUa_UInt32 code, ::xsd::cxx::tree::type *conf)
159  {
160  UaVariant val;
161  val.clear();
162  SDO *sdob = (SDO *)conf;
163 
164  if (code == BA_SDOOBJECT_INDEX) {
165 // m_ind = stoi(sdob->index());
166 // m_ind = strtoul(sdob->index().c_str(), NULL, 16);
167  val.setString(UaString(sdob->index().c_str()));
168 
169  }
170  return val;
171  }
172 
173 
174  UaStatus CanSDOObject::readSdo(UaDataValue &udv)
175  {
176  UaStatus ret = OpcUa_Good;
177  OpcUa_Boolean togleBit = false;
178 
180  m_buffer[0] = SDO_INITIATE_UPLOAD_REQ;
181  m_buffer[1] = 0xFF & m_ind;
182  m_buffer[2] = 0xFF & (m_ind >> 8);
183  m_buffer[3] = getSubIndex();
184  m_buffer[4] = m_buffer[5] = m_buffer[6] = m_buffer[7] = 0;
185  if (getItemType() == OpcUaType_ByteString)
186  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Read request " << "CobId=" << hex << getCobId() << " index = " << hex << getIndex() << " subindex = " << hex << (int)getSubIndex();
187  else
188  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Read request " << "CobId=" << hex << getCobId() << " index = " << hex << getIndex() << " subindex= " << hex << std::hex << (int)getSubIndex();
189 
190 
191  ret = getDeviceMessage(BA_SDOITEMTYPE); // send read request
192  if (ret.isBad()) {
193  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Read request bad " << ret.toString().toUtf8() << " " << "CobId=" << hex << getCobId() << " index = " << hex <<
194  getIndex() << " subindex = " << std::hex << (int)getSubIndex();
195  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Read request bad " << ret.toString().toUtf8() << " " << "CobId=" << hex << getCobId() <<
196  " index = " << hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
197 
198  return ret;
199  }
200  OpcUa_UInt32 len;
201 
202  if (m_buffer[0] & SDO_INITIATE_UPLOAD_RESP) {
203  if ((m_buffer[0] & SDO_EXPEDITED)) { // expedited transfer
204  OpcUa_Byte nn;
205  if (m_buffer[0] & SDO_DATA_SIZE_INDICATED) {
206  nn = 4 - ((m_buffer[0] & SDO_DATA_SIZE_MASK) >> 2);
207  }
208  else nn = 0;
209  if (getItemType() == OpcUaType_ByteString) {
210  m_iLength = nn;
211  m_segSdoBuffer.resize(nn);
212  for (OpcUa_Int32 i = 0; i < nn; i++) {
213  m_segSdoBuffer[i] = m_buffer[4 + i];
214  }
215  }
216  UaDateTime sdt = UaDateTime::now();
217  UaVariant val = getSDOItem()->UnPack();
218  udv.setDataValue(val, OpcUa_False, OpcUa_Good, m_udt, sdt);
219  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Read answer " << "CobId=" << std::hex << getCobId() << " " << std::hex << m_buffer.data()[0] <<
220  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex() << " "
221  << std::hex << (int)m_buffer.data()[4] << " " << std::hex << (int)m_buffer.data()[5] << " " << std::hex << (int)m_buffer.data()[6] << " " << std::hex << (int)m_buffer.data()[7];
222 
223  }
224  else {
225  len = (OpcUa_Byte)m_buffer[7];
226  len = (((((len << 8) + (OpcUa_Byte)m_buffer[6]) << 8) + (OpcUa_Byte)m_buffer[5]) << 8) + (OpcUa_Byte)m_buffer[4];
227  m_segSdoBuffer.resize(len);
228  m_iLength = len;
229  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " initial read answer " << "CobId=" << std::hex << getCobId() << " " << std::hex << m_buffer.data()[0] <<
230  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex() << " len = " << std::hex << len;
231 
232  OpcUa_UInt32 nByte = 0;
233 
234  while (nByte < len) {
235  m_buffer[0] = (togleBit ? (SDO_UPLOAD_SEGMENT_REQ | SDO_TOGGLE_BIT) : SDO_UPLOAD_SEGMENT_REQ);
236  togleBit = !togleBit;
237  m_buffer[1] = 0xFF & m_ind;
238  m_buffer[2] = 0xFF & (m_ind >> 8);
239  m_buffer[3] = getSubIndex();
240  m_buffer[4] = m_buffer[5] = m_buffer[6] = m_buffer[7] = 0;
241  ret = getDeviceMessage(BA_SDOITEMTYPE); // send read request
242  if (ret.isBad()) {
243  LOG(Log::DBG, SegSdoMessage) << " bus= " << getBusName() << " read " << ret.pDiagnosticInfo()->m_localizedText.toString().toUtf8() <<
244  " " << ret.toString().toUtf8() << " CobId= " << std::hex << getCobId() << " index = " << std::hex << getIndex() << " subindex = " << std::hex << getSubIndex();
245 
246  (static_cast<CanNodeObject *>(getParentDevice()))->freeRequestSDO();
247  return ret;
248  }
249  LOG(Log::DBG, SegSdoMessage) << " bus= " << getBusName() << " read " << "CobId=" << std::hex << getCobId() << " " << std::hex <<
250  (int)m_buffer.data()[0] << " " << std::hex << (int)m_buffer.data()[1] << " " << std::hex << (int)m_buffer.data()[2] << " " << std::hex << (int)m_buffer.data()[3] << " " << std::hex <<
251  (int)m_buffer.data()[4] << " " << std::hex << (int)m_buffer.data()[5] << " " << std::hex << (int)m_buffer.data()[6] << " " << std::hex << (int)m_buffer.data()[7];
252 
253  OpcUa_UInt32 header = m_buffer[0] & 0xE0;
254  if (header == SDO_UPLOAD_SEGMENT_RESP) {
255  OpcUa_Int32 lll = len - nByte;
256  if (lll < 8) {
257  if (m_buffer[0] & SDO_LAST_SEGMENT) {
258  for (OpcUa_Int32 i = 1; i <= lll; i++)
259  m_segSdoBuffer[(OpcUa_Int32)nByte++] = m_buffer[i];
260  UaDateTime sdt = UaDateTime::now();
261  UaVariant val;
262  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Read String " << "CobId=" << std::hex << getCobId() <<
263  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex() << " len = " << len << " " << m_segSdoBuffer.toHex().toUtf8();
264 
265  val = getSDOItem()->UnPack();
266  udv.setDataValue(val, OpcUa_False, OpcUa_Good, m_udt, sdt);
267  return ret;
268  }
269  else {
270  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " read length was incorrect " << "CobId=" << std::hex << getCobId() <<
271  " index = " << std::hex << getIndex() << " subindex = " << std::hex << getSubIndex();
272 
273  ret.setStatus(OpcUa_UncertainNoCommunicationLastUsableValue, "Segmented SDO transfer was incorrect");
274  return ret;
275  }
276  }
277  else {
278  for (OpcUa_Int32 i = 1; i < 8; i++)
279  m_segSdoBuffer[(OpcUa_Int32)nByte++] = m_buffer[i];
280  }
281  }
282  else {
283  if (m_buffer[0] & SDO_ABORT_TRANSFER) {
284  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " read Aborted " << "CobId=" << std::hex << getCobId() <<
285  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
286  ret.setStatus(OpcUa_UncertainNoCommunicationLastUsableValue, "SDO transfer was aborted");
287  }
288  else {
289  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " read answer was incorrect " << "CobId=" << std::hex << getCobId() <<
290  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
291  ret.setStatus(OpcUa_UncertainNoCommunicationLastUsableValue, "Segmented SDO transfer was incorrect");
292  }
293  return ret;
294  }
295  }
296  }
297  }
298  else
299  if (m_buffer[0] & SDO_ABORT_TRANSFER) {
300  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " read Aborted " << "CobId=" << std::hex << getCobId() <<
301  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
302  ret.setStatus(OpcUa_UncertainNoCommunicationLastUsableValue, "SDO transfer was aborted");
303  }
304  else {
305  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Read String " << "CobId=" << std::hex << getCobId() <<
306  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex() << " len = " << m_iLength << " " << m_segSdoBuffer.toHex().toUtf8();
307  }
308  return ret;
309  }
310 
311 
312 
313  UaStatus CanSDOObject::writeSdo(UaDataValue &udv)
314  {
315  UaStatus ret = OpcUa_Good;
316  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Write request " << "CobId=" << hex << getCobId() <<
317  " index = " << hex << getIndex() << " subindex=" << hex << (int)getSubIndex() << " fun=" << hex << setw(2) << (int)m_buffer[0];
318  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Write request " << "CobId=" << hex << getCobId() <<
319  " index=" << hex << getIndex() << " subindex=" << hex << std::hex << (int)getSubIndex() << " fun=" << hex << setw(2) << (int)m_buffer[0];
320 
321  ret = getSDOItem()->pack(udv.value());
322  if (ret.isGood()) {
323  OpcUa_Int32 len = m_iLength;
324  OpcUa_Int32 index = 0;
325  OpcUa_Byte nn;
326 
327  ret = sendDeviceMessage(BA_SDOITEMTYPE);
328  if (ret.isBad()) {
329  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Write bad " << ret.toString().toUtf8() << " CobId=" << std::hex << getCobId() <<
330  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
331  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Write bad " << ret.toString().toUtf8() << " CobId=" << std::hex << getCobId() <<
332  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
333 
334  return ret;
335  }
336 
337  OpcUa_UInt32 header = m_buffer[0] & 0xF0;
338  if (header == SDO_INITIATE_DOWNLOAD_RESP) {
339  if (getItemType() == OpcUaType_ByteString) {
340  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " initial write answer " << "CobId=" << std::hex << getCobId() <<
341  " " << std::hex << setw(2) << m_buffer.data()[0] << " index = " << std::hex << getIndex() << " subindex = " << std::hex << setw(2) << (int)getSubIndex() << " len = " << len;
342 
343  // tSegSdoMessage(" bus %s initial write answer node=%X func %X index=%X subindex=%hhX len=%X",getBusName(), getCobId(), m_buffer.c_data()[0],getIndex(),getSubIndex(),len);
344 
345  if (len < 8) {
346  nn = 7 - (OpcUa_Byte)len;
347  m_buffer[0] = SDO_DOWNLOAD_SEGMENT_REQ | SDO_LAST_SEGMENT | (nn << 1);
348  for (OpcUa_Int32 i = 1; i <= len; i++)
349  m_buffer[i] = m_segSdoBuffer[index++];
350  ret = sendDeviceMessage(BA_SDOITEMTYPE);
351  if (ret.isBad()) {
352  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Write bad " << ret.toString().toUtf8() << " CobId=" << std::hex << getCobId() <<
353  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
354 
355  return ret;
356  }
357  }
358  else {
359  m_buffer[0] = SDO_DOWNLOAD_SEGMENT_REQ;
360  while (len > 0) {
361  for (OpcUa_Int32 i = 1; i < 8; i++)
362  m_buffer[i] = m_segSdoBuffer[index++];
363  len -= 7;
364  ret = sendDeviceMessage(BA_SDOITEMTYPE);
365  if (ret.isBad()) {
366  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Write bad " << ret.toString().toUtf8() <<
367  "CobId=" << std::hex << getCobId() << " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
368  return ret;
369  }
370  header = m_buffer[0] & 0xE0;
371  if (header == SDO_DOWNLOAD_SEGMENT_RESP) {
372  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Write answer " << "CobId=" << std::hex << getCobId() << " " << std::hex << m_buffer.data()[0] <<
373  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex() << " len = " << len;
374 
375  if (len < 8) {
376  nn = 7 - (OpcUa_Byte)len;
377  if (m_buffer[0] & SDO_TOGGLE_BIT)
378  m_buffer[0] = SDO_DOWNLOAD_SEGMENT_REQ | SDO_LAST_SEGMENT | (nn << 1);
379  else {
380  m_buffer[0] = SDO_DOWNLOAD_SEGMENT_REQ | SDO_LAST_SEGMENT | (nn << 1);
381  m_buffer[0] = m_buffer[0] | SDO_TOGGLE_BIT;
382  }
383  for (OpcUa_Int32 i = 1; i <= len; i++)
384  m_buffer[i] = m_segSdoBuffer[index++];
385  ret = sendDeviceMessage(BA_SDOITEMTYPE);
386  if (ret.isBad()) {
387  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Write" << ret.toString().toUtf8() << " CobId=" << std::hex << getCobId() <<
388  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
389 
390  return ret;
391  }
392  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " End write answer " << "CobId=" << std::hex << getCobId() << " " << std::hex << m_buffer.data()[0] <<
393  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
394 
395  len = 0;
396  }
397  else {
398  if (m_buffer[0] & SDO_TOGGLE_BIT)
399  m_buffer[0] = SDO_DOWNLOAD_SEGMENT_REQ;
400  else
401  m_buffer[0] = SDO_DOWNLOAD_SEGMENT_REQ | SDO_TOGGLE_BIT;
402  }
403  }
404  else {
405  if (m_buffer[0] & SDO_ABORT_TRANSFER) {
406  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Write request Aborted " << "CobId=" << std::hex << getCobId() <<
407  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
408 
409  ret.setStatus(OpcUa_UncertainNoCommunicationLastUsableValue, "SDO transfer was aborted");
410  }
411  else {
412  LOG(Log::DBG, SegSdoMessage) << " bus " << getBusName() << " Write answer was incorrect " << "CobId=" << std::hex << getCobId() <<
413  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
414 
415  ret.setStatus(OpcUa_UncertainNoCommunicationLastUsableValue, "Segmented SDO transfer was incorrect");
416  }
417  return ret;
418  }
419  }
420  }
421  m_iLength = 0;
422 
423  }
424  }
425  if (m_buffer[0] & SDO_ABORT_TRANSFER) {
426  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Write request Aborted " << "CobId=" << std::hex << getCobId() <<
427  " index = " << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
428 
429  ret.setStatus(OpcUa_UncertainNoCommunicationLastUsableValue, "SDO transfer was aborted");
430  }
431  else {
432  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Last respons Correct " << "CobId=" << std::hex << getCobId() << " index = " << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
433 
434  }
435  }
436  else {
437  LOG(Log::DBG, SdoMessage) << " bus " << getBusName() << " Write bad " << ret.toString().toUtf8() << " CobId=" << std::hex << getCobId() <<
438  " index = " << std::hex << getIndex() << " subindex = " << std::hex << (int)getSubIndex();
439 
440  }
441  return ret;
442  }
443 
444  void CanSDOObject::setSDOItem(CanSDOItem *csdo)
445  {
446  static_cast<CanNodeObject *>(getParentDevice())->setRequestSDO(this);
447  m_pSDOItem = csdo;
448  }
449  void CanSDOObject::freeSDOItem()
450  {
451  m_pSDOItem = 0;
452  static_cast<CanNodeObject *>(getParentDevice())->freeRequestSDO();
453  }
454 
455 }