vect3D Quaternion_toEuler(Quaternion q) {
//http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
vect3D v;
int32 test = mulf32(q.x, q.y) + mulf32(q.z, q.w);
if(test > floatToFixed(0.499f, 12)) { // singularity at north pole
v.x = 2 * atan2Lerp(q.x, q.w);
v.y = floatToFixed(M_PI / 2.0f, 12);
v.z = 0;
return v;
}
if(test < floatToFixed(-0.499f, 12)) { // singularity at south pole
v.x = -2 * atan2Lerp(q.x, q.w);
v.y = floatToFixed(-M_PI / 2.0f, 12);
v.z = 0;
return v;
}
int32 sqx = mulf32(q.x, q.x);
int32 sqy = mulf32(q.y, q.y);
int32 sqz = mulf32(q.z, q.z);
v.x = atan2Lerp(2 * mulf32(q.y, q.w) - 2 * mulf32(q.x, q.z), (1 << 12) - 2 * sqy - 2 * sqz);
v.y = asinLerp(2 * test);
v.z = atan2Lerp(2 * mulf32(q.x, q.w) - 2 * mulf32(q.y, q.z), (1 << 12) - 2 * sqx - 2 * sqz);
return v;
}
void Signal_getEulerAnglesAccel( state_data_t *state, ImuData_t *measurments )
{
int32_t numerator = 0;
int32_t denominator = 0;
// roll calculation
// tan(roll) = y / (Sign(z) * sqrt(z²+(m*x²)))
numerator = (int32_t) measurments->imu_data[accl_y];
denominator = SIGN(measurments->imu_data[accl_z])
* (int32_t) my_square_root(
my_square( measurments->imu_data[accl_z], FP_16_16_SHIFT) +
my_mult( my_square( measurments->imu_data[accl_x], FP_16_16_SHIFT), MY, FP_16_16_SHIFT),
FP_16_16_SHIFT );
state->state_bf[roll_bf] = atan2Lerp( numerator, denominator, FP_16_16_SHIFT);
// Since atan2Lerp has a domain of [0,2pi), and we want a domain of (-pi,pi) we need
// to remove 2pi if value > pi.
if(state->state_bf[roll_bf] > BRAD_PI)
{
state->state_bf[roll_bf] -= BRAD_2PI;
}
// pitch calculation
// tan(pitch) = -x / sqrt(y² + z²)
numerator = (int32_t) (-measurments->imu_data[accl_x]);
denominator = (int32_t) my_square_root( my_square( measurments->imu_data[accl_y], FP_16_16_SHIFT ) +
my_square( measurments->imu_data[accl_z], FP_16_16_SHIFT ) , FP_16_16_SHIFT);
state->state_bf[pitch_bf] = atan2Lerp( numerator, denominator, FP_16_16_SHIFT);
// Since atan2Lerp has a domain of [0,2pi), and we want a domain of (-pi,pi) we need
// to remove 2pi if value > pi.
if(state->state_bf[pitch_bf] > BRAD_PI)
{
state->state_bf[pitch_bf] -= BRAD_2PI;
}
// Indicate which fields are updated and valid.
state->state_valid_bf |= 1 << roll_bf;
state->state_valid_bf |= 1 << pitch_bf;
}
