80 lines
2.4 KiB
C
80 lines
2.4 KiB
C
|
/*----------------------------------------------------------------------------------------------------
|
|||
|
|
# THIS FILE IS PART OF XEROLYSKINNER'S PROJECT
|
|||
|
|
# Quaternion - The core part of the project
|
|||
|
|
#
|
|||
|
|
# THIS PROGRAM IS FREE SOFTWARE
|
|||
|
|
#
|
|||
|
|
# Copyright (c) 2021 XerolySkinner
|
|||
|
|
# E-mail:ZABBCCCBBAZ@163.COM
|
|||
|
|
# QQ:2715099320
|
|||
|
|
#
|
|||
|
|
# All rights reserved.
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
/*----------------------------------------------------------------------------------------------------
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>:Quaternion
|
|||
|
|
ʱ<EFBFBD><EFBFBD>:20211229-2153
|
|||
|
|
<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ա:<EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD>
|
|||
|
|
˵<EFBFBD><EFBFBD>:<EFBFBD><EFBFBD>Ԫ<EFBFBD>ػ<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////
|
|||
|
|
//----------------------------------------------------------------------------------------------------
|
|||
|
|
// ͷ<>ļ<EFBFBD>
|
|||
|
|
#include "Quaternion.h"
|
|||
|
|
#include "math.h"
|
|||
|
|
|
|||
|
|
#define Kp 100.0f//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٶȼ<D9B6>/<2F><>ǿ<EFBFBD><C7BF>
|
|||
|
|
//#define Ki 0.002f//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD>ν<EFBFBD>
|
|||
|
|
#define Ki 0.020f//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>֧<EFBFBD><D6A7><EFBFBD>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƫ<EFBFBD><C6AB><EFBFBD><EFBFBD><EFBFBD>ν<EFBFBD>
|
|||
|
|
#define halfT 0.005f//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڵ<EFBFBD>һ<EFBFBD><D2BB>
|
|||
|
|
|
|||
|
|
float q0=0.1,q1=0.1,q2=0.1,q3=0.1; //<2F><>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʒ<EFBFBD><C6B7><EFBFBD>
|
|||
|
|
float exInt=0.1,eyInt=0.1,ezInt=0.1; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float Yaw=0,Pitch=0,Roll=0; //ƫ<><C6AB><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>,<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
float KalmanPitch;
|
|||
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////
|
|||
|
|
//----------------------------------------------------------------------------------------------------
|
|||
|
|
// <09><><EFBFBD><EFBFBD>
|
|||
|
|
//
|
|||
|
|
float IMUupdate(float gx,float gy,float gz,float ax,float ay,float az){
|
|||
|
|
float norm;
|
|||
|
|
float vx,vy,vz;
|
|||
|
|
float ex,ey,ez;
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
norm=sqrt(ax*ax+ay*ay+az*az);
|
|||
|
|
ax=ax/norm;//<2F><>λ<EFBFBD><CEBB>
|
|||
|
|
ay=ay/norm;
|
|||
|
|
az=az/norm;
|
|||
|
|
//<2F><><EFBFBD>Ʒ<EFBFBD><C6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
vx=2*(q1*q3-q0*q2);
|
|||
|
|
vy=2*(q0*q1+q2*q3);
|
|||
|
|
vz=q0*q0-q1*q1-q2*q2+q3*q3;
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD><CDB7><EFBFBD><F2B4ABB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ο<EFBFBD><CEBF><EFBFBD><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD>˻<EFBFBD><CBBB><EFBFBD><EFBFBD>ܺ<EFBFBD>
|
|||
|
|
ex=(ay*vz-az*vy);
|
|||
|
|
ey=(az*vx-ax*vz);
|
|||
|
|
ez=(ax*vy-ay*vx);
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
exInt=exInt+ex*Ki;
|
|||
|
|
eyInt=eyInt+ey*Ki;
|
|||
|
|
ezInt=ezInt+ez*Ki;
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Dz<EFBFBD><C7B2><EFBFBD>
|
|||
|
|
gx=gx+Kp*ex+exInt;
|
|||
|
|
gy=gy+Kp*ey+eyInt;
|
|||
|
|
gz=gz+Kp*ez+ezInt;
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD>ʺ<EFBFBD><CABA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
q0=q0+(-q1*gx-q2*gy-q3*gz)*halfT;
|
|||
|
|
q1=q1+(q0*gx+q2*gz-q3*gy)*halfT;
|
|||
|
|
q2=q2+(q0*gy-q1*gz+q3*gx)*halfT;
|
|||
|
|
q3=q3+(q0*gz+q1*gy-q2*gx)*halfT;
|
|||
|
|
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ
|
|||
|
|
norm=sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
|
|||
|
|
q0=q0/norm;
|
|||
|
|
q1=q1/norm;
|
|||
|
|
q2=q2/norm;
|
|||
|
|
q3=q3/norm;
|
|||
|
|
Pitch=asin(-2*q1*q3+2*q0*q2)*57.3;//pitch,ת<><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>
|
|||
|
|
//Roll=atan2(2*q2*q3+2*q0*q1,-2*q1*q1-2*q2*q2+1)*57.3;//rollv
|
|||
|
|
//Yaw=atan2(2*(q1*q2+q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3;//<2F>˴<EFBFBD>û<EFBFBD>м<EFBFBD>ֵ,ע<><D7A2>
|
|||
|
|
return Pitch;}
|