colaa.cpp
Go to the documentation of this file.
1 
5 //
6 // colaa.cpp
7 //
8 // (c) 2011 SICK AG, Hamburg, Germany
9 #define _CRT_SECURE_NO_WARNINGS
10 
11 #include "sick_scan/tcp/colaa.hpp"
12 #include <cstring>
13 #include <cassert>
14 #include <stdexcept>
15 #include "sick_scan/tcp/toolbox.hpp" // for "toString"
17 #include <stdlib.h>
18 #include <limits>
19 // #include "Trace.hpp"
20 
21 namespace colaa
22 {
23 
24 //
25 // Konvertiert ein ASCII-Zeichen (= Hex-Wert-Nibble oder Ziffer) in seinen Zahlenwert.
26 //
28 {
29  UINT16 value = 0;
30 
31  if ((c >= '0') && (c <= '9'))
32  {
33  value = c - '0';
34  }
35  else if ((c >= 'A') && (c <= 'F'))
36  {
37  value = 10 + c - 'A';
38  }
39  else
40  {
41  // Keine HEX-Ziffer
42  throw std::out_of_range("Unknown character where 0..9 or A..F was expected: '" + std::string(c, 1) + "'");
43  }
44 
45  return value;
46 }
47 
48 //
49 // Konvertiere ein Hex-Nibble (0..F) nach ASCII.
50 //
51 // Return value: UINT8 (= Char)
52 //
54 {
55  UINT8 c;
56 
57  if (value > 0x0f)
58  {
59  throw std::out_of_range("Tried to convert value > 0x0f into hex-nibble: " + toString((INT16)value));
60  }
61 
62  if (value < 10)
63  {
64  c = ('0' + value);
65  }
66  else
67  {
68  c = ('A' + (value - 10));
69  }
70 
71  return c;
72 }
73 
74 
82 void addFrameToBuffer(UINT8* sendBuffer, UINT8* cmdBuffer, UINT16* len)
83 {
84  UINT16 pos = 0;
85 
86  // Beginne mit dem 23-Frame-Header
87  sendBuffer[pos++] = 0x02;
88  sendBuffer[pos++] = 's';
89 
90  // Nun das Kommando
91  memcpy(&(sendBuffer[pos]), cmdBuffer, *len);
92  pos += *len;
93 
94 // for (UINT16 i = 0; i<(*len); i++)
95 // {
96 // sendBuffer[pos++] = cmdBuffer[i];
97 // }
98 
99  // Schliesse den 23-Frame ab
100  sendBuffer[pos++] = 0x03;
101 
102  // Fertig!
103  *len = pos;
104 }
112 {
113  UINT16 len;
114 
115  len = addUINT32ToBuffer(buffer, (UINT32)value);
116 
117  return len;
118 }
126 {
127  UINT16 len;
128 
129  len = addUINT32ToBuffer(buffer, (UINT32)value);
130 
131  return len;
132 }
133 
140 {
141  UINT16 stellenwert;
142  UINT8 c;
143  UINT16 pos = 0;
144  bool firstZero = true;
145 
146  // Vorzeichen
147  if (value < 0)
148  {
149  buffer[pos++] = '-';
150  value *= -1;
151  }
152  else
153  {
154  buffer[pos++] = '+';
155  }
156 
157  // Dezimal-Konversion
158  stellenwert = 100;
159 
160  while (stellenwert > 0)
161  {
162  c = value / stellenwert;
163  if ((c != 0) || (firstZero == false) || (stellenwert == 1))
164  {
165  // Ziffer schreiben
166  buffer[pos++] = ('0' + c);
167  }
168  if (c != 0)
169  {
170  // Wert != 0, d.h. ab jetzt auch jede 0 schreiben
171  firstZero = false;
172  }
173  value -= c * stellenwert;
174  stellenwert /= 10;
175  }
176 
177  return pos;
178 }
186 {
187  UINT32 uValue = (UINT32)value;
188 
189  UINT16 pos = addUINT32ToBuffer(buffer, uValue);
190 
191  return pos;
192 }
193 
201 {
202  // Alle Nibbles durchgehen
203  bool firstZero = true;
204  UINT16 pos = 0;
205  UINT8 nibble;
206 
207  for (INT16 i = 7; i >= 0; i -= 1)
208  {
209  nibble = (value >> (i * 4)) & 0x0F;
210  if ((nibble != 0) || (firstZero == false) || (i == 0))
211  {
212  buffer[pos++] = nibbleToAscii(nibble);
213  firstZero = false;
214  }
215  }
216 
217  return pos;
218 }
224 UINT16 addStringToBuffer(UINT8* buffer, const std::string& text)
225 {
226  UINT16 len = (UINT16)text.length();
227 
228  // Kopiere den String
229  strcpy ((char*)buffer, text.c_str());
230 
231  return len;
232 }
233 
234 UINT16 detail::writeToBuffer(BYTE* buffer, double value)
235 {
236  std::string tmp(toString(value, 6));
237  printWarning("detail::writeToBuffer: Warning - Writing of floating-point values has not been cross-checked in Cola-A format!");
238  return addStringToBuffer(buffer, tmp);
239 }
240 
246 std::string getNextStringToken(std::string* rxData)
247 {
248  typedef std::string::size_type size_type;
249 
250  // Token finden
251  size_type tokenStart = rxData->find_first_not_of(' '); // Fuehrende Spaces loeschen
252  size_type tokenEnd = rxData->find(' ', tokenStart); // Folgendes Space finden
253  size_type tokenLength = tokenEnd - tokenStart;
254 
255  // Token ausschneiden
256  std::string token = rxData->substr(tokenStart, tokenLength);
257 
258  // Eingangsstring kuerzen
259  *rxData = rxData->substr(tokenEnd + 1);
260 
261  // Rueckgabe des Token
262  return token;
263 }
264 
265 
271 std::string getNextStringToken(UINT8* rxData)
272 {
273 // typedef std::string::size_type size_type;
274 
275  UINT16 pos = 0;
276  std::string token;
277 
278  // Fuehrende Spaces entfernen
279  while (rxData[pos] != 0x00)
280  {
281  if (rxData[pos] != ' ')
282  {
283  break;
284  }
285  pos++;
286  }
287 
288  // Etwas Gueltiges gefunden?
289  if (rxData[pos] != 0x00)
290  {
291  // start zeigt auf etwas gueltiges.
292  // So lange weiter fuellen, bis das Ende erreicht ist.
293  while ((rxData[pos] != 0x00) && (rxData[pos] != ' '))
294  {
295  token += rxData[pos];
296  pos++;
297  }
298  }
299 
300  // Rueckgabe des Token
301  return token;
302 }
303 
304 
305 bool GetNibble(unsigned char data, unsigned char& rNibble)
306 {
307  rNibble = 0;
308  bool validHexData = true;
309  if ((data >= '0') && (data <= '9'))
310  {
311  rNibble = (unsigned char)(data - '0');
312  }
313  else if ((data >= 'A') && (data <= 'F'))
314  {
315  rNibble = (unsigned char)(data - 'A' + 10);
316  }
317  else if ((data >= 'a') && (data <= 'f'))
318  {
319  rNibble = (unsigned char)(data - 'a' + 10);
320  }
321  else
322  {
323  validHexData = false;
324  }
325  return validHexData;
326 }
327 
332 double decodeReal(std::string* rxData)
333 {
334  double value = std::numeric_limits<double>::quiet_NaN();
335  std::string text = colaa::getNextStringToken(rxData);
336  if (text.empty() == false)
337  {
338  // Check representation
339  if ((text[0] == '+') || (text[0] == '-'))
340  {
341  // ASCII
342  value = atof(text.c_str());
343  }
344  else
345  {
346  // HEX
347  // This simple conversion works only for fixed size!
348  union
349  {
350  float f;
351  unsigned char c[4];
352  } converter;
353  memset(&converter, 0, sizeof(converter));
354 
355  if (text.length() == 8)
356  {
357  int hexIndex = 0;
358  int shift = 0;
359  bool success = true;
360  for (int i = 7; i >= 0; --i)
361  {
362  unsigned char nibble;
363  success &= GetNibble(text[i], nibble);
364  converter.c[hexIndex] |= (nibble << shift);
365  hexIndex += (shift >> 2);
366  shift ^= 4;
367  }
368  if (success == true)
369  {
370  value = converter.f;
371  }
372  }
373  }
374  }
375 
376  return value;
377 }
378 
383 double decodeReal(const std::string& rxData)
384 {
385  double value = std::numeric_limits<double>::quiet_NaN();
386  const std::string& text = rxData;
387  if (text.empty() == false)
388  {
389  // Check representation
390  if ((text[0] == '+') || (text[0] == '-'))
391  {
392  // ASCII
393  value = atof(text.c_str());
394  }
395  else
396  {
397  // HEX
398  // TODO: Simple conversion working only for fixed size
399  union
400  {
401  float f;
402  unsigned char c[4];
403  } converter;
404  memset(&converter, 0, sizeof(converter));
405 
406  if (text.length() == 8)
407  {
408  int hexIndex = 0;
409  int shift = 0;
410  bool success = true;
411  for (int i = 7; i >= 0; --i)
412  {
413  unsigned char nibble;
414  success &= GetNibble(text[i], nibble);
415  converter.c[hexIndex] |= (nibble << shift);
416  hexIndex += (shift >> 2);
417  shift ^= 4;
418  }
419  if (success == true)
420  {
421  value = converter.f;
422  }
423  }
424  }
425  }
426 
427  return value;
428 }
429 
430 
434 UINT32 decodeUINT32(std::string* rxData)
435 {
436  UINT32 value = 0;
437  UINT32 tempVal;
438  UINT32 factor = 1;
439  UINT32 baseFactor = 10;
440  UINT16 digits;
441 
442  // Zahlen-String extrahieren
443  std::string number = colaa::getNextStringToken(rxData);
444 
445  if (number.at(0) == '+')
446  {
447  // Dezimalzahl
448  baseFactor = 10;
449  number = number.substr(1);
450  }
451  else
452  {
453  // Hexadezimalzahl
454  baseFactor = 16;
455  }
456  digits = (UINT16)number.length();
457 
458  // Extrahiere die Zahl, letzte Stelle zuerst
459  for (INT16 d = digits - 1; d >= 0; d -= 1)
460  {
461  tempVal = colaa::getValueOfChar(number.at(d));
462  value += tempVal * factor;
463  factor *= baseFactor;
464  }
465 
466  return value;
467 }
468 
472 INT16 decodeINT16(std::string* rxData)
473 {
474  INT32 value = decodeINT32(rxData);
475  assert ((value >= -32768) && (value <= 32767));
476  return (INT16)value;
477 }
478 
483 INT32 decodeINT32(std::string* rxData)
484 {
485  INT32 value = 0;
486  INT32 tempVal;
487  INT32 factor = 1;
488  INT32 baseFactor = 10; // 10 = dez, 16 = hex
489  INT32 sign = 1; // 1 oder -1
490  UINT16 digits;
491 
492  // Zahlen-String extrahieren
493  std::string number = colaa::getNextStringToken(rxData);
494 
495  // Unterscheidung Pos/Neg/Hex
496  if (number.at(0) == '+')
497  {
498  // pos. Dezimalzahl
499  number = number.substr(1); // Vorzeichen abschneiden
500  }
501  else if (number.at(0) == '-')
502  {
503  // neg. Dezimalzahl
504  sign = -1; // Neg. Vorzeichen
505  number = number.substr(1); // Vorzeichen abschneiden
506  }
507  else
508  {
509  // Hexadezimalzahl
510  baseFactor = 16; // Hex.
511  }
512 
513  // Anzahl Ziffern
514  digits = (UINT16)number.length();
515 
516  // Extrahiere die Zahl, letzte Stelle zuerst
517  for (INT16 d = digits - 1; d >= 0; d -= 1)
518  {
519  tempVal = colaa::getValueOfChar(number.at(d));
520  value += tempVal * factor;
521  factor *= baseFactor;
522  }
523 
524  // Vorzeichen einbauen
525  value *= sign;
526 
527  return value;
528 }
529 
530 
534 INT16 decodeINT16(const std::string& rxData)
535 {
536  INT32 value = decodeINT32(rxData);
537  assert ((value >= -32768) && (value <= 32767));
538  return (INT16)value;
539 }
540 
545 INT32 decodeINT32(const std::string& rxData)
546 {
547  INT32 value = 0;
548  INT32 tempVal;
549  INT32 factor = 1;
550  INT32 baseFactor = 10; // 10 = dez, 16 = hex
551  INT32 sign = 1; // 1 oder -1
552  UINT16 digits;
553  UINT16 offset = 0;
554 
555  // Zahlen-String extrahieren
556  const std::string number = rxData;
557 
558  // Unterscheidung Pos/Neg/Hex
559  if (number.at(0) == '+')
560  {
561  // pos. Dezimalzahl
562 // number = number.substr(1); // Vorzeichen abschneiden
563  offset = 1;
564  }
565  else if (number.at(0) == '-')
566  {
567  // neg. Dezimalzahl
568  sign = -1; // Neg. Vorzeichen
569 // number = number.substr(1); // Vorzeichen abschneiden
570  offset = 1;
571  }
572  else
573  {
574  // Hexadezimalzahl
575  baseFactor = 16; // Hex.
576  }
577 
578  // Anzahl Ziffern
579  digits = (UINT16)number.length();
580 
581  // Extrahiere die Zahl, letzte Stelle zuerst
582  for (INT16 d = digits - 1; d >= offset; d -= 1)
583  {
584  tempVal = colaa::getValueOfChar(number.at(d));
585  value += tempVal * factor;
586  factor *= baseFactor;
587  }
588 
589  // Vorzeichen einbauen
590  value *= sign;
591 
592  return value;
593 }
594 
595 
603 std::string decodeString(std::string* rxData, UINT16 len)
604 {
605  std::string text;
606 
607  // String extrahieren
608  if (len == 0)
609  {
610  // Keine spezielle Laenge gewuenscht.
611  text = colaa::getNextStringToken(rxData);
612  }
613  else
614  {
615  // String bekannter Laenge ausschneiden
616  text = rxData->substr(0, len);
617  // Eingangsstring kuerzen
618  *rxData = rxData->substr(len + 1);
619  }
620 
621  return text;
622 }
623 
624 //
625 // Lese einen UINT16-Wert aus dem Empfangspuffer.
626 //
628 {
629  std::string data = getNextStringToken(buffer);
630  return decodeUINT16(data);
631 }
632 
633 //
634 // Lese einen UINT16-Wert aus dem Empfangspuffer.
635 //
636 UINT16 decodeUINT16(std::string* rxData)
637 {
638  UINT32 value;
639 
640  value = decodeUINT32(rxData);
641 
642  assert (value < 0x10000);
643  return (UINT16)value;
644 }
645 
649 UINT8 decodeUINT8(std::string* rxData)
650 {
651  UINT32 value;
652 
653  value = decodeUINT32(rxData);
654 
655  assert (value < 256);
656  return (UINT8)value;
657 }
658 
662 UINT16 decodeUINT16(const std::string& rxData)
663 {
664  UINT32 value;
665 
666  value = decodeUINT32(rxData);
667 
668  assert (value < 0x10000);
669  return (UINT16)value;
670 }
671 
675 UINT8 decodeUINT8(const std::string& rxData)
676 {
677  UINT32 value;
678 
679  value = decodeUINT32(rxData);
680 
681  assert (value < 256);
682  return (UINT8)value;
683 }
684 
690 UINT32 decodeUINT32(const std::string& rxData)
691 {
692  UINT32 value = 0;
693  UINT32 factor = 1;
694  UINT32 tempVal;
695  UINT32 baseFactor = 10; // 10 = dez, 16 = hex
696  UINT16 digits;
697  UINT16 offset = 0;
698 
699  // Zahlen-String extrahieren
700  const std::string& number = rxData;
701 
702  // Unterscheidung Pos/Neg/Hex
703  if (number.at(0) == '+')
704  {
705  // pos. Dezimalzahl
706 // number = number.substr(1); // Vorzeichen abschneiden
707  offset = 1;
708  }
709  else
710  {
711  // Hexadezimalzahl
712  baseFactor = 16; // Hex.
713  }
714 
715  // Anzahl Ziffern
716  digits = (UINT16)number.length();
717 
718  // Extrahiere die Zahl, letzte Stelle zuerst
719  for (INT16 d = digits - 1; d >= offset; d -= 1)
720  {
721  tempVal = colaa::getValueOfChar(number.at(d));
722  value += tempVal * factor;
723  factor *= baseFactor;
724  }
725 
726  return value;
727 }
728 
735 UINT32 decodeXByte(std::string* rxData, UINT16 len)
736 {
737  UINT32 value;
738  UINT32 result = 0;
739 
740  assert (len < 5); // Wir koennen nur bis zu 4 Bytes in einen UINT32 packen
741 
742  for (UINT16 i = 0; i < len; i++)
743  {
744  value = decodeUINT32(rxData);
745  assert (value < 256);
746  result += (value << (i * 8));
747  }
748 
749  return result;
750 }
751 
763 /*
764 UINT32 decodeXByte(tokenizer::const_iterator& tok, const tokenizer::const_iterator& end, UINT16 len)
765 {
766  UINT32 value;
767  UINT32 result = 0;
768 
769  assert (len < 5); // Wir koennen nur bis zu 4 Bytes in einen UINT32 packen
770 
771  tokenizer::const_iterator last = tok;
772 
773  for ( UINT16 i = 0; i < len && tok != end; ++tok, ++i)
774  {
775  value = decodeUINT32(*tok);
776  assert (value < 256);
777  result += (value << (i * 8));
778 
779  last = tok;
780  }
781 
782  // set token one position back
783  tok = last;
784 
785 
786  return result;
787 }
788 */
789 
790 //
791 // Char-To-String-Umwandlung.
792 //
793 std::string convertRxBufferToString(UINT8* buffer, UINT16 bufferLen)
794 {
795  buffer[bufferLen-1] = 0x00; // End-Code 0x03 ueberschreiben mit 0x00
796  std::string result = std::string((char*)(&(buffer[2])));
797  return result;
798 }
799 
800 
801 } // END namespace colaa
UINT16
uint16_t UINT16
Definition: BasicDatatypes.hpp:27
UINT8
uint8_t UINT8
Definition: BasicDatatypes.hpp:29
colaa::decodeUINT8
UINT8 decodeUINT8(std::string *rxData)
Definition: colaa.cpp:649
errorhandler.hpp
colaa::GetNibble
bool GetNibble(unsigned char data, unsigned char &rNibble)
Definition: colaa.cpp:305
INT16
int16_t INT16
Definition: BasicDatatypes.hpp:28
BYTE
unsigned char BYTE
INT8
int8_t INT8
Definition: BasicDatatypes.hpp:30
colaa::getNextStringToken
std::string getNextStringToken(std::string *rxData)
Definition: colaa.cpp:246
colaa::addUINT8ToBuffer
UINT16 addUINT8ToBuffer(UINT8 *buffer, UINT8 value)
Definition: colaa.cpp:111
toString
std::string toString(INT32 value)
Definition: toolbox.cpp:279
colaa
Definition: colaa.cpp:21
data
data
f
f
colaa::convertRxBufferToString
std::string convertRxBufferToString(UINT8 *buffer, UINT16 bufferLen)
Definition: colaa.cpp:793
INT32
int32_t INT32
Definition: BasicDatatypes.hpp:25
colaa::decodeXByte
UINT32 decodeXByte(std::string *rxData, UINT16 len)
Definition: colaa.cpp:735
colaa::addINT8ToBuffer
UINT16 addINT8ToBuffer(UINT8 *buffer, INT8 value)
Definition: colaa.cpp:139
printWarning
void printWarning(std::string message)
Definition: errorhandler.cpp:73
colaa::addUINT16ToBuffer
UINT16 addUINT16ToBuffer(UINT8 *buffer, UINT16 value)
Definition: colaa.cpp:125
colaa::addINT32ToBuffer
UINT16 addINT32ToBuffer(UINT8 *buffer, INT32 value)
Definition: colaa.cpp:185
toolbox.hpp
colaa::decodeReal
double decodeReal(std::string *rxData)
Definition: colaa.cpp:332
d
d
colaa::decodeUINT16
UINT16 decodeUINT16(BYTE *buffer)
Definition: colaa.cpp:627
colaa::decodeINT32
INT32 decodeINT32(std::string *rxData)
Definition: colaa.cpp:483
colaa::decodeString
std::string decodeString(std::string *rxData, UINT16 len)
Definition: colaa.cpp:603
colaa::decodeINT16
INT16 decodeINT16(std::string *rxData)
Definition: colaa.cpp:472
colaa::addUINT32ToBuffer
UINT16 addUINT32ToBuffer(UINT8 *buffer, UINT32 value)
Definition: colaa.cpp:200
colaa::addStringToBuffer
UINT16 addStringToBuffer(UINT8 *buffer, const std::string &text)
Definition: colaa.cpp:224
colaa.hpp
colaa::decodeUINT32
UINT32 decodeUINT32(std::string *rxData)
Definition: colaa.cpp:434
UINT32
uint32_t UINT32
Definition: BasicDatatypes.hpp:26
colaa::detail::writeToBuffer
UINT16 writeToBuffer(BYTE *buffer, double value)
Definition: colaa.cpp:234
colaa::addFrameToBuffer
void addFrameToBuffer(UINT8 *sendBuffer, UINT8 *cmdBuffer, UINT16 *len)
Definition: colaa.cpp:82
colaa::getValueOfChar
UINT16 getValueOfChar(UINT8 c)
Definition: colaa.cpp:27
colaa::nibbleToAscii
UINT8 nibbleToAscii(UINT8 value)
Definition: colaa.cpp:53


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Thu Sep 8 2022 02:30:19