ฉันมีบลูทูธ IMU (เห็นเป็นอุปกรณ์อนุกรมที่ /dev/rfcomm0) ที่คืนค่ามุมปัจจุบันเป็นควอเตอร์เนียน 50 ครั้งต่อวินาที

ฉันทำโปรแกรมทดสอบเพื่ออ่านข้อมูลและมันใช้งานได้ แต่ฉันต้องรวมมันในช่วงเวลาหนึ่งซึ่งหลังจากอ่านเสร็จแล้ว เธรดถูกสร้างขึ้นและการประมวลผลที่มีราคาแพงในการประมวลผล ดังนั้นฉันจึงไม่สามารถอ่านอัตรา ด้วยข้อมูล IMU

IMU ส่งแพ็กเก็ต 54 ไบต์ให้ฉัน และสองไบต์แรกจะถูกตั้งค่าเป็น Ascii "tt" เสมอ

นี่คือรหัส C ที่ฉันใช้:

int set_interface_attribs(int fd, int speed)
{
        struct termios tty;

        if (tcgetattr(fd, &tty) < 0) {
              printf("Error from tcgetattr: %s\n", strerror(errno));
              return -1;
        }

        cfsetospeed(&tty, (speed_t)speed);
        cfsetispeed(&tty, (speed_t)speed);

        tty.c_cflag |= (CLOCAL | CREAD);    /* ignore modem controls */
        tty.c_cflag &= ~CSIZE;
        tty.c_cflag |= CS8;         /* 8-bit characters */
        tty.c_cflag &= ~PARENB;     /* no parity bit */
        tty.c_cflag &= ~CSTOPB;     /* only need 1 stop bit */
        tty.c_cflag &= ~CRTSCTS;    /* no hardware flowcontrol */

        /* setup for non-canonical mode */
        tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
        tty.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
        tty.c_oflag &= ~OPOST;

        /* fetch bytes as they become available */
        tty.c_cc[VMIN] = 54;
        tty.c_cc[VTIME] = 1;

        if (tcsetattr(fd, TCSANOW, &tty) != 0) {
           printf("Error from tcsetattr: %s\n", strerror(errno));
           return -1;
        }
        return 0;
}

int start_serial()
{
    char *portname = "/dev/rfcomm0";
    int fd;
    int wlen;
    fd = open(portname, O_RDWR | O_NOCTTY | O_SYNC);
    if (fd < 0) {
        printf("Error opening %s: %s\n", portname, strerror(errno));
        return -1;
    }
    set_interface_attribs(fd, B115200);
    wlen = fwrite(fd, "?!CALIB050!?\n", 14);
    if (wlen != 14) {
        printf("Error from write: %d, %d\n", wlen, errno);
    }

    tcflush(fd,TCIFLUSH);
    tcdrain(fd);    /* delay for output */
    return fd;
}

int read_serial(int fd)
{
    char buf[54];
    int rdlen;
    rdlen = read(fd, buf, 54);
    if (rdlen > 0)
    {
        struct CPacketCalib cc = CPacketCalibConvert(buf); 
        printf("%f %f %f %f\n",cc.q.I, cc.q.J, cc.q.K,cc.q.W);
    }
    else if (rdlen < 0)
    {
        printf("Error from read: %d: %s\r\n", rdlen, strerror(errno));
    }
    return 1;
}

int stop_serial(int fd)
{
    int wlen = write(fd, "?!STOPSEND!?\n", 14);
    if (wlen != 14)
    {
        printf("Error from write: %d, %d\n", wlen, errno);
    }
    fclose(fd);
    return 1;
}

ฉันจะล้างบัฟเฟอร์อนุกรมทุกรอบและเริ่มรับแพ็กเก็ตของฉันอย่างถูกต้องได้อย่างไร

ขอขอบคุณสำหรับความช่วยเหลือของคุณ.

ตอบ

ปัญหาของคุณเป็นอย่างไรคือคุณเริ่มอ่านกลางแพ็กเก็ตและรับครึ่งหนึ่งของแพ็กเก็ตหนึ่งและอีกครึ่งหนึ่ง ในกรณีนั้น คุณต้องอ่านจนกว่าจะถึงจุดเริ่มต้นของแพ็กเก็ต จากนั้นจึงป้อนไปยังฟังก์ชันการแยกวิเคราะห์ ดังนั้นสิ่งนี้:

char ch;
char[52] buf;
while(1){
    if(read(fd, &ch, sizeof(ch)) > 0 && ch == 't'){  
         if(read(fd, &ch, sizeof(ch)) > 0 && ch == 't'){
             if(read(fd, buf, sizeof(buf)) == sizeof(buf)){
                  struct CPacketCalib cc = CPacketCalibConvert(buf);
             }
         }
    }
}

ดังนั้นCPacketCalibConvertจะถูกเรียกด้วยแพ็กเก็ตข้อมูลที่สมบูรณ์เท่านั้น (ลบสอง 't อาจเพิ่มกลับเข้าไปในบล็อกชั้นในสุด?)