[经验]

飞控姿态解算中，欧拉角与四元数之间的转换

小狼

2016-11-17 13:09:34

6236

//欧拉角转四元数

void cjx_from_euler(float roll, float pitch, float yaw)

{

float cr2 = cosf(roll*0.5f);

float cp2 = cosf(pitch*0.5f);

float cy2 = cosf(yaw*0.5f);

float sr2 = sinf(roll*0.5f);

float sp2 = sinf(pitch*0.5f);

float sy2 = sinf(yaw*0.5f);

q1 = cr2*cp2*cy2 + sr2*sp2*sy2;

q2 = sr2*cp2*cy2 - cr2*sp2*sy2;

q3 = cr2*sp2*cy2 + sr2*cp2*sy2;

q4 = cr2*cp2*sy2 - sr2*sp2*cy2;

}

#define RAD_TO_DEG 57.29577951f

//弧度转角度

float degrees(float rad)

{

return rad * RAD_TO_DEG;

}

//四元数转欧拉角

void cjx_to_euler(float *roll, float *pitch, float *yaw)

{

if (roll) {

*roll = degrees(atan2f(2.0f*(q1*q2 + q3*q4),1 - 2.0f*(q2*q2 + q3*q3)));

}

if (pitch) {

// 使用safe_asin()来处理pitch接近90/-90时的奇点

*pitch = degrees(safe_asin(2.0f*(q1*q3 - q2*q4)));

}

if (yaw) {

*yaw = degrees(atan2f(2.0f*(q2*q3 - q1*q4), 2.0f*(q1*q1 + q2*q2) - 1));

}

只看该作者 淘帖0 举报

精选推荐

STM32F103上位机升级，RS485升级、CAN升级

2135 浏览 0 评论
STM32配合可编程加密芯片SMEC88ST的防抄板加密方案设计

1220 浏览 0 评论
记录我的stm32开发学习过程

2883 浏览 1 评论
AD7686芯片不传输数据给STM32，但是手按住就会有数据。

2643 浏览 3 评论
有什么比较热门的嵌入式项目？

5329 浏览 0 评论

快速回复 返回顶部 返回列表

关注微信公众号

电子发烧友网

电子发烧友william hill官网

社区合作: 刘勇; 联系电话：15994832713; 邮箱地址：liuyong@huaqiu.com

社区管理: elecfans短短; 微信：elecfans_666; 邮箱：users@huaqiu.com

返回 STM32/STM8技术william hill官网

搜索历史

飞控姿态解算中，欧拉角与四元数之间的转换

相关推荐

评论

精选推荐

站长推荐 /5