[问答]

请问YAW的公式怎么写？

800

问答对人有帮助，内容完整，我也想知道答案 0 #include #include #include #define pi 3.14159265f #define Kp 1.5f #define Ki 0.5f #define halfT 0.004f float q0=1,q1=0,q2=0,q3=0; float exInt=0,eyInt=0,ezInt=0; double Angle,Angley; void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; float q0q0 = q0q0; float q0q1 = q0q1; float q0q2 = q0q2; float q0q3 = q0q3; float q1q1 = q1q1; float q1q2 = q1q2; float q1q3 = q1q3; float q2q2 = q2q2; float q2q3 = q2q3; float q3q3 = q3q3; norm = sqrt(axax + ayay + azaz); ax = ax /norm; ay = ay / norm; az = az / norm; vx = 2(q1q3 - q0q2); vy = 2(q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3 ; ex = (ayvz - azvy) ; ey = (azvx - axvz) ; ez = (axvy - ayvx) ; exInt = exInt + ex Ki; eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; gx = gx + Kpex + exInt; gy = gy + Kpey + eyInt; gz = gz + Kpez + ezInt; q0 = q0 + (-q1gx - q2gy - q3gz)halfT; q1 = q1 + (q0gx + q2gz - q3gy)halfT; q2 = q2 + (q0gy - q1gz + q3gx)halfT; q3 = q3 + (q0gz + q1gy - q2gx)halfT; norm = sqrt(q0q0 + q1q1 + q2q2 + q3q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; Angle=asin(2(q0q2-q1q3 ))* 57.2957795f; // ¸©Ñö Angley=asin(2(q0q1+q2q3 )) 57.2957795f; // oá1ö // imu.yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2q2 - 2 q3* q3 + 1)* 57.3; // yaw // imu.yaw = atan2(2(q1q2 + q0q3),q0q0+q1q1-q2q2-q3q3) 57.3; //yaw } 0
2019-5-17 03:26:54　　评论淘帖0 邀请回答您可以邀请以下用户，快速回答问题 × wanqing192 该类别下有 6 个回答。邀请回答 zsjjx 该类别下有 5 个回答。邀请回答 raylue 该类别下有 5 个回答。邀请回答 xiezhe272 该类别下有 5 个回答。邀请回答 wyerwwr 该类别下有 5 个回答。邀请回答 ZQW发烧友该类别下有 4 个回答。邀请回答 60user160 该类别下有 4 个回答。邀请回答 mashiquan123 该类别下有 4 个回答。邀请回答 tunkanxi8082 该类别下有 4 个回答。邀请回答 moxlanlan 该类别下有 4 个回答。邀请回答 ickey_feng 该类别下有 4 个回答。邀请回答报纸弟弟麦花该类别下有 4 个回答。邀请回答帅帅的欧尼酱该类别下有 4 个回答。邀请回答红粉小Q猪该类别下有 4 个回答。邀请回答 hjfjsdgfjdsf 该类别下有 4 个回答。邀请回答 dfgsgdg 该类别下有 4 个回答。邀请回答泰坦之心该类别下有 4 个回答。邀请回答 zhushanyu262 该类别下有 4 个回答。邀请回答 zweipcb 该类别下有 4 个回答。邀请回答 xujun621 该类别下有 4 个回答。邀请回答举报黄明光相关推荐 • 写公式时出现HB未定义，请问怎么去解决？ 752 • 请问yaw的pid怎么调？ 1949 • HMC5883计算偏航角yaw会跟着roll一起变 3309 • 请问mpu9250 DMP初始化怎么确定yaw角？ 3432 • 请问四轴需要精确的yaw角吗？ 1714 • 请问yaw角指向哪个方向是0°？ 7173 • 请问姿态生成后的yaw角一直漂，yaw角的漂移对控制四轴的稳飞会有什么影响？ 5588 • 公式节点编写PID 2394 • 请问罗盘方向校正成什么效果算好？ 1259 • 请问yaw会飘的原因是什么？ 3495 1个回答