讀 mpu sensor 轉出 yaw, pitch, roll

  // Accel + gyro.
    imu->update();
    imu->read_accelerometer(&ax, &ay, &az);
    imu->read_gyroscope(&gx, &gy, &gz);

    ax /= G_SI;
    ay /= G_SI;
    az /= G_SI;
    gx *= 180 / PI;
    gy *= 180 / PI;
    gz *= 180 / PI;

    ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);

     //------------------- Discard the time of the first cycle -----------------

    if (!isFirst)

    {

        if (dt > maxdt) maxdt = dt;

        if (dt < mindt) mindt = dt;

    }

    isFirst = 0;

    //------------- Console and network output with a lowered rate ------------

    dtsumm += dt;

    if(dtsumm > 0.05)

    {

        // Console output

        printf("ROLL: %+05.2f PITCH: %+05.2f YAW: %+05.2f PERIOD %.4fs RATE %dHz \n", roll, pitch, yaw * -1, dt, int(1/dt));

        // Network output

        sprintf(sendline,"%10f %10f %10f %10f %dHz\n", ahrs.getW(), ahrs.getX(), ahrs.getY(), ahrs.getZ(), int(1/dt));

        sendto(sockfd, sendline, strlen(sendline), 0, (struct sockaddr *)&servaddr, sizeof(servaddr));

        dtsumm = 0;

    }