source: pacpussensors/trunk/Gps/ui/polarxmainwindow_utils.cpp

#include "polarxmainwindow.hpp"

using namespace std ;

const long PolarxMainWindow::bufferToLong(int startByte, vector<unsigned char> *pbuffer){

   unsigned char tab4[4];
   long *plong;

   for (int i=0;i<4;i++)
      tab4[i]=pbuffer->at(startByte+i);

   plong = reinterpret_cast<long *>(tab4);
   return *plong;
}

const unsigned long PolarxMainWindow::bufferToULong(int startByte, vector<unsigned char> *pbuffer){

   unsigned char tab4[4];
   unsigned long *pULong;

   for (int i=0;i<4;i++)
      tab4[i]=pbuffer->at(startByte+i);

   pULong = reinterpret_cast<unsigned long *>(tab4);
   return *pULong;
}


const float PolarxMainWindow::bufferToFloat(int startByte, vector<unsigned char> *pbuffer){

   unsigned char tab4[4];
   float *pfloat;

   for (int i=0;i<4;i++)
      tab4[i]=pbuffer->at(startByte+i);

   pfloat = reinterpret_cast<float *>(tab4);
   return *pfloat;
}


const double PolarxMainWindow::bufferToDouble(int startByte, vector<unsigned char> *pbuffer){

   unsigned char tab8[8];
   double *pdouble;

   for (int i=0;i<8;i++)
      tab8[i]=pbuffer->at(startByte+i);

   pdouble = reinterpret_cast<double *>(tab8);
   return *pdouble;
}



const unsigned short PolarxMainWindow::bufferToUShort(int startByte, vector<unsigned char> *pbuffer){

   unsigned char tab2[2];
   unsigned short *pushort;

   for (int i=0;i<2;i++)
      tab2[i]=pbuffer->at(startByte+i);

   pushort = reinterpret_cast<unsigned short *>(tab2);
   return *pushort;
}


const short PolarxMainWindow::bufferToShort(int startByte, vector<unsigned char> *pbuffer){

   unsigned char tab2[2];
   short *pshort;

   for (int i=0;i<2;i++)
      tab2[i]=pbuffer->at(startByte+i);

   pshort = reinterpret_cast<short *>(tab2);
   return *pshort;
}

char PolarxMainWindow::extractChar(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
{
   char output;
   unsigned short indexDB, firstOffsetDB, lastOffsetDB;
   char charMask2[9] = {0, 1, 3, 7, 15, 31, 63, 127, 255 };

   if (length>8)
      length = 8;

        //ATTENTION index start at 0
   indexDB = (startBit-1)/8;
   firstOffsetDB = (startBit-1)%8;
   lastOffsetDB = (startBit+length-2)%8;

   if (bytesSpread( startBit, length)<=1)
   {
      output =  pbuffer->at(indexDB);
      output = ((output>>(7-lastOffsetDB)) & charMask2[length]);
   }
   else
   {
      char c1, c2;
      c1 = pbuffer->at(indexDB+1)>>(7-lastOffsetDB);
      c2 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
      output =  c1 + (c2<<(lastOffsetDB+1));
   }

   return output;
}

short PolarxMainWindow::extractShort(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
{
   short Unsgnd;
   short comp = (1<<(length-1));

   Unsgnd = extractUShort(pbuffer, startBit, length);

   if (Unsgnd>comp)
      return (2*comp - Unsgnd)*-1;
   else
      return Unsgnd;
}

unsigned short PolarxMainWindow::extractUShort(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
{
   unsigned short output;
   unsigned short indexDB, firstOffsetDB, lastOffsetDB;
   char charMask2[9] = {0, 1, 3, 7, 15, 31, 63, 127, 255 };


   if (length>16)
      length = 16;

   if (length<=8)
      return (short)extractChar(pbuffer, startBit, length);

   indexDB = (startBit-1)/8;
   firstOffsetDB = (startBit-1)%8;
   lastOffsetDB = (startBit+length-2)%8;

   if (bytesSpread( startBit, length)<=2)
   {
      unsigned short s1, s2;
      s1 = pbuffer->at(indexDB+1)>>(7-lastOffsetDB);
      s2 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
      output =  s1 + (s2<<(lastOffsetDB+1));
   }
   else
   {
      unsigned short s1, s2, s3;
      s1 = pbuffer->at(indexDB+2)>>(7-lastOffsetDB);
      s2 = pbuffer->at(indexDB+1);
      s3 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
      output = s1 + (s2<<(lastOffsetDB+1)) + (s3<<(lastOffsetDB+9));
   }

   return output;
}


long PolarxMainWindow::extractLong(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
{
   long Unsgnd;
   long comp = (1<<(length-1));

   Unsgnd = extractULong(pbuffer, startBit, length);

   if (Unsgnd>comp)
      return (2*comp - Unsgnd)*-1;
   else
      return Unsgnd;
}


unsigned long PolarxMainWindow::extractULong(vector<unsigned char> *pbuffer, unsigned short startBit, unsigned short length)
{
   unsigned long output;
   unsigned short indexDB, firstOffsetDB, lastOffsetDB;
   char charMask2[9] = {0, 1, 3, 7, 15, 31, 63, 127, 255 };


   if (length>32)
      length = 32;

   if (length<=16)
      return (long)extractShort(pbuffer, startBit, length);

   indexDB = (startBit-1)/8;
   firstOffsetDB = (startBit-1)%8;
   lastOffsetDB = (startBit+length-2)%8;

   unsigned long l1, l2, l3, l4, l5;
   l1 = 0; l2 = 0; l3 = 0; l4 = 0; l5 = 0;

   switch(bytesSpread( startBit, length))
   {
      case 3:
      {
         l1 = pbuffer->at(indexDB+2)>>(7-lastOffsetDB);
         l2 = pbuffer->at(indexDB+1);
         l3 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
         output = l1 + (l2<<(lastOffsetDB+1)) + (l3<<(lastOffsetDB+9));
         break;
      }
      case 4:
      {
         l1 = pbuffer->at(indexDB+3)>>(7-lastOffsetDB);
         l2 = pbuffer->at(indexDB+2);
         l3 = pbuffer->at(indexDB+1);
         l4 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
         output = l1 + (l2<<(lastOffsetDB+1)) + (l3<<(lastOffsetDB+9)) + (l3<<(lastOffsetDB+17));
         break;
      }
      case 5:
      {
         l1 = pbuffer->at(indexDB+4)>>(7-lastOffsetDB);
         l2 = pbuffer->at(indexDB+3);
         l3 = pbuffer->at(indexDB+2);
         l3 = pbuffer->at(indexDB+1);
         l4 = (pbuffer->at(indexDB) & charMask2[8-firstOffsetDB]);
         output = l1 + (l2<<(lastOffsetDB+1)) + (l3<<(lastOffsetDB+9)) + (l3<<(lastOffsetDB+17)) +  (l4<<(lastOffsetDB+25));
         break;
      }
   }
   return output;
}

unsigned short PolarxMainWindow::bytesSpread(unsigned short startBit, unsigned short length)
{
   unsigned short nbBytes;
   nbBytes = ((startBit+length-2)/8 - (startBit-1)/8 +1);
   return nbBytes;
}