mpu_get_sample_rate(&gyro_rate);
mpu_get_gyro_fsr(&gyro_fsr);
mpu_get_accel_fsr(&accel_fsr);
mpu_get_compass_fsr(&compass_fsr);
inv_set_gyro_sample_rate(1000000L/gyro_rate);
inv_set_accel_sample_rate(1000000L/gyro_rate);
、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@brief Body-to-world frame euler angles.
* The euler angles are output with the following convention:
* Pitch: -180 to 180
* Roll: -90 to 90
* Yaw: -180 to 180
* @param[out] data Euler angles in degrees, q16 fixed point.
* @param[out] accuracy Accuracy of the measurement from 0 (least accurate)
* to 3 (most accurate).
* @param[out] timestamp The time in milliseconds when this sensor was read.
* @return 1 if data was updated.
*/
int inv_get_sensor_type_euler(long *data, int8_t *accuracy, inv_time_t *timestamp)
{
long t1, t2, t3;
long q00, q01, q02, q03, q11, q12, q13, q22, q23, q33;
float values[3];
q00 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[0]);
q01 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[1]);
q02 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[2]);
q03 = inv_q29_mult(eMPL_out.quat[0], eMPL_out.quat[3]);
q11 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[1]);
q12 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[2]);
q13 = inv_q29_mult(eMPL_out.quat[1], eMPL_out.quat[3]);
q22 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[2]);
q23 = inv_q29_mult(eMPL_out.quat[2], eMPL_out.quat[3]);
q33 = inv_q29_mult(eMPL_out.quat[3], eMPL_out.quat[3]);
/* X component of the Ybody axis in World frame */
t1 = q12 - q03;
/* Y component of the Ybody axis in World frame */
t2 = q22 + q00 - (1L << 30);
values[2] = -atan2f((float) t1, (float) t2) * 180.f / (float) M_PI;
/* Z component of the Ybody axis in World frame */
t3 = q23 + q01;
values[0] =
atan2f((float) t3,
sqrtf((float) t1 * t1 +
(float) t2 * t2)) * 180.f / (float) M_PI;
/* Z component of the Zbody axis in World frame */
t2 = q33 + q00 - (1L << 30);
if (t2 < 0) {
if (values[0] >= 0)
values[0] = 180.f - values[0];
else
values[0] = -180.f - values[0];
}
/* X component of the Xbody axis in World frame */
t1 = q11 + q00 - (1L << 30);
/* Y component of the Xbody axis in World frame */
t2 = q12 + q03;
/* Z component of the Xbody axis in World frame */
t3 = q13 - q02;
values[1] =
(atan2f((float)(q33 + q00 - (1L << 30)), (float)(q13 - q02)) *
180.f / (float) M_PI - 90);
if (values[1] >= 90)
values[1] = 180 - values[1];
if (values[1] < -90)
values[1] = -180 - values[1];
data[0] = (long)(values[0] * 65536.f);
data[1] = (long)(values[1] * 65536.f);
data[2] = (long)(values[2] * 65536.f);
accuracy[0] = eMPL_out.quat_accuracy;
timestamp[0] = eMPL_out.nine_axis_timestamp;
return eMPL_out.nine_axis_status;
}
友情提示: 此问题已得到解决,问题已经关闭,关闭后问题禁止继续编辑,回答。
一周热门 更多>