// 参数说明：
// 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;}
}