Проблема со скоростью передачи данных Bluetooth

Добрый вечер всем,

Я пытаюсь установить связь между Arduino lilypad и Raspberry Pi3B.

Я могу отправлять и получать данные, но связь очень медленная!!!

Я отправляю символ из Rasp в arduino, который отвечает, отправляя обратно пакет из 30 байт с некоторой информацией в следующем виде:

# байт1 байт2 ... байт28 !

Код малины:

#include <stdio.h>   /* Standard input/output definitions */
#include <string.h>  /* String function definitions */
#include <fcntl.h>   /* File control definitions */
#include <errno.h>   /* Error number definitions */
#include <termios.h> /* POSIX terminal control definitions */
#include <vector>                                                   
#include <fstream> //for file read/write
#include <iostream> 
#include <unistd.h> 
#include <sstream> //use for hex conversion
#include <sys/time.h>
#include <time.h>

using namespace std;

    char* buffer;
    int SerialHandleSensorsFoot;                                    
    char* bufferTemp;                                               
    char* bufferDEF;                                                
    typedef vector<double> rawData;





int main()
{




    bufferTemp=new char [1000];
    bufferDEF = new char[1000];
    const char* _portNameSensorsBT="/dev/rfcomm0";

    struct termios options;
    SerialHandleSensorsFoot=open(_portNameSensorsBT, O_RDWR ); //SerialHandleSensors=open(_portNameSensors, O_RDWR | O_NOCTTY | O_NDELAY); non blocking
    if (SerialHandleSensorsFoot == -1 )
    {
        cout<<endl<<"......ERROR: Unable to open: "<<_portNameSensorsBT<<endl;
        return 0;
    }
    else
    {
        fcntl(SerialHandleSensorsFoot, F_SETFL,0);
        cout<<"......OPENED PORT: Succesfully opened: "<<_portNameSensorsBT<<endl;
    }

    //GET THE OPTIONS, MODIFY AND SET
    tcgetattr(SerialHandleSensorsFoot,&options);
    cfsetispeed(&options,B115200); //BAUD RATE IN
    cfsetospeed(&options,B115200); //BAUD RATE OUT
    // options.c_lflag |= (ICANON | ECHO | ECHOE);

    options.c_iflag = IGNBRK | IGNPAR;
    options.c_oflag = 0; 
    options.c_lflag = 0; 

    options.c_cflag |= (CLOCAL | CREAD);
    options.c_cflag &= ~PARENB;
    options.c_cflag |= CSTOPB;
    options.c_cflag &= ~CSIZE;
    options.c_cflag |= CS8;
    tcsetattr(SerialHandleSensorsFoot,TCSAFLUSH,&options);

    usleep(5000000);

    ////END OF SETTINGS, START OF COMMUNICATION/////++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++


    unsigned char bytes[4];
    rawData FootSensors;        
    rawData AllData;  

struct timeval tvalBefore, tvalAfter;  

    int tempRead=0;
     int posizione = 0;
    int tempWrite=0;
    //int byteDaLeggere = 29;
    //int numerobyte=28;
    bytes[0]=0x45;
    //int value;
    //struct timeval tStart, tEnd, t0, t1, t2;
//  long int elapsedTime10[70], elapsedTimeES, elapsedTime20[70];
    long unsigned int c1=0;
     long unsigned int c2=0;
     long unsigned int c3=0;
     long unsigned int c4=0;
     long unsigned int c5=0;
     long unsigned int c6=0;
    cout<<"START"<<endl;
    gettimeofday (&tvalBefore, NULL);                   ///SET START TIME
    int num=50;             ///NUMBER OF TIMES YOU REQUEST A SET OF 14 SENSORS VALUES  

    /// data:    # b1 b2 b3 b4 b5 b6 b7 ... b27 b28 !
    ///          ^                                  ^
    ///          |                                  |
    ///        start                               end

    for(int k=0; k<num; k++){  

    int byteDaLeggere=29;   ///after you have found the '#' you want to read 29 byte

    tempWrite=write(SerialHandleSensorsFoot,bytes,1);  ///send 'E' to Arduino



    if(tempWrite==-1)
    {
        cout<<"......ERROR: PROBLEM WRITING TO ROBOTIC HAND"<<endl;

         return 0;
    }

    else{
            do{     c1++;                                                                           ///          C1= how many times we read before finding a '#'
                    tempRead = read(SerialHandleSensorsFoot, bufferTemp, 1);


                    if (tempRead == -1) 
                        {   cout<<"......ERROR: PROBLEM READING FROM ROBOTIC HAND"<<endl;


                            return 0; 
                        }
                } while (bufferTemp[0] != '#');






            do{  
                c2=c2+1;                ///         C2
                tempRead = read (SerialHandleSensorsFoot, bufferTemp,byteDaLeggere);

//cout<<"tempread vale: " <<tempRead<<endl; 
                    if (tempRead <0) 
                            {   cout<<"......ERROR: PROBLEM READING FROM ROBOTIC HAND"<<endl;
                                c3++;                                                               ///          C3
                                return 0;                   
                            }

                    if (tempRead > 0){
                                c4++;                                                               ///         C4
                        for(int i=0; i< tempRead; i++){ 
                        bufferDEF[posizione] = bufferTemp[i];

                        posizione = posizione + 1;}


                        byteDaLeggere=byteDaLeggere-tempRead;

                    }

                    if (tempRead ==0){
                        c5++;
                        cout<<"tempRead vale zero!!"<<endl;}

            }while (byteDaLeggere);

            //cout<<"Posizione vale: "<<posizione<<endl<<flush;

            if(bufferDEF[posizione-1] == '!'){

        c6++;

            }
            else {cout<<"DATO NON LETTO"<<endl;}
}
} 
gettimeofday (&tvalAfter, NULL);
cout<<"FINE"<<endl;
printf("Time in microseconds: %ld microseconds\n",
            ((tvalAfter.tv_sec - tvalBefore.tv_sec)*1000000L
           +tvalAfter.tv_usec) - tvalBefore.tv_usec);

          cout<<"I've read "<<c1-num<<" charactes before finding '#'"<<endl;
          cout<<"I've tried to read "<<c2<<" times before getting all the "<<num<<" datas"<<endl;

          cout<<c3<<"--> Errors in reading function 'tempRead()'"<<endl;
          cout<<c4<<"--> times I've read something"<<endl;
          cout<<c5<<"--> times tempread() was 0"<<endl;
          cout<<c6<<"/"<<num<<" Dati corretti"<<endl;


}

Код Ардуино:

#include "SPI.h"
byte incomingByte = 0;   // for incoming serial data

const int N_SENSORI=14;
const int DATAOUT = 11;
const int DATAIN = 12;
const int SPICLOCK = 13;
const int SLAVESELECT = 10;


//===============FUNZIONE================
short write_read_spi16(short what) {
  digitalWrite(SS, LOW);
  short res = SPI.transfer16(what);
  digitalWrite(SS, HIGH); 
  return res;
}

void longInt2Byte( int x){
  unsigned char buf[sizeof( int)];
  memcpy(buf,&x,sizeof(int));
Serial.write(buf,sizeof(buf));



}

void send16(int value)
{
   Serial.write(value & 0xFF);
   Serial.write((value >> 8) & 0xFF); 
   /*byte Most=(value & 0xFF);
   byte Last=((value >> 8) & 0xFF);
   Serial.print("\nPrimo: ");
   Serial.print(Most,BIN);
   Serial.print("\nSecondo: ");
   Serial.print(Last,BIN);*/
}
//=======================================

void setup() {

  Serial.begin(115200);     
  SPI.begin();
  pinMode(DATAOUT, OUTPUT);
  pinMode(DATAIN, INPUT);
  pinMode(SPICLOCK,OUTPUT);
  pinMode(SLAVESELECT,OUTPUT); 

}
void loop() {

        // Mando solo quando ricevo qualcosa
if (Serial.available() > 0) {// Se ho un byte in entrata...           
  incomingByte= Serial.read();

  if (incomingByte=='E') {

 Serial.write('#');
 /*  write_read_spi16(0b1000001101110000); 

    for (int i = 1; i<N_SENSORI+1; i++) {
      short s = write_read_spi16(0b1000001101110000 | (i<<10));
      int Lettura = s&0xFFF;
     send16(Lettura);
    }
*/

      send16(1234);
      send16(1111);
      send16(2222);
      send16(3333);
      send16(4444);
      send16(5555);
      send16(6666);
      send16(7777);
      send16(8888);
      send16(9999);
      send16(9898);
      send16(7676);
      send16(5454);
      send16(3232);


    Serial.write('!');


}}}

Как видите, со стороны Arduino данные жестко закодированы, чтобы убедиться, что такое поведение не связано с какой-либо проблемой АЦП.

Вывод этого кода:

....OPENED PORT: Succesfully opened: /dev/rfcomm0
START
FINE
Time in microseconds: 2902017 microseconds
I've read 0 characters before finding '#'
I've tried to read 93 times before getting all the 50 datas
0 --> Errors in reading function "tempRead()"
93 --> Times I've read something
0 --> Times tempRead() was 0
50/50 correct datas

как видите, он пытается прочитать все 29 байт сразу, но в половине случаев терпит неудачу, поэтому для сбора полного пакета из 29 байт требуется 2 лекции. Для передачи 50 раз требуется 3 секунды! Мне нужно что-то вроде 50 Гц

Если я использую USB-соединение, для передачи 50 раз требуется 0,2 секунды, но для сбора 29 байтов требуется более 6 циклов.

Спасибо за ваше время!!!

Дарио