colab.cpp
Go to the documentation of this file.
1 
6 //
7 // colab.cpp
8 //
9 // (c) 2010 SICK AG, Hamburg, Germany
10 #define _CRT_SECURE_NO_WARNINGS
11 #include "sick_scan/tcp/colab.hpp"
12 #include <cstring>
13 #include <cassert>
14 #include <stdexcept>
15 #include <stdlib.h>
16 #include <limits>
17 
18 namespace colab
19 {
20 
21 void addStringToBuffer(UINT8* buffer, UINT16& pos, const std::string& stringValue)
22 {
23  UINT16 length = (UINT16)stringValue.length();
24  strcpy((char*) &buffer[pos], stringValue.c_str());
25  pos += length;
26 }
27 
28 
29 
30 void addStringToBuffer(BYTE* buffer, const std::string& stringValue)
31 {
32 // UINT16 length = stringValue.length();
33  strcpy((char*) buffer, stringValue.c_str());
34 // buffer += length;
35 }
36 
37 
38 
39 std::string getStringFromBuffer(UINT8* buffer, UINT16& pos, UINT16 length)
40 {
41  UINT16 start = pos;
42  pos += length;
43  return std::string((char *) &buffer[start], length);
44 }
45 
46 
47 
48 std::string getStringFromBuffer(BYTE*& buffer, UINT16 length)
49 {
50  std::string str((char *) &buffer[0], length);
51  buffer += length;
52  return str;
53 }
54 
55 
56 std::string getCommandStringFromBuffer(UINT8* buffer)
57 {
58  return std::string((char*) &buffer[9], 2);
59 }
60 
61 std::string getIdentifierFromBuffer(UINT8* buffer, UINT16& nextData, UINT16 bufferLength)
62 {
63  UINT16 start;
64  UINT16 length;
65 
66  if (buffer[11] == 0x20)
67  {
68  start = 12;
69  }
70  else
71  {
72  start = 11;
73  }
74 
75  int i = start;
76  do
77  {
78  if (i == bufferLength - 2)
79  {
80  // found checksum field -> end of buffer reached.
81  nextData = 0; // indicates that there is no more data
82  break;
83  }
84  if (buffer[i] == 0x20)
85  {
86  // found identifier delimiter
87  nextData = i + 1; // points to next data field
88  break;
89  }
90  i++;
91  }
92  while (true);
93 
94  length = i - start; // last byte of identifier
95 
96  return std::string((char*) &buffer[start], length);
97 }
98 
99 void addFrameToBuffer(UINT8* sendBuffer, UINT8* cmdBuffer, UINT16* len)
100 {
101  UINT16 pos = 0;
102  UINT32 length = *len;
103 
104  // write header
105  sendBuffer[pos++] = 0x02;
106  sendBuffer[pos++] = 0x02;
107  sendBuffer[pos++] = 0x02;
108  sendBuffer[pos++] = 0x02;
109  // Write payload length to buffer
110  colab::addIntegerToBuffer<UINT32>(sendBuffer, pos, length + 1); // s counts to the payload length
111  sendBuffer[pos++] = 's';
112 
113  // write telegram
114  memcpy(&(sendBuffer[pos]), cmdBuffer, length);
115  pos += length;
116 
117  // write checksum (of payload)
118  UINT8 checksum = sendBuffer[8];
119  for (int i = 9; i < pos; i++)
120  {
121  checksum = checksum ^ sendBuffer[i]; // XOR
122  }
123  colab::addIntegerToBuffer<UINT8>(sendBuffer, pos, checksum);
124 
125  *len = pos;
126 }
127 
128 
129 double getDoubleFromBuffer(UINT8* buffer, UINT16& pos)
130 {
131  UINT16 width = sizeof(double); // 8
132 // UINT8* buffer2 = buffer;
133 // T floatValue = memread<T>(buffer2);
134  double* valuePtr = (double*)buffer;
135  double value = *valuePtr;
136 
137  pos += width;
138  return value;
139 }
140 
141 
143 {
144  UINT16 value = (((UINT16)buffer[0]) << 8) +
145  ((UINT16)buffer[1]);
146  return value;
147 }
148 
149 
150 } // END namespace colab
ROSCPP_DECL void start()
unsigned char BYTE
uint16_t UINT16
UINT16 decodeUINT16(BYTE *buffer)
Definition: colab.cpp:142
std::string getStringFromBuffer(UINT8 *buffer, UINT16 &pos, UINT16 length)
Definition: colab.cpp:39
uint32_t UINT32
void addStringToBuffer(UINT8 *buffer, UINT16 &pos, const std::string &stringValue)
Definition: colab.cpp:21
Definition: colab.cpp:18
void addFrameToBuffer(UINT8 *sendBuffer, UINT8 *cmdBuffer, UINT16 *len)
Definition: colab.cpp:99
std::string getCommandStringFromBuffer(UINT8 *buffer)
Definition: colab.cpp:56
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
std::string getIdentifierFromBuffer(UINT8 *buffer, UINT16 &nextData, UINT16 bufferLength)
Definition: colab.cpp:61
double getDoubleFromBuffer(UINT8 *buffer, UINT16 &pos)
Definition: colab.cpp:129
uint8_t UINT8


sick_scan
Author(s): Michael Lehning , Jochen Sprickerhof , Martin Günther
autogenerated on Wed Sep 7 2022 02:25:06