? ?　检测感应器：MPU9250，九轴感应器，在其中有三个轴便是手机陀螺仪的三个方位角速度。

　　手机陀螺仪在每一个取样点得到：取样时时刻刻（企业细微），XYZ三个方位的角速度（企业倾斜度/秒），记为：wx， wy， wz。手机陀螺仪静止不动时，wx， wy， wz也是有读值的，这就是手机陀螺仪的零漂。

　　试验一：将手机陀螺仪绕X轴转动时，仅有wx有读值；将手机陀螺仪绕Y轴转动时，仅有wy有读值；将手机陀螺仪绕Z轴转动时，仅有wz有读值；

　　试验二：将手机陀螺仪绕XY表面的轴转动，wz读值为零，即与转动轴竖直的轴上的角速度为零。

　　由于手机陀螺仪采样频率很高（1000Hz），根据瞬间读值测算姿势，能够看作：先绕X轴转动，再绕Y轴转动，再绕Z轴转动。

　　下边一段编码完成了一个简洁的手机陀螺仪姿势优化算法，启动并静放几十秒后，拿着手机陀螺仪转动，十几分钟内姿势是合理的，以后因为積分积累，偏差就越发变大。
? ? ? ??// 主要参数表明：

// sampleTS? ?: 取样时时刻刻，企业：微秒 // wx, wy, wz ：手机陀螺仪取样，企业：倾斜度/秒 void GyroExperiment(uint64_t sampleTS, float wx, float wy, float wz) { ? ? // 感应器运行时时刻刻 ? ? static uint64_t s_lastTIme = 0; ? ? staTIc uint64_t s_lastlog = 0; ? ? if(s_lastTIme == 0){ ? ? ? ? s_lastTIme = sampleTS; ? ? ? ? s_lastlog = sampleTS; ? ? ? ? return; ? ? } ? ? // 选用运行后5秒-35秒的选用均值做为手机陀螺仪零漂 ? ? // 在这段时间，应维持手机陀螺仪静止不动 ? ? static float s_wx = 0, s_wy = 0, s_wz = 0;? ? // 手机陀螺仪零漂 ? ? static uint64_t s_elapsed = 0; ? ? if(s_elapsed < 35000000){ ? ? ? ? static int s_SampleCnt = 0; ? ? ? ? if(s_elapsed > 5000000){ ? ? ? ? ? ? s_wx = (wx - s_wx) / (s_SampleCnt 1); ? ? ? ? ? ? s_wy = (wy - s_wy) / (s_SampleCnt 1); ? ? ? ? ? ? s_wz = (wz - s_wz) / (s_SampleCnt 1); ? ? ? ? ? ? s_SampleCnt ; ? ? ? ? } ? ? ? ? s_elapsed = (sampleTS - s_lasttime); ? ? ? ? s_lasttime = sampleTS; ? ? } ? ? // 原始姿势，选用三个轴向量表示 ? ? static float Xx=1,Xy=0,Xz=0;? ? // X轴 ? ? static float Yx=0,Yy=1,Yz=0;? ? // Y轴 ? ? static float Zx=0,Zy=0,Zz=1;? ? // Z轴 ? ? // 依据手机陀螺仪读值测算三个轴的转动量 ? ? float interval = (sampleTS - s_lasttime) / 1e6; ? ? float rx = (wx - s_wx) * interval; ? ? float ry = (wy - s_wy) * interval; ? ? float rz = (wz - s_wz) * interval; ? ? // 各自结构绕三个轴转动的四元数 ? ? float cos,sin; ? ? cos = cosf(rx/2); sin = sinf(rx/2); Quaternion qx(cos, Xx * sin, Xy * sin, Xz * sin); ? ? cos = cosf(ry/2); sin = sinf(ry/2); Quaternion qy(cos, Yx * sin, Yy * sin, Yz * sin); ? ? cos = cosf(rz/2); sin = sinf(rz/2); Quaternion qz(cos, Zx * sin, Zy * sin, Zz * sin); ? ? // 先后转动三个轴空间向量 ? ? Quaternion q = qx*qz*qy; q.normalize(); Quaternion qi = q.inverse(); ? ? Quaternion QX(0, Xx, Xy, Xz); QX = q*QX*qi; QX.normalize(); Xx = QX.q2; Xy = QX.q3; Xz = QX.q4;? ? ? ? // 转动X轴; ? ? Quaternion QY(0, Yx, Yy, Yz); QY = q*QY*qi; QY.normalize(); Yx = QY.q2; Yy = QY.q3; Yz = QY.q4;? ? ? ? // 转动Y轴; ? ? Quaternion QZ(0, Zx, Zy, Zz); QZ = q*QZ*qi; QZ.normalize(); Zx = QZ.q2; Zy = QZ.q3; Zz = QZ.q4;? ? ? ? // 转动Z轴; ? ? // 每1秒輸出一次姿势数据信息 ? ? s_lasttime = sampleTS; ? ? if(sampleTS - s_lastlog > 1000000){ ? ? ? ? console->printf("attitude: [%f, %f, %f]; [%f, %f, %f]; [%f, %f, %f]\n", Xx, Xy, Xz, Yx, Yy, Yz, Zx, Zy, Zz); ? ? ? ? s_lastlog = sampleTS; ? ? } }

上一篇：MEMS加速度计工作原理介绍

下一篇：角度传感器工作原理与使用方法