crazyflie-firmware sensfusion6GetEulerRPY funtion issue

Firmware/software/electronics/mechanics
Post Reply
justinleeyang
Expert
Posts: 186
Joined: Mon Dec 28, 2015 5:07 am

crazyflie-firmware sensfusion6GetEulerRPY funtion issue

Post by justinleeyang »

Hi, All:
I am interesting in the crazyflie2.0, so try analyze your code. the funtion get the Euler form quatertion:
void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw)
{
float gx, gy, gz; // estimated gravity direction

gx = 2 * (q1*q3 - q0*q2);
gy = 2 * (q0*q1 + q2*q3);
gz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

if (gx>1) gx=1;
if (gx<-1) gx=-1;

*yaw = atan2f(2*(q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180 / M_PI_F;
*pitch = asinf(gx) * 180 / M_PI_F; //Pitch seems to be inverted
*roll = atan2f(gy, gz) * 180 / M_PI_F;
}
but I feel should be the following code, Z axis as Yaw, X axis as Pitch and Y axis as Roll.
void sensfusion6GetEulerRPYNormal(float* roll, float* pitch, float* yaw)
{
*yaw = -atan2f(2*(-q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180 / M_PI_F;
*pitch = asinf(2*q2*q3 + 2*q0*q1) * 180 / M_PI_F; //Pitch seems to be inverted
*roll = -atan2f((2*q1*q3-2*q0*q2), q0*q0 + q3*q3 - q1*q1 - q2*q2) * 180 / M_PI_F;
}
when try the code, crazyflie PC client display X and Y axis switch.
please help, Thanks!
Post Reply