logzhan
/
GeekIMU
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
GeekIMU/2.Firmware/STM32/Firmware/sensor/ahrs.c

167 lines
6.6 KiB
C
Raw Normal View History

上传GeekIMU资料
2024-11-09 21:39:20 +08:00
/******************** (C) COPYRIGHT 2020 GEEKIMU *******************************
* File Name : ahrs.c
* Current Version : V2.0 & ST 3.5.0
* Author : zhanli 719901725@qq.com & JustFeng.
* Date of Issued : 2019.5.26 zhanli : Create
* Comments : <EFBFBD><EFBFBD><EFBFBD>ٶȡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̬
********************************************************************************/
#include "ahrs.h"
#include "sys.h"
#include "math.h"
#include "time.h"
#include "usb_lib.h"
extern uint8_t packet_buf[64];
extern int NumOfSensor;
float q0 = 1.0f;
float q1 = 0.0f;
float q2 = 0.0f;
float q3 = 0.0f;
float exInt = 0.0f;
float eyInt = 0.0f;
float ezInt = 0.0f;
#define Kp 2.5f /*<2A><><EFBFBD>ٶȼƺʹ<C6BA><CDB4><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵı<C7B5><C4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
#define Ki 0.005f /*<2A><><EFBFBD>ٶȼƺʹ<C6BA><CDB4><EFBFBD><EFBFBD>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǵĻ<C7B5><C4BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
#define Kd 0.0f /*<2A><><EFBFBD>Ż<EFBFBD><C5BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
/**----------------------------------------------------------------------
* Function : AHRSupdate
* Description : <EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><EFBFBD><EFBFBD> 1/Sqrt(x)<EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0x5f3759df
* Input : <EFBFBD><EFBFBD><EFBFBD>ٶȼơ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǡ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֵ
* Output : <EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD> & ŷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
* Author : zhanli&719901725@qq.com
* Date : 2016/11/5 : <EFBFBD><EFBFBD><EFBFBD>ݴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ʋ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱȽϵ͵<EFBFBD><EFBFBD>ص㣬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2<EFBFBD><EFBFBD>ģʽ<EFBFBD><EFBFBD>
1<EFBFBD><EFBFBD><EFBFBD>ںϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 2<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ںϴ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2016/11/6 <EFBFBD><EFBFBD><EFBFBD>Ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD>ԭ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD>
<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ۼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
2016/11/9 : <EFBFBD>涨<EFBFBD>˱<EFBFBD><EFBFBD>㷨<EFBFBD><EFBFBD><EFBFBD>е<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ᣬ<EFBFBD><EFBFBD>д<EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><EFBFBD>
<EFBFBD>DZ任<EFBFBD>Ĵ<EFBFBD><EFBFBD>롣
*---------------------------------------------------------------------**/
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az,
float mx, float my, float mz,char useMag, char UseTimer)
{
/*ԭ<>㷨<EFBFBD>Ķ<EFBFBD><C4B6><EFBFBD>*/
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;
/*<2A><><EFBFBD><EFBFBD>֮<EFBFBD><D6AE><EFBFBD>ij<EFBFBD><C4B3><EFBFBD>ʹ<EFBFBD>ã<EFBFBD><C3A3><EFBFBD><EFBFBD>ټ<EFBFBD><D9BC><EFBFBD>ʱ<EFBFBD><CAB1>*/
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;
norm = invSqrt(ax*ax + ay*ay + az*az); /*<2A>Ѽ<EFBFBD><D1BC>ٶȼ<D9B6><C8BC><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>*/
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; /*<2A>Ѵ<EFBFBD><D1B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>Ϊ<EFBFBD><CEAA>λ<EFBFBD><CEBB><EFBFBD><EFBFBD> */
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)); /*<2A><>(mx,my,mz)ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5>(hx,hy,hz),<2C><><EFBFBD><EFBFBD>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));
/*<2A>Ƶ<EFBFBD><C6B5><EFBFBD>ֵΪ(bx,by,0),<2C><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Dz<EFBFBD><C7B2><EFBFBD>עhx<68><78><EFBFBD><EFBFBD>ֵ */
// bx = sqrtf((hx*hx) + (hy*hy)); /*ʹ<><CAB9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
by = sqrtf((hx*hx) + (hy*hy));
bz = hz;
/*v<><76><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰѵ<C7B0><D1B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ļ<EFBFBD><C4BC>ٶ<EFBFBD>(0,0,1g)ת<><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD>ļ<EFBFBD> */
vx = 2.0f*(q1q3 - q0q2); /*<2A>ٶȼ<D9B6>(ax,ay,az),<2C><>ʵ<EFBFBD><CAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>R*(0,0,1),RΪ<52><CEAA>ת<EFBFBD><D7AA><EFBFBD>󣬴˾<F3A3ACB4><CBBE><EFBFBD><EFBFBD><EFBFBD> */
vy = 2*(q0q1 + q2q3); /*<2A><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>ת<EFBFBD><D7AA><EFBFBD>õ<EFBFBD> */
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2.0f*(by*(q1q2 + q0q3) + bz*(q1q3 - q0q2)); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>H<EFBFBD><48>B<EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵת<CFB5><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD>W = Q * B */
wy = 2.0f*(by*(0.5f - q1q1 - q3q3) + bz*(q0q1 + q2q3)); /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊbx = 0 */
wz = 2.0f*(by*(q2q3 - q0q1) + bz*(0.5f - q1q1 - q2q2));
if(useMag != 0) /*<2A><><EFBFBD><EFBFBD>ʹ<EFBFBD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD>ƣ<EFBFBD><C6A3><EFBFBD><EFBFBD>ںϴ<DABA><CFB4><EFBFBD><EFBFBD><EFBFBD>*/
{
ex = AccelAjust*(ay*vz - az*vy) + MagAjust*(my*wz - mz*wy); /*<2A>õ<EFBFBD>ǰ<EFBFBD><C7B0>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC>ٶȵ<D9B6><C8B5><EFBFBD>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ˣ<EFBFBD><CBA3>˻<EFBFBD>ԽС˵<D0A1><CBB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD><D4BD>ͬ*/
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 = AccelAjust*(ax*vy - ay*vx);
/*<2A><>Ϊ<EFBFBD><CEAA><EFBFBD>ٶȼ<D9B6>û<EFBFBD><C3BB>У׼<D0A3><D7BC>ez<65><7A><EFBFBD><EFBFBD><EFBFBD><EFBFBD>*/
}
if(UseTimer == 1) {
halfT = Get_NowTime(); /*<2A>õ<EFBFBD>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>̬<EFBFBD><CCAC><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ڵ<EFBFBD>һ<EFBFBD><D2BB>t = (1/SampleFrq) * 0.5 */
}else{
halfT = 0.1;
}
if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
{
/*<2A><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9><EFBFBD><EFBFBD>PID<49><44><EFBFBD>ڵķ<DAB5>ʽ<EFBFBD><CABD><EFBFBD>Խ<EFBFBD><D4BD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD>,Ki <20>ǻ<EFBFBD><C7BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Kp<4B><70>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */
exInt = exInt + ex*Ki * halfT; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>л<EFBFBD><D0BB><EFBFBD> */
eyInt = eyInt + ey*Ki * halfT;
ezInt = ezInt + ez*Ki * halfT;
gx = gx + Kp*(1 + Kd*fabs(ex))*ex + exInt; /*ʹ<>ñ<EFBFBD><C3B1><EFBFBD><EFBFBD>ͻ<EFBFBD><CDBB><EFBFBD><EFBFBD>ۺ϶<DBBA><CFB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǽ<EFBFBD><C7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڴ<EFBFBD><DAB4>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD><D4B2><EFBFBD>ȷ<EFBFBD><C8B7><EFBFBD><EFBFBD> */
gy = gy + Kp*(1 + Kd*fabs(ey))*ey + eyInt; /*<2A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD>λ<EFBFBD><CEBB> */
gz = gz + Kp*(1 + Kd*fabs(ez))*ez + ezInt;
}
// <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD>
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;
// <20><><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA><EFBFBD><EFBFBD><EFBFBD>й淶<D0B9><E6B7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ԫ<EFBFBD><D4AA>ģΪ1
norm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 * norm; //w
q1 = q1 * norm; //x
q2 = q2 * norm; //y
q3 = q3 * norm; //z
}
/**----------------------------------------------------------------------
* Function : invSqrt
* Description : <EFBFBD><EFBFBD><EFBFBD>ټ<EFBFBD><EFBFBD><EFBFBD> 1/Sqrt(x)<EFBFBD><EFBFBD>Դ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>3<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>0x5f3759df
* Author : zhanli&719901725@qq.com
* Date : 2019/5/26 zhanli
*---------------------------------------------------------------------**/
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;
}