LSM303AGR - conversion accelerometer's and magnetometer's output to pitch, roll and yaw.

[Wojciech Jasiewicz]

Posted on March 01, 2018 at 10:29

Hello, I have a problem with conversion accelerometer's and magnetometer's output to pitch, roll and yaw angles. I am trying to use equations from DT0058. I wrote function like below.

void newPitchRollYaw(float* const acc, float* const mag)

{ float roll = atan2(acc[LSM303AGR_Y], acc[LSM303AGR_Z] + acc[LSM303AGR_X] * 0.01); float pitch = atan(-acc[LSM303AGR_X] / (acc[LSM303AGR_Y] * sin(roll) + acc[LSM303AGR_Z] * cos(roll)));

float By2 = mag[LSM303AGR_Z] * sin(roll) - mag[LSM303AGR_Y] * cos(roll);

float Bz2 = mag[LSM303AGR_Y] * sin(roll) + mag[LSM303AGR_Z] * cos(roll); float Bx3 = mag[LSM303AGR_X] * cos(pitch) + Bz2 * sin(pitch);

float yaw = atan2( By2 , Bx3);

float factor = LSM303AGR_180_DEGREES / LSM303AGR_PI;

log_PushLine(e_logLevel_Info, 'pitch: %f , roll: %f , yaw: %f', pitch * factor , roll * factor , yaw * factor);

float Qw = cos(roll/2)*cos(pitch/2)*cos(roll/2) + sin(roll/2)*sin(pitch/2)*sin(roll/2);

float Qx = sin(roll/2)*cos(pitch/2)*cos(roll/2) - cos(roll/2)*sin(pitch/2)*sin(roll/2); float Qy = cos(roll/2)*sin(pitch/2)*cos(roll/2) + sin(roll/2)*cos(pitch/2)*sin(roll/2); float Qz = cos(roll/2)*cos(pitch/2)*sin(roll/2) - sin(roll/2)*sin(pitch/2)*cos(roll/2);

log_PushLine(e_logLevel_Info, 'qw: %f , qx: %f , qy: %f , qz: %f', Qw * factor, Qx * factor, Qy * factor, Qz * factor);

}

   When compass rotates around z-axis it changes yaw value but very irregularly, for example from 130 to 40 and then form -100 to - 30 etc. When I change only pitch or roll value, yaw is also changed. I think that it's not correct, because  device's direction is no changed. For example if pitch is 1, yaw is -50, if pitch is 14 yaw = -120. I don't understand these results. I think that pitch and roll are correct. 

   I use accelerometer in normal mode, full scale default ( +- 2g), odr = 400 Hz, magetometer in continuous mode, 100Hz. I don't use interrupts, only check data available.

Thanks for all answers. 

[maurizio23]

Posted on March 28, 2018 at 16:47

HI,

I can tell you some suggestion based on my experience with LSM303D and LSM303DLHC.

1) Have you make the magnetometer calibration before start with measures?

2) seem you haven't make all the test for border limit explained in application note for trigonometric functions.

You wrote: 'When compass rotates around z-axis it changes yaw value but very irregularly'

Answer: this is not correct. I confirm that this must change only yaw.

I personally move the processing on Scilab or Matlab from raw data to examine if data are good for yaw, pitch roll calculations.

I suggest to search for discovery-stm32f3 where there is a source code to calculate Heading=Yaw from LSM303DLHC that is the same procedure.

Regards

Maurizio

[Miroslav BATEK]

Posted on March 29, 2018 at 15:00

If you don't want to spend time with the algorithm development you can use already existing eCompass library which we provide in

http://www.st.com/content/st_com/en/products/embedded-software/mcus-embedded-software/stm32-embedded-software/stm32cube-expansion-packages/x-cube-mems1.html

package.

Here is the user manual for the

http://www.st.com/content/ccc/resource/technical/document/user_manual/group0/3e/50/50/0b/9d/3f/47/2d/DM00396079/files/DM00396079.pdf/jcr:content/translations/en.DM00396079.pdf

libary.

[Wojciech Jasiewicz]

Posted on June 29, 2018 at 11:06

Hello,

I collected raw data from magnetometer 2 times ( first time with my own library and then with XNucleoIKS01A2 library from this example 

https://os.mbed.com/teams/ST/code/HelloWorld_IKS01A2/

 ). Datas are very similar in both cases (expressed in gauss). Compass was rotated around Z axis, step by step, by several degrees. Compass was at horizontal position with 0 titl angle.

It looks like sine and cosine functions, but with movements. I tried to remove these movements and after I had following data. 

And then I tried to transform it in

ellipse

 figure with following equations:

because I want to reached result, which is similar to this:

These pictures origin from this document: 

https://aerospace.honeywell.com/en/~/media/aerospace/files/technical-articles/applicationsofmagnetoresistivesensorsinnavigationsystems_ta.pdf

 

I need ellipse to compensate ''hard iron'' (offsets for register) and ''soft iron''(factors for converting ellipse to circle). These steps provide clear calibrate data for MotionEC library and this library will counts necessary data for me.

In my opinion, calibration based on raw data collecting,  processing it's to ellipse form, counting offsets (hard iron) and counting ellipse's factories (for making circle). Are followings steps correct? Is self test necessary to magnetometer's calibrating? 

In API MotionEC library, in MEC_input_t structure description, I read that ''

Mag[3] is an array of magnetometer calibrated data in ENU convention, measured in ?T/50'', hence I need to convert data manually.  Why period of data reading is required? 

I described my reasoning  and my doubts. In need explanations, step by step, how to get correct raw data for high level library, for example MotionEC. Previously, I implemented my library for accelerometer, without any problems, without calibration, etc. 

Thanks 

Maurizio and Miroslav B. for your's previous replies.

Best regards. Wojciech Jasiewicz.