colab.cpp
Go to the documentation of this file.
00001 //
00002 // colab.cpp
00003 //
00004 // (c) 2010 SICK AG, Hamburg, Germany
00005 
00006 #include "colab.hpp"
00007 #include <cstring>
00008 #include <cassert>
00009 #include <stdexcept>
00010 #include <stdlib.h>
00011 #include <limits>
00012 
00013 namespace colab
00014 {
00015 
00016 void addStringToBuffer(UINT8* buffer, UINT16& pos, const std::string& stringValue)
00017 {
00018         UINT16 length = stringValue.length();
00019         strcpy((char*) &buffer[pos], stringValue.c_str());
00020         pos += length;
00021 }
00022 
00023 
00024 
00025 void addStringToBuffer(BYTE* buffer, const std::string& stringValue)
00026 {
00027 //      UINT16 length = stringValue.length();
00028         strcpy((char*) buffer, stringValue.c_str());
00029 //      buffer += length;
00030 }
00031 
00032 
00033 
00034 std::string getStringFromBuffer(UINT8* buffer, UINT16& pos, UINT16 length)
00035 {
00036         UINT16 start = pos;
00037         pos += length;
00038         return std::string((char *) &buffer[start], length);
00039 }
00040 
00041 
00042 
00043 std::string getStringFromBuffer(BYTE*& buffer, UINT16 length)
00044 {
00045         std::string str((char *) &buffer[0], length);
00046         buffer += length;
00047         return str;
00048 }
00049 
00050 
00051 
00052 std::string getCommandStringFromBuffer(UINT8* buffer)
00053 {
00054         return std::string((char*) &buffer[9], 2);
00055 }
00056 
00057 
00058 
00059 std::string getIdentifierFromBuffer(UINT8* buffer, UINT16& nextData, UINT16 bufferLength)
00060 {
00061         UINT16 start;
00062         UINT16 length;
00063 
00064         if (buffer[11] == 0x20)
00065         {
00066                 start = 12;
00067         }
00068         else
00069         {
00070                 start = 11;
00071         }
00072 
00073         int i = start;
00074         do
00075         {
00076                 if (i == bufferLength - 2)
00077                 {
00078                         // found checksum field -> end of buffer reached.
00079                         nextData = 0; // indicates that there is no more data
00080                         break;
00081                 }
00082                 if (buffer[i] == 0x20)
00083                 {
00084                         // found identifier delimiter
00085                         nextData = i + 1; // points to next data field
00086                         break;
00087                 }
00088                 i++;
00089         }
00090         while (true);
00091 
00092         length = i - start; // last byte of identifier
00093 
00094         return std::string((char*) &buffer[start], length);
00095 }
00096 
00097 
00098 
00099 void addFrameToBuffer(UINT8* sendBuffer, UINT8* cmdBuffer, UINT16* len)
00100 {
00101         UINT16 pos = 0;
00102         UINT32 length = *len;
00103 
00104         // write header
00105         sendBuffer[pos++] = 0x02;
00106         sendBuffer[pos++] = 0x02;
00107         sendBuffer[pos++] = 0x02;
00108         sendBuffer[pos++] = 0x02;
00109         // Write payload length to buffer
00110         colab::addIntegerToBuffer<UINT32>(sendBuffer, pos, length + 1); // s counts to the payload length
00111         sendBuffer[pos++] = 's';
00112 
00113         // write telegram
00114         memcpy(&(sendBuffer[pos]), cmdBuffer, length);
00115         pos += length;
00116 
00117         // write checksum (of payload)
00118         UINT8 checksum = sendBuffer[8];
00119         for (int i = 9; i < pos; i++)
00120         {
00121                 checksum = checksum ^ sendBuffer[i]; // XOR
00122         }
00123         colab::addIntegerToBuffer<UINT8>(sendBuffer, pos, checksum);
00124 
00125         *len = pos;
00126 }
00127 
00128 //
00129 //  Returns the requested value. pos points then to the first byte of the next data field.
00130 //
00131 double getDoubleFromBuffer(UINT8* buffer, UINT16& pos)
00132 {
00133         UINT16 width = sizeof(double);  // 8
00134 //      UINT8* buffer2 = buffer;
00135 //      T floatValue = memread<T>(buffer2);
00136         double* valuePtr = (double*)buffer;
00137         double value = *valuePtr;
00138         
00139         pos += width;
00140         return value;
00141 }
00142 
00143 
00144 UINT16 decodeUINT16(BYTE* buffer)
00145 {
00146         UINT16 value = (((UINT16)buffer[0]) << 8) +
00147                                         ((UINT16)buffer[1]);
00148         return value;
00149 }
00150 
00151 
00152 } // END namespace colab


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50