colaa.cpp
Go to the documentation of this file.
00001 //
00002 // colaa.cpp
00003 //
00004 // (c) 2011 SICK AG, Hamburg, Germany
00005 
00006 #include "colaa.hpp"
00007 #include <cstring>
00008 #include <cassert>
00009 #include <stdexcept>
00010 #include "../tools/toolbox.hpp" // for "toString"
00011 #include "../tools/errorhandler.hpp"
00012 #include <stdlib.h>
00013 #include <limits>
00014 // #include "Trace.hpp"
00015 
00016 namespace colaa
00017 {
00018 
00019 //
00020 // Konvertiert ein ASCII-Zeichen (= Hex-Wert-Nibble oder Ziffer) in seinen Zahlenwert.
00021 //
00022 UINT16 getValueOfChar(UINT8 c)
00023 {
00024         UINT16 value = 0;
00025 
00026         if ((c >= '0') && (c <= '9'))
00027         {
00028                 value = c - '0';
00029         }
00030         else if ((c >= 'A') && (c <= 'F'))
00031         {
00032                 value = 10 + c - 'A';
00033         }
00034         else
00035         {
00036                 // Keine HEX-Ziffer
00037                 throw std::out_of_range("Unknown character where 0..9 or A..F was expected: '" + std::string(c, 1) + "'");
00038         }
00039 
00040         return value;
00041 }
00042 
00043 //
00044 // Konvertiere ein Hex-Nibble (0..F) nach ASCII.
00045 //
00046 // Return value: UINT8 (= Char)
00047 //
00048 UINT8 nibbleToAscii(UINT8 value)
00049 {
00050         UINT8 c;
00051 
00052         if (value > 0x0f)
00053         {
00054                 throw std::out_of_range("Tried to convert value > 0x0f into hex-nibble: " + toString((INT16)value));
00055         }
00056 
00057         if (value < 10)
00058         {
00059                 c = ('0' + value);
00060         }
00061         else
00062         {
00063                 c = ('A' + (value - 10));
00064         }
00065 
00066         return c;
00067 }
00068 
00069 
00077 void addFrameToBuffer(UINT8* sendBuffer, UINT8* cmdBuffer, UINT16* len)
00078 {
00079         UINT16 pos = 0;
00080 
00081         // Beginne mit dem 23-Frame-Header
00082         sendBuffer[pos++] = 0x02;
00083         sendBuffer[pos++] = 's';
00084 
00085         // Nun das Kommando
00086         memcpy(&(sendBuffer[pos]), cmdBuffer, *len);
00087         pos += *len;
00088 
00089 //      for (UINT16 i = 0; i<(*len); i++)
00090 //      {
00091 //              sendBuffer[pos++] = cmdBuffer[i];
00092 //      }
00093 
00094         // Schliesse den 23-Frame ab
00095         sendBuffer[pos++] = 0x03;
00096 
00097         // Fertig!
00098         *len = pos;
00099 }
00106 UINT16 addUINT8ToBuffer(UINT8* buffer, UINT8 value)
00107 {
00108         UINT16 len;
00109 
00110         len = addUINT32ToBuffer(buffer, (UINT32)value);
00111 
00112         return len;
00113 }
00120 UINT16 addUINT16ToBuffer(UINT8* buffer, UINT16 value)
00121 {
00122         UINT16 len;
00123 
00124         len = addUINT32ToBuffer(buffer, (UINT32)value);
00125 
00126         return len;
00127 }
00128 
00134 UINT16 addINT8ToBuffer(UINT8* buffer, INT8 value)
00135 {
00136         UINT16 stellenwert;
00137         UINT8 c;
00138         UINT16 pos = 0;
00139         bool firstZero = true;
00140 
00141         // Vorzeichen
00142         if (value < 0)
00143         {
00144                 buffer[pos++] = '-';
00145                 value *= -1;
00146         }
00147         else
00148         {
00149                 buffer[pos++] = '+';
00150         }
00151 
00152         // Dezimal-Konversion
00153         stellenwert = 100;
00154 
00155         while (stellenwert > 0)
00156         {
00157                 c = value / stellenwert;
00158                 if ((c != 0) || (firstZero == false) || (stellenwert == 1))
00159                 {
00160                         // Ziffer schreiben
00161                         buffer[pos++] = ('0' + c);
00162                 }
00163                 if (c != 0)
00164                 {
00165                         // Wert != 0, d.h. ab jetzt auch jede 0 schreiben
00166                         firstZero = false;
00167                 }
00168                 value -= c * stellenwert;
00169                 stellenwert /= 10;
00170         }
00171 
00172         return pos;
00173 }
00180 UINT16 addINT32ToBuffer(UINT8* buffer, INT32 value)
00181 {
00182         UINT32 uValue = (UINT32)value;
00183 
00184         UINT16 pos = addUINT32ToBuffer(buffer, uValue);
00185 
00186         return pos;
00187 }
00188 
00195 UINT16 addUINT32ToBuffer(UINT8* buffer, UINT32 value)
00196 {
00197         // Alle Nibbles durchgehen
00198         bool firstZero = true;
00199         UINT16 pos = 0;
00200         UINT8 nibble;
00201 
00202         for (INT16 i = 7; i >= 0; i -= 1)
00203         {
00204                 nibble = (value >> (i * 4)) & 0x0F;
00205                 if ((nibble != 0) || (firstZero == false) || (i == 0))
00206                 {
00207                         buffer[pos++] = nibbleToAscii(nibble);
00208                         firstZero = false;
00209                 }
00210         }
00211 
00212         return pos;
00213 }
00219 UINT16 addStringToBuffer(UINT8* buffer, const std::string& text)
00220 {
00221         UINT16 len = text.length();
00222 
00223         // Kopiere den String
00224         strcpy ((char*)buffer, text.c_str());
00225 
00226         return len;
00227 }
00228 
00229 UINT16 detail::writeToBuffer(BYTE* buffer, double value)
00230 {
00231         std::string tmp(toString(value, 6));
00232         printWarning("detail::writeToBuffer: Warning - Writing of floating-point values has not been cross-checked in Cola-A format!");
00233         return addStringToBuffer(buffer, tmp);
00234 }
00235 
00241 std::string getNextStringToken(std::string* rxData)
00242 {
00243         typedef std::string::size_type size_type;
00244 
00245         // Token finden
00246         size_type tokenStart = rxData->find_first_not_of(' ');  // Fuehrende Spaces loeschen
00247         size_type tokenEnd = rxData->find(' ', tokenStart);             // Folgendes Space finden
00248         size_type tokenLength = tokenEnd - tokenStart;
00249 
00250         // Token ausschneiden
00251         std::string token = rxData->substr(tokenStart, tokenLength);
00252 
00253         // Eingangsstring kuerzen
00254         *rxData = rxData->substr(tokenEnd + 1);
00255 
00256         // Rueckgabe des Token
00257         return token;
00258 }
00259 
00260 
00266 std::string getNextStringToken(UINT8* rxData)
00267 {
00268 //      typedef std::string::size_type size_type;
00269         
00270         UINT16 pos = 0;
00271         std::string token;
00272         
00273         // Fuehrende Spaces entfernen
00274         while (rxData[pos] != 0x00)
00275         {
00276                 if (rxData[pos] != ' ')
00277                 {
00278                         break;
00279                 }
00280                 pos++;
00281         }
00282         
00283         // Etwas Gueltiges gefunden?
00284         if (rxData[pos] != 0x00)
00285         {
00286                 // start zeigt auf etwas gueltiges.
00287                 // So lange weiter fuellen, bis das Ende erreicht ist.
00288                 while ((rxData[pos] != 0x00) && (rxData[pos] != ' '))
00289                 {
00290                         token += rxData[pos];
00291                         pos++;
00292                 }
00293         }
00294 
00295         // Rueckgabe des Token
00296         return token;
00297 }
00298 
00299 
00300 bool GetNibble(unsigned char data, unsigned char& rNibble)
00301 {
00302         rNibble = 0;
00303         bool validHexData = true;
00304         if ((data >= '0') && (data <= '9'))
00305         {
00306                 rNibble = (unsigned char)(data - '0');
00307         }
00308         else if ((data >= 'A') && (data <= 'F'))
00309         {
00310                 rNibble = (unsigned char)(data - 'A' + 10);
00311         }
00312         else if ((data >= 'a') && (data <= 'f'))
00313         {
00314                 rNibble = (unsigned char)(data - 'a' + 10);
00315         }
00316         else
00317         {
00318                 validHexData = false;
00319         }
00320         return validHexData;
00321 }
00322 
00327 double decodeReal(std::string* rxData)
00328 {
00329         double value = std::numeric_limits<double>::quiet_NaN();
00330         std::string text = colaa::getNextStringToken(rxData);
00331         if (text.empty() == false)
00332         {
00333                 // Check representation
00334                 if ((text[0] == '+') || (text[0] == '-'))
00335                 {
00336                         // ASCII
00337                         value = atof(text.c_str());
00338                 }
00339                 else
00340                 {
00341                         // HEX
00342                         // This simple conversion works only for fixed size!
00343                         union
00344                         {
00345                                 float f;
00346                                 unsigned char c[4];
00347                         } converter;
00348                         memset(&converter, 0, sizeof(converter));
00349 
00350                         if (text.length() == 8)
00351                         {
00352                                 int hexIndex = 0;
00353                                 int shift = 0;
00354                                 bool success = true;
00355                                 for (int i = 7; i >= 0; --i)
00356                                 {
00357                                         unsigned char nibble;
00358                                         success &= GetNibble(text[i], nibble);
00359                                         converter.c[hexIndex] |= (nibble << shift);
00360                                         hexIndex += (shift >> 2);
00361                                         shift ^= 4;
00362                                 }
00363                                 if (success == true)
00364                                 {
00365                                         value = converter.f;
00366                                 }
00367                         }
00368                 }
00369         }
00370 
00371         return value;
00372 }
00373 
00378 double decodeReal(const std::string& rxData)
00379 {
00380         double value = std::numeric_limits<double>::quiet_NaN();
00381         const std::string& text = rxData;
00382         if (text.empty() == false)
00383         {
00384                 // Check representation
00385                 if ((text[0] == '+') || (text[0] == '-'))
00386                 {
00387                         // ASCII
00388                         value = atof(text.c_str());
00389                 }
00390                 else
00391                 {
00392                         // HEX
00393                         // TODO: Simple conversion working only for fixed size
00394                         union
00395                         {
00396                                 float f;
00397                                 unsigned char c[4];
00398                         } converter;
00399                         memset(&converter, 0, sizeof(converter));
00400 
00401                         if (text.length() == 8)
00402                         {
00403                                 int hexIndex = 0;
00404                                 int shift = 0;
00405                                 bool success = true;
00406                                 for (int i = 7; i >= 0; --i)
00407                                 {
00408                                         unsigned char nibble;
00409                                         success &= GetNibble(text[i], nibble);
00410                                         converter.c[hexIndex] |= (nibble << shift);
00411                                         hexIndex += (shift >> 2);
00412                                         shift ^= 4;
00413                                 }
00414                                 if (success == true)
00415                                 {
00416                                         value = converter.f;
00417                                 }
00418                         }
00419                 }
00420         }
00421 
00422         return value;
00423 }
00424 
00425 
00429 UINT32 decodeUINT32(std::string* rxData)
00430 {
00431         UINT32 value = 0;
00432         UINT32 tempVal;
00433         UINT32 factor = 1;
00434         UINT32 baseFactor = 10;
00435         UINT16 digits;
00436 
00437         // Zahlen-String extrahieren
00438         std::string number = colaa::getNextStringToken(rxData);
00439 
00440         if (number.at(0) == '+')
00441         {
00442                 // Dezimalzahl
00443                 baseFactor = 10;
00444                 number = number.substr(1);
00445         }
00446         else
00447         {
00448                 // Hexadezimalzahl
00449                 baseFactor = 16;
00450         }
00451         digits = number.length();
00452 
00453         // Extrahiere die Zahl, letzte Stelle zuerst
00454         for (INT16 d = digits - 1; d >= 0; d -= 1)
00455         {
00456                 tempVal = colaa::getValueOfChar(number.at(d));
00457                 value += tempVal * factor;
00458                 factor *= baseFactor;
00459         }
00460 
00461         return value;
00462 }
00463 
00467 INT16 decodeINT16(std::string* rxData)
00468 {
00469         INT32 value = decodeINT32(rxData);
00470         assert ((value >= -32768) && (value <= 32767));
00471         return (INT16)value;
00472 }
00473 
00478 INT32 decodeINT32(std::string* rxData)
00479 {
00480         INT32 value = 0;
00481         INT32 tempVal;
00482         INT32 factor = 1;
00483         INT32 baseFactor = 10;  // 10 = dez, 16 = hex
00484         INT32 sign = 1;         // 1 oder -1
00485         UINT16 digits;
00486 
00487         // Zahlen-String extrahieren
00488         std::string number = colaa::getNextStringToken(rxData);
00489 
00490         // Unterscheidung Pos/Neg/Hex
00491         if (number.at(0) == '+')
00492         {
00493                 // pos. Dezimalzahl
00494                 number = number.substr(1);      // Vorzeichen abschneiden
00495         }
00496         else if (number.at(0) == '-')
00497         {
00498                 // neg. Dezimalzahl
00499                 sign = -1;                                      // Neg. Vorzeichen
00500                 number = number.substr(1);      // Vorzeichen abschneiden
00501         }
00502         else
00503         {
00504                 // Hexadezimalzahl
00505                 baseFactor = 16;                        // Hex.
00506         }
00507 
00508         // Anzahl Ziffern
00509         digits = number.length();
00510 
00511         // Extrahiere die Zahl, letzte Stelle zuerst
00512         for (INT16 d = digits - 1; d >= 0; d -= 1)
00513         {
00514                 tempVal = colaa::getValueOfChar(number.at(d));
00515                 value += tempVal * factor;
00516                 factor *= baseFactor;
00517         }
00518 
00519         // Vorzeichen einbauen
00520         value *= sign;
00521 
00522         return value;
00523 }
00524 
00525 
00529 INT16 decodeINT16(const std::string& rxData)
00530 {
00531         INT32 value = decodeINT32(rxData);
00532         assert ((value >= -32768) && (value <= 32767));
00533         return (INT16)value;
00534 }
00535 
00540 INT32 decodeINT32(const std::string& rxData)
00541 {
00542         INT32 value = 0;
00543         INT32 tempVal;
00544         INT32 factor = 1;
00545         INT32 baseFactor = 10;  // 10 = dez, 16 = hex
00546         INT32 sign = 1;         // 1 oder -1
00547         UINT16 digits;
00548         UINT16 offset = 0;
00549 
00550         // Zahlen-String extrahieren
00551         const std::string number = rxData;
00552 
00553         // Unterscheidung Pos/Neg/Hex
00554         if (number.at(0) == '+')
00555         {
00556                 // pos. Dezimalzahl
00557 //              number = number.substr(1);      // Vorzeichen abschneiden
00558                 offset = 1;
00559         }
00560         else if (number.at(0) == '-')
00561         {
00562                 // neg. Dezimalzahl
00563                 sign = -1;                                      // Neg. Vorzeichen
00564 //              number = number.substr(1);      // Vorzeichen abschneiden
00565                 offset = 1;
00566         }
00567         else
00568         {
00569                 // Hexadezimalzahl
00570                 baseFactor = 16;                        // Hex.
00571         }
00572 
00573         // Anzahl Ziffern
00574         digits = number.length();
00575 
00576         // Extrahiere die Zahl, letzte Stelle zuerst
00577         for (INT16 d = digits - 1; d >= offset; d -= 1)
00578         {
00579                 tempVal = colaa::getValueOfChar(number.at(d));
00580                 value += tempVal * factor;
00581                 factor *= baseFactor;
00582         }
00583 
00584         // Vorzeichen einbauen
00585         value *= sign;
00586 
00587         return value;
00588 }
00589 
00590 
00598 std::string decodeString(std::string* rxData, UINT16 len)
00599 {
00600         std::string text;
00601 
00602         // String extrahieren
00603         if (len == 0)
00604         {
00605                 // Keine spezielle Laenge gewuenscht.
00606                 text = colaa::getNextStringToken(rxData);
00607         }
00608         else
00609         {
00610                 // String bekannter Laenge ausschneiden
00611                 text = rxData->substr(0, len);
00612                 // Eingangsstring kuerzen
00613                 *rxData = rxData->substr(len + 1);
00614         }
00615 
00616         return text;
00617 }
00618 
00619 //
00620 // Lese einen UINT16-Wert aus dem Empfangspuffer.
00621 //
00622 UINT16 decodeUINT16(BYTE* buffer)
00623 {
00624         std::string data = getNextStringToken(buffer);
00625         return decodeUINT16(data);
00626 }
00627 
00628 //
00629 // Lese einen UINT16-Wert aus dem Empfangspuffer.
00630 //
00631 UINT16 decodeUINT16(std::string* rxData)
00632 {
00633         UINT32 value;
00634 
00635         value = decodeUINT32(rxData);
00636 
00637         assert (value < 0x10000);
00638         return (UINT16)value;
00639 }
00640 
00644 UINT8 decodeUINT8(std::string* rxData)
00645 {
00646         UINT32 value;
00647 
00648         value = decodeUINT32(rxData);
00649 
00650         assert (value < 256);
00651         return (UINT8)value;
00652 }
00653 
00657 UINT16 decodeUINT16(const std::string& rxData)
00658 {
00659         UINT32 value;
00660 
00661         value = decodeUINT32(rxData);
00662 
00663         assert (value < 0x10000);
00664         return (UINT16)value;
00665 }
00666 
00670 UINT8 decodeUINT8(const std::string& rxData)
00671 {
00672         UINT32 value;
00673 
00674         value = decodeUINT32(rxData);
00675 
00676         assert (value < 256);
00677         return (UINT8)value;
00678 }
00679 
00685 UINT32 decodeUINT32(const std::string& rxData)
00686 {
00687         UINT32 value = 0;
00688         UINT32 factor = 1;
00689         UINT32 tempVal;
00690         UINT32 baseFactor = 10; // 10 = dez, 16 = hex
00691         UINT16 digits;
00692         UINT16 offset = 0;
00693 
00694         // Zahlen-String extrahieren
00695         const std::string& number = rxData;
00696 
00697         // Unterscheidung Pos/Neg/Hex
00698         if (number.at(0) == '+')
00699         {
00700                 // pos. Dezimalzahl
00701 //              number = number.substr(1);      // Vorzeichen abschneiden
00702                 offset = 1;
00703         }
00704         else
00705         {
00706                 // Hexadezimalzahl
00707                 baseFactor = 16;                        // Hex.
00708         }
00709 
00710         // Anzahl Ziffern
00711         digits = number.length();
00712 
00713         // Extrahiere die Zahl, letzte Stelle zuerst
00714         for (INT16 d = digits - 1; d >= offset; d -= 1)
00715         {
00716                 tempVal = colaa::getValueOfChar(number.at(d));
00717                 value += tempVal * factor;
00718                 factor *= baseFactor;
00719         }
00720 
00721         return value;
00722 }
00723 
00730 UINT32 decodeXByte(std::string* rxData, UINT16 len)
00731 {
00732         UINT32 value;
00733         UINT32 result = 0;
00734 
00735         assert (len < 5);       // Wir koennen nur bis zu 4 Bytes in einen UINT32 packen
00736 
00737         for (UINT16 i = 0; i < len; i++)
00738         {
00739                 value = decodeUINT32(rxData);
00740                 assert (value < 256);
00741                 result += (value << (i * 8));
00742         }
00743 
00744         return result;
00745 }
00746 
00758 /*
00759 UINT32 decodeXByte(tokenizer::const_iterator& tok, const tokenizer::const_iterator& end, UINT16 len)
00760 {
00761         UINT32 value;
00762         UINT32 result = 0;
00763 
00764         assert (len < 5);       // Wir koennen nur bis zu 4 Bytes in einen UINT32 packen
00765 
00766         tokenizer::const_iterator last = tok;
00767 
00768         for ( UINT16 i = 0; i < len && tok != end; ++tok, ++i)
00769         {
00770                 value = decodeUINT32(*tok);
00771                 assert (value < 256);
00772                 result += (value << (i * 8));
00773 
00774                 last = tok;
00775         }
00776 
00777         // set token one position back
00778         tok = last;
00779 
00780 
00781         return result;
00782 }
00783 */
00784 
00785 //
00786 // Char-To-String-Umwandlung.
00787 //
00788 std::string convertRxBufferToString(UINT8* buffer, UINT16 bufferLen)
00789 {
00790         buffer[bufferLen-1] = 0x00;     // End-Code 0x03 ueberschreiben mit 0x00
00791         std::string result = std::string((char*)(&(buffer[2])));
00792         return result;
00793 }
00794 
00795 
00796 } // END namespace colaa


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