595 lines
15 KiB
C++
595 lines
15 KiB
C++
/******************** (C) COPYRIGHT 2023 Geek************************************
|
||
* File Name : SensorImpl.cpp
|
||
* Current Version : V1.0
|
||
* Author : zhanli
|
||
* Date of Issued : 2023.09.10
|
||
* Comments : GeekIMU 传感器实现模块
|
||
********************************************************************************/
|
||
/* Header File Including -----------------------------------------------------*/
|
||
|
||
#include "windows.h"
|
||
#include "SensorImpl.h"
|
||
#include "stdio.h"
|
||
#include "quaternion.h"
|
||
#include "math.h"
|
||
#include "UsbHid_CAPI.h"
|
||
#include "MyCls.h"
|
||
extern "C" FILE* fpdata;
|
||
Quaternion GlobalQ;
|
||
BOOL GlobalQ_Updating;
|
||
float LocX,LocY,LocZ;
|
||
float Yaw,Pitch,Roll;
|
||
extern FILE* Getfpdata();
|
||
extern int global_calibration_flag;
|
||
extern int global_Show;
|
||
UINT8 global_key[4] = { 0 };
|
||
float gxc=0, gyc=0, gzc=0;
|
||
int gyro_cali_count = 0;
|
||
#define GYRO_CALIBRATION_SAMPLE_NUMBER 3000
|
||
float gx0=0, gy0=0, gz0=0;
|
||
BOOL GlobalMag_Updating = false;
|
||
Vector3 GlobalMag = {0};
|
||
static bool HmdStandByState;
|
||
|
||
Quaternion CQ_DM = { 1, 0, 0, 0 };
|
||
|
||
bool DM_Corrected = false;
|
||
|
||
void MagFilter(float *mx,float *my,float *mz,float sita);
|
||
void GyroComputeDrift(float *gx, float *gy, float *gz);
|
||
|
||
void LowPassFilterYaw(float *z, float alpha);
|
||
|
||
bool IsHmdIntoStandBy(float *Pitch, float *Roll, float dTime);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : UnpackSensor
|
||
* Description : Oculus 数据解包
|
||
* Date : 2023/09/10 logzhan : 重新整理代码
|
||
*---------------------------------------------------------------------**/
|
||
void UnpackSensor(const UCHAR* buffer, long* x, long* y, long* z)
|
||
{
|
||
struct {INT32 x:21;} s;
|
||
*x = s.x = (buffer[0] << 13) | (buffer[1] << 5) | ((buffer[2] & 0xF8) >> 3);
|
||
*y = s.x = ((buffer[2] & 0x07) << 18) | (buffer[3] << 10) | (buffer[4] << 2) |
|
||
((buffer[5] & 0xC0) >> 6);
|
||
*z = s.x = ((buffer[5] & 0x3F) << 15) | (buffer[6] << 7) | (buffer[7] >> 1);
|
||
}
|
||
|
||
Vector3 GetMagData()
|
||
{
|
||
Vector3 v = { 0 };
|
||
static Vector3 VLast = { 0 };
|
||
if (!GlobalMag_Updating)
|
||
{
|
||
VLast = GlobalMag;
|
||
}
|
||
v = VLast;
|
||
return v;
|
||
}
|
||
|
||
void GetControllerSensor(unsigned char usb_input_buffer[])
|
||
{
|
||
static UINT8 updatecnt = 0;
|
||
INT16 MagX,MagY,MagZ;
|
||
long GX,GY,GZ;
|
||
long AX,AY,AZ;
|
||
UINT NowTime;
|
||
float ax, ay, az, gx, gy, gz, mx, my, mz, mx1, my1, mz1;;
|
||
static float mxlast=0,mylast=0,mzlast=0;
|
||
static int Dtime = 0;
|
||
static UINT LastTime = 0;
|
||
float q0,q1,q2,q3;
|
||
|
||
|
||
NowTime = *(UINT16* )(usb_input_buffer+2);
|
||
if(LastTime != 0)
|
||
{
|
||
if(NowTime >= LastTime)
|
||
{
|
||
Dtime = NowTime - LastTime;
|
||
LastTime = NowTime;
|
||
}else
|
||
{
|
||
Dtime = (65535 - LastTime) + NowTime;
|
||
LastTime = NowTime;
|
||
}
|
||
|
||
}else
|
||
{
|
||
Dtime = 1;
|
||
LastTime = NowTime;
|
||
}
|
||
|
||
if (global_Show==5)
|
||
{
|
||
//clrscr();
|
||
back2zero();
|
||
printf("Hex数据\n\n");
|
||
for (int i = 0; i < 64; i++)
|
||
{
|
||
if ((i%16)==0)
|
||
{
|
||
printf("\n");
|
||
}
|
||
//if (global_debug)
|
||
printf("%2x ", usb_input_buffer[i]);
|
||
|
||
}
|
||
printf("\n");
|
||
}
|
||
|
||
|
||
MagX = *(INT16 *)(usb_input_buffer + 56);
|
||
MagY = *(INT16 *)(usb_input_buffer + 58);
|
||
MagZ = *(INT16 *)(usb_input_buffer + 60);
|
||
|
||
UnpackSensor(usb_input_buffer+8, &AX,&AY,&AZ);
|
||
UnpackSensor(usb_input_buffer+16, &GX,&GY,&GZ);
|
||
|
||
|
||
ax = (float)0.0001*AX;
|
||
ay = (float)0.0001*AY;
|
||
az = (float)0.0001*AZ;
|
||
|
||
gx = (float)0.0001*GX;
|
||
gy = (float)0.0001*GY;
|
||
gz = (float)0.0001*GZ;
|
||
|
||
|
||
printf("%f %f %f\n", ax, ay, az);
|
||
|
||
float x = 16.4 * 180 / 3.14159265358979323846F;
|
||
|
||
if (global_Show == 1)
|
||
{
|
||
printf("gx = %f,gy = %f gz =%f\n", gx*x, gy*x, gz*x);
|
||
}
|
||
|
||
GyroComputeDrift(&gx, &gy, &gz);
|
||
|
||
if (global_calibration_flag == 1)
|
||
{
|
||
if (gyro_cali_count > GYRO_CALIBRATION_SAMPLE_NUMBER)
|
||
{
|
||
gyro_cali_count = 0;
|
||
gxc = gyc = gzc = 0;
|
||
}
|
||
gyro_cali_count++;
|
||
gxc += gx*x;
|
||
gyc += gy*x;
|
||
gzc += gz*x;
|
||
if (gyro_cali_count == GYRO_CALIBRATION_SAMPLE_NUMBER)
|
||
{
|
||
gx0 = gxc / gyro_cali_count;
|
||
gy0 = gyc / gyro_cali_count;
|
||
gz0 = gzc / gyro_cali_count;
|
||
gyro_cali_count++;
|
||
global_calibration_flag = 0;
|
||
printf("%f %f %f\n", gx0, gy0, gz0);
|
||
}
|
||
}
|
||
|
||
|
||
global_key[0] = usb_input_buffer[24];
|
||
global_key[3] = usb_input_buffer[27];
|
||
|
||
|
||
mx = (float)MagX / 16;
|
||
my = (float)MagY / 16;
|
||
mz = (float)MagZ / 16;
|
||
if (global_calibration_flag==3)
|
||
{
|
||
GlobalMag_Updating = true;
|
||
mx = (float)MagX;
|
||
my = (float)MagY;
|
||
mz = (float)MagZ;
|
||
|
||
GlobalMag.x = (float)MagX;
|
||
GlobalMag.y = (float)MagY;
|
||
GlobalMag.z = (float)MagZ;
|
||
GlobalMag_Updating = false;
|
||
}
|
||
|
||
|
||
if(mxlast == 0&&mylast == 0&&mzlast == 0)
|
||
{
|
||
mxlast = mx;
|
||
mylast = my;
|
||
mzlast = mz;
|
||
|
||
MagFilter(&mx, &my, &mz, 0.95f);
|
||
for (int i = 0; i < 200; i++)
|
||
AHRSupdate(0, 0, 0, az, ax, ay, mz, mx, my, 1, Dtime * 400);
|
||
}
|
||
else
|
||
{
|
||
updatecnt++;
|
||
if (updatecnt > 10)
|
||
updatecnt = 0;
|
||
if (mx == mxlast&&my == my&&mz == mzlast && updatecnt < 10)
|
||
{
|
||
AHRSupdate(gz,gx,gy,az,ax,ay,mz,mx,my,1,Dtime);
|
||
|
||
}
|
||
else
|
||
{
|
||
mxlast = mx;
|
||
mylast = my;
|
||
mzlast = mz;
|
||
|
||
MagFilter(&mx,&my,&mz,0.95f);
|
||
|
||
AHRSupdate(gz,gx,gy,az,ax,ay,mz,mx,my,1,Dtime);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/**********************************************************************************************************
|
||
* Function Name : AHRSupdate
|
||
* 输入 : 三轴加速度计、陀螺仪、磁力计的值
|
||
* 描述 ：加速度计、陀螺仪、磁力计融合得到的欧拉角
|
||
|
||
1）这个算法基于标准笛卡尔右手坐标系，z轴朝上，x轴朝人，y轴朝右
|
||
2）传感器的方向需要变换到和改算法定义的坐标轴相同。
|
||
|
||
* 修改 ：2016/11/5 : 根据磁力计采样率比较低的特点，设置了2种模式：1、融合磁力计 2、不融合磁力计
|
||
2016/11/6 ：改进了四元数更新算法，原来的四元数更新算法会导致累计误差。
|
||
2016/11/9 : 规定了本算法运行的坐标轴，重写了四元数到欧拉角变换的代码。
|
||
***********************************************************************************************************/
|
||
|
||
|
||
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;
|
||
float exInt = 0, eyInt = 0, ezInt = 0; /*缩放积分误差 */
|
||
#define Kp 2.5f /*加速度计和磁力计对陀螺仪的比例修正参数 */
|
||
#define Ki 0.005f /*加速度计和磁力计对陀螺仪的积分修正参数 */
|
||
#define Kd 0.0f
|
||
static int AHRS_Init = 0;
|
||
|
||
|
||
void ResetAHRS()
|
||
{
|
||
AHRS_Init = 0; q0 = 1; q1 = 0; q2 = 0; q3 = 0;
|
||
}
|
||
|
||
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, char useMag, int Dtime)
|
||
{
|
||
/*原算法的定义*/
|
||
float norm, halfT;
|
||
float hx, hy, hz, bz, by;
|
||
float vx, vy, vz, wx, wy, wz;
|
||
float ex, ey, ez;
|
||
float qw, qx, qy, qz;
|
||
float AccelAjust, MagAjust;
|
||
/*方便之后的程序使用，减少计算时间*/
|
||
float q0q0 = q0*q0; float q0q1 = q0*q1;
|
||
float q0q2 = q0*q2; float q0q3 = q0*q3;
|
||
float q1q1 = q1*q1; float q1q2 = q1*q2;
|
||
float q1q3 = q1*q3; float q2q2 = q2*q2;
|
||
float q2q3 = q2*q3; float q3q3 = q3*q3;
|
||
if (AHRS_Init > 200)
|
||
{
|
||
halfT = 0.0005f*Dtime; /*得到每次姿态更新的周期的一半t = (1/SampleFrq) * 0.5 */
|
||
}
|
||
else
|
||
{
|
||
if (useMag == 1)
|
||
{
|
||
halfT = 0.1;
|
||
gx = gy = gz = 0;
|
||
AHRS_Init++;
|
||
}
|
||
else
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
|
||
norm = invSqrt(ax*ax + ay*ay + az*az); /*把加速度计向量转换为单位向量*/
|
||
|
||
if ((1.0f / norm) > 1.6 || (1.0f / norm) < 0.4){
|
||
AccelAjust = 0;
|
||
MagAjust = 1.2f;
|
||
|
||
}
|
||
else{
|
||
AccelAjust = 1.0f;
|
||
MagAjust = 1.0f;
|
||
}
|
||
|
||
|
||
ax = ax * norm;
|
||
ay = ay * norm;
|
||
az = az * norm; /*把磁力计向量转换为单位向量 */
|
||
norm = invSqrt(mx*mx + my*my + mz*mz);
|
||
mx = mx * norm;
|
||
my = my * norm;
|
||
mz = mz * norm;
|
||
|
||
|
||
hx = 2.0f*(mx*(0.5f - q2q2 - q3q3) + my*(q1q2 - q0q3) + mz*(q1q3 + q0q2)); /*把(mx,my,mz)转换到地理坐标系的(hx,hy,hz),利用H = Q^-1 * M */
|
||
hy = 2.0f*(mx*(q1q2 + q0q3) + my*(0.5f - q1q1 - q3q3) + mz*(q2q3 - q0q1));
|
||
hz = 2.0f*(mx*(q1q3 - q0q2) + my*(q2q3 + q0q1) + mz*(0.5f - q1q1 - q2q2));
|
||
|
||
/*计的数值为(bx,by,0),所以我们不关注hx的数值 */
|
||
// bx = sqrtf((hx*hx) + (hy*hy)); /*使磁力计正交化*/
|
||
by = sqrtf((hx*hx) + (hy*hy));
|
||
bz = hz;
|
||
|
||
/*v代表的是把地理坐标系的加速度(0,0,1g)转换到机体坐标系的加 */
|
||
vx = 2.0f*(q1q3 - q0q2); /*速度计(ax,ay,az),其实就是用R*(0,0,1),R为旋转矩阵，此矩阵可 */
|
||
vy = 2 * (q0q1 + q2q3); /*由四元数转换得到 */
|
||
vz = q0q0 - q1q1 - q2q2 + q3q3;
|
||
|
||
wx = 2.0f*(by*(q1q2 + q0q3) + bz*(q1q3 - q0q2)); /*把正交化的H即B从地理坐标系转换到飞行器坐标系，利用W = Q * B */
|
||
wy = 2.0f*(by*(0.5f - q1q1 - q3q3) + bz*(q0q1 + q2q3)); /*这里认为bx = 0 */
|
||
wz = 2.0f*(by*(q2q3 - q0q1) + bz*(0.5f - q1q1 - q2q2));
|
||
|
||
|
||
|
||
/*如果使用磁力计，则融合磁力计*/
|
||
if (useMag != 0)
|
||
{ /*用当前姿态向量和加速度的姿态向量做叉乘，乘积越小说明两个向量方向越相同*/
|
||
ex = AccelAjust*(ay*vz - az*vy) + MagAjust*(my*wz - mz*wy);
|
||
ey = AccelAjust*(az*vx - ax*vz) + MagAjust*(mz*wx - mx*wz);
|
||
ez = AccelAjust*(ax*vy - ay*vx) + MagAjust*(mx*wy - my*wx);
|
||
}
|
||
else
|
||
{
|
||
|
||
ex = AccelAjust*(ay*vz - az*vy);
|
||
ey = AccelAjust*(az*vx - ax*vz); /*因为加速度计没有校准，ez不靠谱*/
|
||
ez = AccelAjust*(ax*vy - ay*vx);
|
||
ez = 0;
|
||
}
|
||
|
||
if (ex != 0.0f && ey != 0.0f && ez != 0.0f)
|
||
{
|
||
/*这里使用了PID调节的方式，对角速度修正,Ki 是积分修正，Kp是直接修正 */
|
||
exInt = exInt + ex*Ki * halfT; /*对误差进行积分 */
|
||
eyInt = eyInt + ey*Ki * halfT;
|
||
ezInt = ezInt + ez*Ki * halfT;
|
||
gx = gx + Kp*(1 + Kd*fabsf(ex))*ex + exInt; /*使用比例和积分综合对陀螺仪进行修正，由于存在积分修正，所以才能确保回 */
|
||
gy = gy + Kp*(1 + Kd*fabsf(ey))*ey + eyInt; /*到期望回到的位置 */
|
||
gz = gz + Kp*(1 + Kd*fabsf(ez))*ez + ezInt;
|
||
}
|
||
|
||
|
||
qw = (-q1*gx - q2*gy - q3*gz)*halfT; /*利用一阶龙格库塔法对四元数进行更新*/
|
||
qx = (q0*gx + q2*gz - q3*gy)*halfT;
|
||
qy = (q0*gy - q1*gz + q3*gx)*halfT;
|
||
qz = (q0*gz + q1*gy - q2*gx)*halfT;
|
||
|
||
q0 = q0 + qw;
|
||
q1 = q1 + qx;
|
||
q2 = q2 + qy;
|
||
q3 = q3 + qz;
|
||
|
||
|
||
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); /*对四元数进行规范化，即四元数模为1*/
|
||
|
||
q0 = q0 * norm; //w
|
||
q1 = q1 * norm; //x
|
||
q2 = q2 * norm; //y
|
||
q3 = q3 * norm; //z
|
||
|
||
|
||
/*对原变换公式进行重写，避免了以前坐标轴混乱的问题，按照标准y上z右x前*/
|
||
Pitch = asin(-2.0f*(q2*q1 - q0*q3))* (180.0f / 3.141592f); /*轴进行四元数到欧拉角的转换 */
|
||
Yaw = atan2(q3*q1 + q0*q2, 0.5f - q3*q3 - q2*q2)* (180.0f / 3.141592f);
|
||
Roll = atan2(q3*q2 + q0*q1, 0.5f - q3*q3 - q1*q1)* (180.0f / 3.141592f);
|
||
|
||
LowPassFilterYaw(&Yaw, 0.9f);
|
||
|
||
if (AHRS_Init > 200)
|
||
{
|
||
GlobalQ_Updating = TRUE;
|
||
GlobalQ.w = q0;
|
||
GlobalQ.x = q2;
|
||
GlobalQ.y = q3;
|
||
GlobalQ.z = q1;
|
||
GlobalQ_Updating = FALSE;
|
||
}
|
||
printf("%f %f %f\n", Yaw, Roll, Pitch);
|
||
|
||
IsHmdIntoStandBy(&Pitch, &Roll, 1.0f);
|
||
}
|
||
|
||
|
||
void MagFilter(float *mx, float *my, float *mz, float sita)
|
||
{
|
||
static float xlast = 0, ylast = 0, zlast = 0;
|
||
if (xlast == 0 && ylast == 0 && zlast == 0)
|
||
{
|
||
xlast = *mx;
|
||
ylast = *my;
|
||
zlast = *mz;
|
||
}
|
||
else
|
||
{
|
||
xlast = sita*xlast + (1 - sita)*(*mx);
|
||
ylast = sita*ylast + (1 - sita)*(*my);
|
||
zlast = sita*zlast + (1 - sita)*(*mz);
|
||
}
|
||
}
|
||
|
||
|
||
/*******************************************************************************
|
||
* Function Name : invSqrt
|
||
* Description : 快速计算 1/Sqrt(x)，源自雷神3，神奇的0x5f3759df
|
||
* 输入 : x
|
||
* 输出 ：1/sqrt(x)
|
||
*******************************************************************************/
|
||
float invSqrt(float x)
|
||
{
|
||
float halfx = 0.5f * x;
|
||
float y = x;
|
||
long i = *(long*)&y;
|
||
i = 0x5f3759df - (i >> 1);
|
||
y = *(float*)&i;
|
||
y = y * (1.5f - (halfx * y * y));
|
||
return y;
|
||
}
|
||
|
||
|
||
void GyroComputeDrift(float *gx, float *gy, float *gz)
|
||
{
|
||
static float GxOffect = 0.0f, GyOffect = 0.0f, GzOffect = 0.0f;
|
||
static float GxOffectTmp = 0.0f, GyOffectTmp = 0.0f, GzOffectTmp = 0.0f;
|
||
static float GxTmp = 0.0f, GyTmp = 0.0f, GzTmp = 0.0f;
|
||
static int count = 0;
|
||
|
||
*gx = *gx - GxOffect;
|
||
*gy = *gy - GyOffect;
|
||
*gz = *gz - GzOffect;
|
||
|
||
if (HmdStandByState == true){
|
||
GxTmp = GxTmp + *gx;
|
||
GyTmp = GyTmp + *gy;
|
||
GzTmp = GzTmp + *gz;
|
||
count++;
|
||
if (count == 3000){
|
||
//printf("陀螺仪校准完成\n");
|
||
|
||
GxOffectTmp = GxTmp / count;
|
||
GyOffectTmp = GyTmp / count;
|
||
GzOffectTmp = GzTmp / count;
|
||
|
||
GxOffect = GxOffect + GxOffectTmp;
|
||
GyOffect = GyOffect + GyOffectTmp;
|
||
GzOffect = GzOffect + GzOffectTmp;
|
||
|
||
GxTmp = 0.0f;
|
||
GyTmp = 0.0f;
|
||
GzTmp = 0.0f;
|
||
count = 0;
|
||
}
|
||
}
|
||
else{
|
||
count = 0;
|
||
GxTmp = 0.0f;
|
||
GyTmp = 0.0f;
|
||
GzTmp = 0.0f;
|
||
}
|
||
|
||
}
|
||
|
||
void LowPassFilterYaw(float *z, float alpha)
|
||
{
|
||
static float lastz = 0;
|
||
if (lastz == 0)
|
||
{
|
||
lastz = *z;
|
||
}
|
||
else
|
||
{
|
||
*z = alpha*lastz + (1 - alpha)*(*z);
|
||
lastz = *z;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
bool IsHmdIntoStandBy(float *Pitch, float *Roll, float dTime)
|
||
{
|
||
static float LastPitch = 0.0f, LastRoll = 0.0f;
|
||
static float LastGyroX = 0.0f, LastGyroY = 0.0f;
|
||
static int count = 0;
|
||
float GyroX = 0.0f, GyroY = 0.0f, alpha = 0.98f;
|
||
|
||
|
||
/* 初始化静态变量 */
|
||
if (LastPitch == 0.0f&&LastRoll == 0.0f){
|
||
LastPitch = *Pitch;
|
||
LastRoll = *Roll;
|
||
}
|
||
/*对Pitch和Roll微分，求Gx和Gy */
|
||
GyroX = fabsf(*Roll - LastRoll) / dTime;
|
||
GyroY = fabsf(*Pitch - LastPitch) / dTime;
|
||
/*初始化静态变量*/
|
||
if (LastGyroX == 0.0f&&LastGyroY == 0.0f){
|
||
LastGyroX = GyroX;
|
||
LastGyroY = GyroY;
|
||
}
|
||
/*进行一个滑动窗口滤波降低偶然误差*/
|
||
GyroX = alpha*LastGyroX + (1 - alpha)*GyroX;
|
||
GyroY = alpha*LastGyroY + (1 - alpha)*GyroY;
|
||
|
||
printf("%f %f\n", GyroX, GyroY);
|
||
|
||
if (GyroX < 0.0055f&&GyroY < 0.0015f){
|
||
count++;
|
||
if (count == 3000){
|
||
count = 0;
|
||
HmdStandByState = true;
|
||
printf("头显进入休眠\n");
|
||
}
|
||
}
|
||
else{
|
||
count = 0;
|
||
HmdStandByState = false;
|
||
}
|
||
/* 更新静态变量 */
|
||
LastRoll = *Roll;
|
||
LastPitch = *Pitch;
|
||
LastGyroX = GyroX;
|
||
LastGyroY = GyroY;
|
||
|
||
return HmdStandByState;
|
||
}
|
||
|
||
float DM_GetYaw(void)
|
||
{
|
||
return Yaw;
|
||
}
|
||
|
||
float DM_GetPitch(void)
|
||
{
|
||
return Pitch;
|
||
}
|
||
float DM_GetRoll(void)
|
||
{
|
||
return Roll;
|
||
}
|
||
|
||
|
||
|
||
|
||
Quaternion CorrectQuaterinonYaw(Quaternion q)
|
||
{
|
||
Quaternion qcorr;
|
||
qcorr.w = q.w; qcorr.x = 0.0f;
|
||
qcorr.y = q.y; qcorr.z = 0.0f;
|
||
qcorr = QuaternionInversion(qcorr);
|
||
float mod = QuaternionMod(qcorr);
|
||
qcorr.w = qcorr.w / mod;
|
||
qcorr.x = qcorr.x / mod;
|
||
qcorr.y = qcorr.y / mod;
|
||
qcorr.z = qcorr.z / mod;
|
||
return qcorr;
|
||
}
|
||
int ResetHeading(void)
|
||
{
|
||
Quaternion q = { GlobalQ.w, 0, GlobalQ.y, 0 };
|
||
CQ_DM = CorrectQuaterinonYaw(q);
|
||
DM_Corrected = true;
|
||
return DM_Corrected ? 1 : 0;
|
||
}
|
||
|
||
Quaternion GetPoseQuat(void)
|
||
{
|
||
static Quaternion QLast = { 1.0f, 0.0f, 0.0f, 0.0f };
|
||
if (!GlobalQ_Updating)
|
||
{
|
||
QLast.w = GlobalQ.w;
|
||
QLast.x = GlobalQ.x;
|
||
QLast.y = GlobalQ.y;
|
||
QLast.z = GlobalQ.z;
|
||
}
|
||
if (DM_Corrected){
|
||
return QuaternionMulti(QLast, CQ_DM);
|
||
}
|
||
return QLast;
|
||
} |