陀螺仪pitch的奇点问题

最新推荐文章于 2022-03-14 20:42:57 发布

花开雨下

最新推荐文章于 2022-03-14 20:42:57 发布

阅读量349

点赞数

分类专栏： esp32

本文链接：https://blog.csdn.net/weixin_48628854/article/details/118878410

版权

esp32 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

最近在用陀螺仪给我的网络时钟做交互，我用的是6050的dmp库看别人直接把四元数转欧拉角，当我拿器我的时钟玩耍时一旦动作过于激烈会出现各种问题。通过在网上的不断搜索学习我寻找到了一个办法，求两个四元数的差通过差求欧拉角我只需要在我的程序里不断更新减数与被减数就不会有pitch的问题了思路是这样。

一个四元数乘另一个四元数的共轭就是差了然后就可以取出旋转角

这里是代码最后会放上我代码的思路来源。
闲的蛋痛用数组指针折磨一下观众…

#include <stdio.h>
#include <math.h>

void Quaternion_Err(float (*q)[4],float* p);
void Quaternion_Euler(float* p,float* q);

int main(void)
{
	int i,j;
	float a[2][4] = {{1,0,0,0},{0.766,0,0.643,0}};
	float b[4] = {0};
	float c[3] = {0};
	Quaternion_Err(a,b);
	Quaternion_Euler(b,c);
	return 0;
}

//保存两个四元数的二位数组
void Quaternion_Err(float (*q)[4],float* p)
{
	int i;
	for(i=1;i<4;i++)
	{
		*(*(q+1)+i) = -*(*(q+1)+i); //共轭
	}
	printf("\n");
	*p = (*(*(q)))*(*(*(q+1)))-(*(*(q)+1))*(*(*(q+1)+1))-(*(*(q)+2))*(*(*(q+1)+2))-(*(*(q)+3))*(*(*(q+1)+3));//w
	*(p+1) =(*(*(q)))*(*(*(q+1)+1))+(*(*(q+1)))*(*(*(q)+1))+(*(*(q)+2))*(*(*(q+1)+3))-(*(*(q+1)+2))*(*(*(q)+3));//i
	*(p+2) =(*(*(q)))*(*(*(q+1)+2))+(*(*(q+1)))*(*(*(q)+2))+(*(*(q)+3))*(*(*(q+1)+1))-(*(*(q+1)+3))*(*(*(q)+1));//j
	*(p+3) =(*(*(q)))*(*(*(q+1)+3))+(*(*(q+1)))*(*(*(q)+3))+(*(*(q)+1))*(*(*(q+1)+2))-(*(*(q+1)+1))*(*(*(q)+2));//k
	printf("%0.5f %0.5f %0.5f %0.5f\n",*p,*(p+1),*(p+2),*(p+3));
}

void Quaternion_Euler(float* p,float* q)
{
	*q = asin(-2 * (*(p+1)) * (*(p+3)) + 2 * (*p)* (*(p+2)))* 57.3;//pitch
	*(q+1) = atan2(2 * (*(p+2)) * (*(p+3)) + 2 * (*p) * (*(p+1)), -2 * (*(p+1)) * (*(p+1)) - 2 * (*(p+2))* (*(p+2)) + 1)* 57.3;//roll
	*(q+2) = atan2(2*((*(p+1))*(*(p+2)) + (*p)*(*(p+3))),(*p)*(*p)+(*(p+1))*(*(p+1))-(*(p+2))*(*(p+2))-(*(p+3))*(*(p+3)))* 57.3; //yaw
	printf("pitch:%0.5f   roll:%0.5f   yaw:%0.5f\n",*q,*(q+1),*(q+2));
}