你的浏览器版本过低,可能导致网站不能正常访问!为了你能正常使用网站功能,请使用这些浏览器。
举报
查看全部评分
feixiang20 发表于 2018-4-19 00:10 要么去看看基于STM32F4的四轴航拍飞行器(开源分享),几个基于STM32的开源无人机飞控,STM32F103T8U6 +MPU60 ...
Your UAT Feedback is needed within today.
5.18活动板子会不会是STM32L4呢
STM32隐藏
STM32 CUBE软件中找不到STM32L4系列单片机
应用
AAA
tiwen
BBBB
aaa
2222
包括
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float delta_2=0;
const static float FACTOR = 0.002;
float norm=0.0f;
float vx, vy, vz;
float ex, ey, ez;
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q1q1 = q1*q1;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
norm = sqrt(ax*ax + ay*ay +az*az); // 测量正常化 把加速度计的三维向量转成单位向量。
ax = ax /norm;
ay = ay / norm;
az = az / norm;
vx = 2*(q1q3 - q0q2); // 估计方向的重力
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
ex = (ay*vz - az*vy);
ey = (az*vx - ax*vz);
ez = (ax*vy - ay*vx);
halfT=0.002;
gx = gx + ex*FACTOR/halfT; //校正陀螺仪测量值 用叉积误差来做PI修正陀螺零偏
gy = gy + ey*FACTOR/halfT;
gz = gz + ez*FACTOR/halfT;
delta_2=(2*halfT*gx)*(2*halfT*gx)+(2*halfT*gy)*(2*halfT*gy)+(2*halfT*gz)*(2*halfT*gz);
q0 = (1-delta_2/8)*q0 + (-q1*gx - q2*gy - q3*gz)*halfT; // 整合四元数率 四元数微分方程 四元数更新算法,二阶毕卡法
q1 = (1-delta_2/8)*q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = (1-delta_2/8)*q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = (1-delta_2/8)*q3 + (q0*gz + q1*gy - q2*gx)*halfT;
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); // 正常化四元
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//转换为欧拉角
Q_ANGLE.Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Q_ANGLE.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 57.3; // yaw
}
资料里有详细参考
评分
查看全部评分
谢谢哈
应该有吧;