I am very confused by the earth to body frame conversion code when I compare it with the frame conversion matrix that I found on the pixhawk website. Could someone please help me out… I cannot believe how much time I have wasted on this… Many thanks!
What are the exact definitions of “earth frame” and “body frame”?
earth frame = inertial frame?
body frame = body-NED frame? Or body-fixed frame?
The conversion code is here:
// earth-frame <-> body-frame conversion functions
// frame_conversion_ef_to_bf - converts earth frame vector to body frame vector
void AC_AttitudeControl::frame_conversion_ef_to_bf(const Vector3f& ef_vector, Vector3f& bf_vector)
// convert earth frame rates to body frame rates
bf_vector.x = ef_vector.x - _ahrs.sin_pitch() * ef_vector.z;
bf_vector.y = _ahrs.cos_roll() * ef_vector.y + _ahrs.sin_roll() * _ahrs.cos_pitch() * ef_vector.z;
bf_vector.z = -_ahrs.sin_roll() * ef_vector.y + _ahrs.cos_pitch() * _ahrs.cos_roll() * ef_vector.z;
The frame conversion matrix that I am referring to is here:
Based on this matrix, I was expecting something like (take the x-axis as an example):
bf_vector.x = _ahrs.cos_pitch() * ef_vector.x - _ahrs.sin_pitch() * ef_vector.z;
Thanks for reading the post~~