0

我使用库https://www.google.com/url?sa=t&source=web&rct=j&url=https://github.com/arduino-libraries/MadgwickAHRS&ved=2ahUKEwiGrpOakcvjAhWOlIsKHXsQD_EQFjAAegQIBRAB&usg=AOvVaw3198y6BzSifkJqTH8BGy8M 我从树莓派 mpu9250 获取数据Pi平台。但是一个四元数几乎等于1，这正常吗？我使用 updateIMU（仅使用加速度计和陀螺仪），然后调用该函数将四元数转换为 Eiler 的角。但是m_pitch、m_roll、m_yaw都很小，我也试过把这些值乘以180/3.14，但是值还是很小。如何解决此问题并获得从 0 到 360 的法线角度？

陀螺仪以度/秒为单位，加速度计范围为 2G

madgwick_init();
//begin(50);

while(1){

    gyro_get_raw(calibrate_raw, &gx, &gy, &gz);
    accel_get_axis(calibrate_range, &ax, &ay, &az);

    updateIMU((float)gx, (float)gy, (float)gz, (float)ax, (float)ay, (float)az);

    printf("accel: x = %d\n", ax);
    printf("accel: y = %d\n", ay);
    printf("accel: z = %d\n\n", az);

    printf("gyro: x = %d\n", gx);
    printf("gyro: y = %d\n", gy);
    printf("gyro: z = %d\n\n", gz);

    printf("q0 = %f\n", q0);
    printf("q1 = %f\n", q1);
    printf("q2 = %f\n", q2);
    printf("q3 = %f\n", q3);

    // get_euler_angles(&q0, &q1, &q2, &q3);
    computeAngles();

    printf("roll: %f\n", m_roll);
    printf("pitch: %f\n", m_pitch);
    printf("yaw: %f\n", m_yaw);

    // printf("deg: x = %f\n", gyro_trapezoid_integration(GYRO_INTEGRATION_X, 1));
    // printf("deg: y = %f\n", gyro_trapezoid_integration(GYRO_INTEGRATION_Y, 1));
    // printf("deg: z = %f\n\n", gyro_trapezoid_integration(GYRO_INTEGRATION_Z, 1));



    //printf("x = %d\n", gyro_get_x_raw());
    //printf("(double)x = %f\n", gyro_get_x_calib_raw(0.00875));
    //printf("rpm: %d\n", 1500);
    //set_speed_motor(4, 1500);

    delay(250);
    system("clear");
}
4

0 回答 0