Really confused by the earth frame to body frame conversion code...

[Yang Wang]

Hi all~

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!

I am assuming "earth frame" = NED, and "body frame" = forward-right-down. Is it correct? If I am correct, then I don't think the conversion code makes sense...

The conversion code is here:

https://github.com/diydrones/ardupilot/blob/ArduCopter-3.2.1/librar...

Line 425

// 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:

http://www.pixhawk.org/dev/know-how/frames_of_reference

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;

This is not even counting the YAW...

Thanks for reading the post~~

[Jonathan Challinger]

The nomenclature is confusing at best, plain wrong at worst. I think that this "frame conversion" is really a conversion to/from Euler angles.

--
You received this message because you are subscribed to the Google Groups "drones-discuss" group.
To unsubscribe from this group and stop receiving emails from it, send an email to drones-discus...@googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Yang Wang]

Hi Jonathon,
Thanks for getting back to me. Sorry I'm not quite following you... Do you mean that I have misunderstood the code? This is actually what I would expect. But do you know where I can find any document that would explain this conversion, or the definitions of the ef and bf in this conversion?
Thanks a lot for the help:)

[Randy Mackay]

Yang,

The earth-frame to body-frame conversion converts the angles or rates from the IMU's point of view, into a forward-right-down frame. It doesn't include a heading change from the vehicle's heading to north. So if the vehicle/IMU is perfectly level the conversion doesn't change anything but if the vehicle is leaning it's important. Normally the pilot or position controllers will request an absolute earth frame roll or pitch angle but the gyros provide the vehicle's rotating rates from the IMU's point of view and the motors provide rotational force in the IMU/body's frame. It's this difference in the request angles vs sensor and motor output that requires this conversion here and there.

Not sure it's totally clear but hope that helps a bit.

-Randy

[Yang Wang]

Hi Randy,

Thank you very much for your detailed explanation! I feel I am really close to the answer, and I hope that you could help me a bit more :) Really appreciate it!

I am very puzzled what is the point of view of the IMU? The way that I look at the conversion is by using the conversion matrix from the following link (near bottom of the page) and setting YAW = 0:
http://www.pixhawk.org/dev/know-how/frames_of_reference

However, setting YAW=0 would NOT lead to the conversion in the code:

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;
}

Therefore, my understanding is that the IMU has a unique point of view that is different from the common frames. And judging from the code, it seems the x-axis is NOT orthogonal to y and z-axis, which is unheard of to me...

Thanks again for your time!

-Yang

You received this message because you are subscribed to a topic in the Google Groups "drones-discuss" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/drones-discuss/PiPvKGSf_tU/unsubscribe.
To unsubscribe from this group and all its topics, send an email to drones-discus...@googlegroups.com.

For more options, visit https://groups.google.com/d/optout.

--

Yang Wang

[Kristian Klausen]

Keep in mind, that this is for transforming angular rates, not translational vectors. One can derive the code in a way similar to this (this is for a zyx-convention and NED)

[Yang Wang]

Hi Kristian,

Thanks! Now very clear where I got it wrong... Helped a lot

- Yang