Guest User

IMU Linux

a guest
Jul 16th, 2014
544
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 1.59 KB | None | 0 0
  1. int IMU_thread()   
  2. {
  3.     char portname[] = "/dev/ttyACM0";
  4.     char readBuffer[1024];
  5.         int numBytesRead;
  6.  
  7.         FILE *serPort = fopen(portname, "r");
  8.  
  9.     if (serPort == NULL)    //Checks connection
  10.     {
  11.         printf("ERROR");   
  12.         return 0;
  13.     }
  14.     char incomingData[25] = "";         // don't forget to pre-allocate memory
  15.     int dataLength = 500;
  16.     int readResult = 0;
  17.     int toMove = 0;
  18.     int numtimes = 0;
  19.     clock_t t;
  20.     t = clock();
  21.     while(serPort != NULL)  // While it's plugged in
  22.     {
  23.         memset(incomingData,0,25);
  24.         readResult = 0;
  25.         char coord[10];
  26.         int coordopen = 0;
  27.         int coord_len = 0;
  28.         while (readResult <= 0 || incomingData[readResult-1] != '\n')
  29.         {
  30.             // read a raw byte
  31.             if (fread(incomingData+readResult, sizeof(char),25,serPort) ==1)    //THIS DOES NOT WORK
  32.                 readResult++;
  33.             else
  34.                 continue;
  35.  
  36.             // if not special demarcating character, keep adding to same coord
  37.             if (coordopen > 0 && incomingData[readResult-1] >= 44)
  38.             {
  39.                 coord[coord_len] = incomingData[readResult-1];
  40.                 coord_len++;
  41.             }
  42.  
  43.             // tab
  44.             else if (incomingData[readResult-1] == 9)
  45.             {
  46.                 // store coord if have been writing one
  47.                 if (coordopen > 0)
  48.                 {
  49.                     coord[coord_len] = '\n';
  50.                     coords[coordopen-1] = atof(coord);
  51.                 }
  52.  
  53.                 // prepare for new coord
  54.                 coordopen++;
  55.                 coord_len = 0;
  56.                 memset(coord,0,10);
  57.             }
  58.             // record last coord
  59.             else if (incomingData[readResult-1] == '\n' && coordopen > 0)
  60.             {
  61.                 mtx.lock();
  62.                 coords[coordopen-1] = atof(coord);
  63.                 mtx.unlock();
  64.             }
  65.         }
  66.     //  printf("Bytes read: (-1 means no data available) %i\n",readResult);
  67.     }
  68.     return 0;
  69.  
  70. }
Advertisement
Add Comment
Please, Sign In to add comment