OpcUaCanOpen
CANopen OPC-UA server
UaCalcItem.cpp
Go to the documentation of this file.
1 #include "UaCalcItem.h"
2 #include "boost/bind/bind.hpp"
3 #include "UaCanTrace.h"
4 
5 #include <math.h>
6 
7 #ifdef WIN32
8 #define isnan(x) _isnan(x)
9 #define isinf(x) (!_finite(x))
10 #define fpu_error(x) (isinf(x) || isnan(x))
11 #endif
12 
13 namespace AddressSpace
14 {
15  using namespace std::placeholders;
16 // UaCalcItem::UaCalcItem(const UaString &name,UaNodeId& uaId, NmBuildingAutomation * pNodeConfig, UaVariant &dv, ITEM &citem, UaControlVariableMap &cvs, UaMutexRefCounted* pSharedMutex):
17 // UaControlVariable(name,uaId,pNodeConfig,dv,pSharedMutex)
18 
19 
20  struct ad_usr : public parser_t::unknown_symbol_resolver
21  {
22  typedef parser_t::unknown_symbol_resolver usr_t;
23 
24 // ad_usr(symbol_table_t &sb, UaCalcItem& par, void(*pushDV)(const string&, UaControlVariable *) )
25  ad_usr( UaCalcItem *par)// , std::function<void(const string&, UaControlVariable *)> pushVD)
26 // ad_usr()
27  : usr_t(usr_t::e_usrmode_extended), p(par)//, pDV(pushVD)
28  {}
29 
30  void setFunc(std::function<bool(const string&, UaControlVariable *)> pushVD) { pDV = pushVD; }
31 
32  virtual bool process(const std::string& unknown_symbol,
33  symbol_table_t& symbol_table,
34  std::string& error_message)
35  {
36  bool result = false;
37  UaNodeId nodeId;
38  UaNode *node;
39 
40  nodeId = p->m_pNodeManager->getNewNodeId(p->m_pParent, UaString(unknown_symbol.c_str()));
41  node = p->m_pNodeManager->findNode(nodeId);
42  if (node == NULL) {
43  nodeId = p->m_pNodeManager->getNewNodeId(NULL, UaString(unknown_symbol.c_str()));
44  node = p->m_pNodeManager->findNode(nodeId);
45  }
46 
47 
48  if (node != NULL)
49  {
50  // Default value of zero
51 // result = m_sb.create_variable(unknown_symbol,static_cast<UaControlVariable*>(node)->curValue);
52  result = pDV(unknown_symbol, static_cast<UaControlVariable*>(node));
53  if (!result)
54  {
55  error_message = "Failed to create variable...";
56  }
57  }
58  else
59  error_message = "Indeterminable symbol type.";
60  return result;
61  }
62  // symbol_table_t m_sb;
63  UaCalcItem *p;
64  std::function<bool(const string&, UaControlVariable *)> pDV;
65  };
66 
67  UaCalcItem::UaCalcItem(const UaString& name, UaNodeId& uaId, const UaNode *parent,
68  NmBuildingAutomation * pNodeConfig, UaVariant &dv, ITEM &citem, UaControlVariableMap &cvs, UaMutexRefCounted* pSharedMutex) :
69  UaControlVariable(name, uaId, pNodeConfig, dv, pSharedMutex), m_pNodeManager(pNodeConfig)
70  {
71  int i = 0;
72  double res;
73  UaDateTime sdt;
74  UaDataValue dataValue;
75  bool result = false;
76 // fmod f1;
77 
78  m_pParent = static_cast<const OpcUa::BaseObjectType*>(parent);
79  // string nname = citem.name();
80  m_cExists = citem.when().present();
81  m_sExists = citem.status().present();
82  string ssFormul = citem.value();
83  replaceAll(ssFormul, " && ", " and ");
84  replaceAll(ssFormul, " || ", " or ");
85 
86  m_sFormul = ssFormul;
87  if (checkDouble(m_sFormul, res)) {
88 
89  sdt = UaDateTime::now();
90 
91  UaVariant val = res;
92  if (isnan(res) || isinf(res)) {
93  dataValue.setDataValue(val, OpcUa_False, OpcUa_BadUnexpectedError, sdt, sdt);
94  }
95  else
96  dataValue.setDataValue(val, OpcUa_False, OpcUa_True, sdt, sdt);
97 
98  setValue(0, dataValue, false);
99  m_fExists = false;
100 
101 // return;
102  }
103  else {
104  m_fExists = true;
105 
106 // cout << "Create " << this->browseName().toString().toUtf8() << endl << flush;
107 
108  for (UaControlVariableMap::const_iterator vit = cvs.begin(); vit != cvs.end(); vit++)
109  {
110  string nn = (*vit).first;
111  UaControlVariable *uac = (*vit).second;
112 // cout << "Add Variable " << nn << " " << this->browseName().toString().toUtf8() << endl << flush;
113 
114  if (m_sFormul.find(nn) != string::npos)
115  result = pushDataVariable(nn, uac);
116  }
117  }
118  if (m_cExists) {
119  ssFormul = citem.when().get();
120  replaceAll(ssFormul, " && ", " and ");
121  replaceAll(ssFormul, " || ", " or ");
122  m_sWhenCondition = ssFormul;
123 
124  for (UaControlVariableMap::const_iterator vit = cvs.begin(); vit != cvs.end(); vit++)
125  {
126  string nn = (*vit).first;
127  UaControlVariable *uac = (*vit).second;
128  if (m_sWhenCondition.find(nn) != string::npos)
129  result = pushConditionVariable(nn, uac);
130  }
131 
132  }
133 
134  if (m_sExists) {
135  ssFormul = citem.status().get();
136  replaceAll(ssFormul, " && ", " and ");
137  replaceAll(ssFormul, " || ", " or ");
138  m_sStatusCondition = ssFormul;
139 
140  for (UaControlVariableMap::const_iterator vit = cvs.begin(); vit != cvs.end(); vit++)
141  {
142  string nn = (*vit).first;
143  UaControlVariable *uac = (*vit).second;
144  if (m_sStatusCondition.find(nn) != string::npos)
145  result = pushStatusVariable(nn, uac);
146  }
147  }
148 // res = m_whenCondition_table.add_function("fmod", f1);
149 // res = m_statusFormula_table.add_function("fmod", f1);
150  res = m_formul_table.add_function("fmod", f1);
151 // m_formul_table.add_constants();
152 // bool l = compile();
153  setValueHandling(UaVariable_Value_CacheIsSource);
154  setUserData(NULL);
155  }
156 
157  bool UaCalcItem::compile()
158  {
159 
160  bool ret;
161  std::vector<std::string> variable_list;
162  // OpcUa_Int32 nVar;
163  // UaControlVariableSet ucVariables;
164  if (m_cExists) {
165 
166  m_whenCondition.register_symbol_table(m_formul_table);
167  ad_usr wusr(this);
168  auto fp = std::bind(&UaCalcItem::pushConditionVariable, this, std::placeholders::_1, std::placeholders::_2);
169  wusr.setFunc(fp);
170  parser_b.enable_unknown_symbol_resolver(&wusr);
171  m_sWhenCondition.erase(std::remove(m_sWhenCondition.begin(), m_sWhenCondition.end(), '$'), m_sWhenCondition.end());
172  ret = parser_b.compile(m_sWhenCondition, m_whenCondition);
173 
174 // m_whenCondition.value();
175 
176 
177  if (!ret)
178  {
179  cout << "Error Condition: " << parser_b.error().c_str() << " " << this->browseName().toString().toUtf8() << " " << m_sWhenCondition << endl;
180  exit(-1);
181  }
182  // return ret;
183  /*
184  unknown_var_whenCondition_table.get_variable_list(variable_list);
185 
186  for (auto& var_name : variable_list)
187  {
188  double& v = unknown_var_whenCondition_table.variable_ref(var_name);
189  nodeId = m_pNodeManager->getNewNodeId(m_pParent, UaString(var_name.c_str()));
190  node = m_pNodeManager->findNode(nodeId);
191  v = static_cast<UaControlVariable*>(node)->curValue;
192  }
193  */
194 
195  }
196 
197  m_formul.register_symbol_table(m_formul_table);
198 
199  m_sFormul.erase(std::remove(m_sFormul.begin(), m_sFormul.end(), '$'), m_sFormul.end());
200 
201  ad_usr musr(this);
202  auto fp = std::bind(&UaCalcItem::pushDataVariable, this,std::placeholders::_1,std::placeholders::_2);
203  musr.setFunc(fp);
204 
205  parser_d.enable_unknown_symbol_resolver(&musr); //.enable_unknown_symbol_resolver(&musr);
206 
207  ret = parser_d.compile(m_sFormul, m_formul);
208  if (!ret) {
209  cout << "Error value: " << parser_d.error().c_str()<< " " << this->browseName().toString().toUtf8() << " " << m_sFormul << endl;
210  exit(-1);
211  }
212  /*
213  unknown_var_formul_table.get_variable_list(variable_list);
214 
215  for (auto& var_name : variable_list)
216  {
217  double& v = unknown_var_formul_table.variable_ref(var_name);
218  nodeId = m_pNodeManager->getNewNodeId(m_pParent, UaString(var_name.c_str()));
219  node = m_pNodeManager->findNode(nodeId);
220  v = static_cast<UaControlVariable*>(node)->curValue;
221  }
222  */
223 
224  if (m_sExists) {
225  m_statusFormula.register_symbol_table(m_formul_table);
226  m_sStatusCondition.erase(std::remove(m_sStatusCondition.begin(), m_sStatusCondition.end(), '$'), m_sStatusCondition.end());
227  auto fp = std::bind(&UaCalcItem::pushStatusVariable, this, std::placeholders::_1, std::placeholders::_2);
228  ad_usr susr(this);
229  susr.setFunc(fp);
230  parser_i.enable_unknown_symbol_resolver(&susr);
231  ret = parser_i.compile(m_sStatusCondition, m_statusFormula);
232  if (!ret)
233  {
234  cout << "Error Status: " << parser_i.error().c_str() << " " << this->browseName().toString().toUtf8() << " " << m_sStatusCondition << endl;
235  exit(-1);
236  }
237  /*
238  unknown_var_statusFormula_table.get_variable_list(variable_list);
239 
240  for (auto& var_name : variable_list)
241  {
242  double& v = unknown_var_statusFormula_table.variable_ref(var_name);
243  nodeId = m_pNodeManager->getNewNodeId(m_pParent, UaString(var_name.c_str()));
244  node = m_pNodeManager->findNode(nodeId);
245  v = static_cast<UaControlVariable*>(node)->curValue;
246  }
247  */
248  }
249 
250  return ret;
251  }
252 
253  void UaCalcItem::calculateOnChange(Session *pSession, const UaDataValue& dValue, OpcUa_Boolean checkAccessLevel)
254  {
255  boost::arg<1> _1;
256  UaDataValue dataValue;
257  UaDateTime udt,sdt;
258  UaStatus Status = OpcUa_Good;
259  double res = this->curValue;
260  bool cond = false;
261  UaString sDiag;
262 // OpcUa_StatusCode when_status;
263  OpcUa_StatusCode val_status;
264  OpcUa_StatusCode status_status;
265 
269  try {
270  val_status = checkStatus(m_var);
271 // cout << "Main Block " << hex << val_status << endl << flush;
272  if (val_status != OpcUa_Good) {
273  sDiag = "Sensor is bad";
274  Status.setStatus(val_status, sDiag);
275 // cout << sDiag.toUtf8() << endl;
276  }
277  else {
278 
279  res = m_formul.value(); // calculation
280 // cout << "Result " << hex << res << endl;
281  if (m_sExists) {
282  status_status = checkStatus(m_statusVar);
283 // cout << "Status Status " << hex << status_status << endl << flush;
284  if (status_status != OpcUa_Good) {
285  sDiag = "Status sensor is bad";
286  Status.setStatus(status_status, sDiag);
287 // cout << sDiag.toUtf8() << endl;
288  }
289  else {
290  cond = m_statusFormula.value();
291 // cout << "Condition value " << cond << endl << flush;
292  if (!cond) {
293  sDiag = "Condition is false";
294  Status.setStatus(OpcUa_Bad, sDiag);
295 // cout << sDiag.toUtf8() << endl;
296  }
297  else {
298  sDiag = "OK";
299  Status.setStatus(OpcUa_Good, sDiag);
300 // cout << sDiag.toUtf8() << endl;
301  }
302  }
303  }
304  else {
305  sDiag = "OK";
306  Status.setStatus(OpcUa_Good, sDiag);
307 // cout << sDiag.toUtf8() << endl << flush;
308  }
309  }
310  }
311  catch (...)
312  {
313  sDiag = "Calculation Error";
314  Status.setStatus(OpcUa_BadUnexpectedError,sDiag);
315  }
316 
317  udt = dValue.sourceTimestamp();
318  sdt = UaDateTime::now();
319 
320  UaVariant val = res;
321  if (isnan(res) || isinf(res)) {
322  dataValue.setDataValue(val, OpcUa_False, OpcUa_BadUnexpectedError, udt, sdt);
323  }
324  else
325  dataValue.setDataValue(val, OpcUa_False, Status.code(), udt, sdt);
326 
327  Status = setValue(pSession, dataValue, checkAccessLevel);
328  }
329 
330  void UaCalcItem::initCalculation()
331  {
332  boost::arg<1> _1;
333  UaDataValue dataValue;
334  UaDateTime udt,sdt;
335  UaStatus Status = OpcUa_Good;
336 // double res;
337  UaVariant val;
338 
343 // m_formul->getValueFromItem = boost::bind(&AddressSpace::UaCalcItem::getValueFromItem,this,_1);
344  calculateOnChange(0, dataValue, OpcUa_False);
345 
346  //try {
347  // res = m_formul.Eval(); // calculation
348  // val = res;
349  // udt = UaDateTime::now();
350  // sdt = UaDateTime::now();
351 
352  // if (isnan(res) || isinf(res)) {
353  // UaString sDiag = "Calculation Error";
354  // Status.setStatus(OpcUa_BadUnexpectedError, sDiag);
355  // }
356 
357  // dataValue.setDataValue(val, OpcUa_False, Status.statusCode(), udt, sdt);
358 
359  // Status = setValue(0, dataValue, OpcUa_False);
360  //}
361  //catch (...)
362  //{
363  // UaString sDiag = "No Initial Data";
364  // Status.setStatus(OpcUa_BadWaitingForInitialData, sDiag);
365 
366  //}
367 
368  }
369 
370 
376  /*
377  double UaCalcItem::getValueFromItem(std::string ind)
378  {
379  double res = 0.0;
380  variableSet::iterator vit = m_var.find(ind);
381  if (vit != m_var.end() ) {
382  res = convertToDouble(vit);
383  }
384  return res;
385  }
386 
387  double UaCalcItem::getValueFromWhenItem(std::string ind)
388  {
389  double res = 0.0;
390  variableSet::iterator vit = m_whenVar.find(ind);
391  if (vit != m_whenVar.end() ) {
392  res = convertToDouble(vit);
393  }
394  return res;
395  }
396 
397  double UaCalcItem::getValueFromStatusItem(std::string ind)
398  {
399  double res = 0.0;
400  variableSet::iterator vit = m_statusVar.find(ind);
401  if (vit != m_statusVar.end() ) {
402  res = convertToDouble(vit);
403  }
404  return res;
405  }
406 
407  double UaCalcItem::convertToDouble(variableSet::iterator &vit)
408  {
409  UaDataValue dataValue;
410  double res = 0.0;
411  dataValue = (*vit).second->value(0);
412  const OpcUa_Variant *val = dataValue.value();
413  switch (val->Datatype) {
414  case OpcUaType_Boolean:
415  res = val->Value.Boolean;
416  break;
417  case OpcUaType_SByte:
418  res = val->Value.SByte;
419  break;
420  case OpcUaType_Byte:
421  res = val->Value.Byte;
422  break;
423  case OpcUaType_Int16:
424  res = val->Value.Int16;
425  break;
426  case OpcUaType_UInt16:
427  res = val->Value.UInt16;
428  break;
429  case OpcUaType_Int32:
430  res = val->Value.Int32;
431  break;
432  case OpcUaType_UInt32:
433  res = val->Value.UInt32;
434  break;
435  case OpcUaType_Int64:
436  res = (double)val->Value.Int64;
437  break;
438  case OpcUaType_UInt64:
439  res = (double)val->Value.UInt64;
440  break;
441  case OpcUaType_Float:
442  res = val->Value.Float;
443  break;
444  case OpcUaType_Double:
445  res = val->Value.Double;
446  break;
447  default:
448  throw 1;
449 // res = 0.0;
450  };
451  return res;
452  }
453  */
457  /*
458  UaDateTime UaCalcItem::UaGetSourceTimeStamp()
459  {
460  UaDateTime udt=0,tudt;
461  UaDataValue dataValue;
462  for(variableSet::iterator vit = m_var.begin(); vit != m_var.end(); vit++ ) {
463  dataValue = (*vit).second->value(0);
464  tudt = dataValue.sourceTimestamp();
465  if (tudt > udt ) udt = tudt;
466  }
467  return udt;
468  }
469  */
470  //UaStatus UaCalcCompilerItem::findLinkToUaVariable(UaCalcItem *pcn,NmBuildingAutomation *pNodeManager,OpcUaFormula *formul,OpcUa_Int index,pushUaVariable puv)
471  //{
472 
473  // UaStatus ret = OpcUa_Good;
474  // if (formul) {
475  // for (operands_type_set::iterator it = formul->opt.begin(); it != formul->opt.end(); it++)
476  // {
477  // UaVariable* t_var;
478  // string stdName;
479  // UaString sName;
480  // UaNodeId uaId;
481  // if ((*it).which() == 3) {
482  // stdName = boost::get<std::string>((*it));
483  // sName = stdName.c_str();
484  // if (sName.at(0) == UaChar('$')) {
485  // if (isVariableMap(stdName))
486  // {
487  // UaControlVariableSet setOfVariable = getMapDataVariable().at(stdName);
488  // (pcn->*puv)(stdName, setOfVariable[index]);
489  // }
490  // else
491  // {
492  // ret = OpcUa_Bad;
493  // LOG(Log::INF) << "The variable is not defined " << stdName.c_str();
494  // UA_ASSERT(ret.isGood());
495  // return ret;
496  // }
497 
498  // }
499  // else {
500  // uaId = pNodeManager->getNewNodeId(getParentCalc(), sName);
501  // t_var = pNodeManager->getInstanceDeclarationVariable( uaId);
502  // if (t_var) {
503  // (pcn->*puv)(stdName,(UaControlVariable *)t_var);
504  // }
505  // else {
506  // uaId = pNodeManager->getNewNodeId(NULL, sName);
507  // t_var = pNodeManager->getInstanceDeclarationVariable( uaId);
508  // if (t_var) {
509  // (pcn->*puv)(stdName,(UaControlVariable *)t_var);
510  // }
511  // else {
512  // ret = OpcUa_Bad;
513  // LOG(Log::INF) << "Could not find the item " << stdName.c_str();
514  // UA_ASSERT(ret.isGood());
515  // return ret;
516  // }
517  // }
518  // }
519  // }
520  // }
521  // }
522  // return ret;
523  //}
524 
525  //UaStatus UaCalcCompilerItem::CreateLinks(NmBuildingAutomation *pNodeManager)
526  //{
527  // UaStatus ret;
528  // for (unsigned int i = 0; i < m_vCalcItem.size(); i++)
529  // {
530  // UaCalcItem *uci = m_vCalcItem[i];
531  // OpcUaFormula *formul = getFormula();
532  // OpcUaFormula *whenCondition = getWhenCondition();
533  // OpcUaFormula *statusCondition = getStatusCondition();
534 
535  // pushUaVariable puv = &UaCalcItem::pushDataVariable;
536  // ret = findLinkToUaVariable(uci,pNodeManager,formul,i,puv);
537  // UA_ASSERT(ret.isGood());
538  // if (ret.isBad())
539  // exit(-1);
540 
541  // puv = &UaCalcItem::pushConditionVariable;
542  // ret = findLinkToUaVariable(uci,pNodeManager,whenCondition,i,puv);
543  // UA_ASSERT(ret.isGood());
544  // if (ret.isBad())
545  // exit(-1);
546  //
547  // puv = &UaCalcItem::pushStatusVariable;
548  // ret = findLinkToUaVariable(uci,pNodeManager,statusCondition,i,puv);
549  // UA_ASSERT(ret.isGood());
550  // if (ret.isBad())
551  // exit(-1);
552 
553  // }
554  // return ret;
555  //}
556  //
557  //void UaCalcCompilerItem::initCalculation()
558  //{
559  // for (unsigned int i = 0; i < m_vCalcItem.size(); i++)
560  // {
561  // UaCalcItem *uci = m_vCalcItem[i];
562  // uci->initCalculation();
563  // }
564  //}
565 
566  //UaStatus UaCalcCompiler::addCalcCompilerItem(UaCalcCompilerItem *ucci)
567  //{
568  // UaStatus ret = OpcUa_Good;
569 
570  // m_vCCI.push_back(ucci);
571  // return ret;
572  //}
573 
574 }
575