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


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33