LSM303DLHC Magnetometer Help! POLOLU IMU

Hello All,

I have been working on an propeller based autopilot for a while now, and things have been coming along great. Although I was nervous about the lack of interrupts, the faster speed and multiple cogs have made timing the program efficient. I purchased the Pololu brand IMU v2 for $50, and my only complaint might be that the board is TOO small. I am implementing a complementary filter to filter all vibration noise, and it seems to be working just fine. 

I am able to set the attitude of the plane using a simple PID loop, and I am receiving data back from the airplane to my laptop via xbee. The only problem is: I cannot tilt-compensate the compass module. 

The LSM303DLHC contains both an accelerometer and magnetometer (I assume specifically for this purpose!) I am able to read the whole sensor using the I2C bus. 

I have tried both the algorithms found at these websites:

https://www.loveelectronics.co.uk/Tutorials/13/tilt-compensated-compass-arduino-tutorial

https://gist.github.com/timtrueman/322555

Before implementing the tilt-compensation, I normalize the magnetometer values by find the total magnitude:

r = sqrt( magX ^2 + magY^2 + magZ^2)

and then divinding each component by magnitude.

However, after using my atan2 function, I do not get a full 360 degrees!!

I don't know if my IMU is orientated correctly:

   X-axis  -> towards nose of plane

   Y-axis -> towards right wingtip

   Z-axis  -> downwards

While this reference frame is used in the first link above ^^ it is not what I am used to from math class lol

My yaw value stays center at about 170 degrees and -170. It actually changes very little as I spin the plane horizontally, however, the value scales up and down as I change the pitch and roll of the plane.

If i don't normalize the magnetometer vectors, I get a value between 60-120 for my yaw while spinning it level, but again is not tilt-compensated.

What is more confusing, if i use atan2(mz,mx) and add 360 if its less than 0, i get a full 360 degrees while spinning the plane horizontally. The only problem is that its not tilt compensated...

Any ideas on this one guys? Has anyone succesfully tilt compensated this magnetometer before? I do not understand why this algorithm is not working, we only care about the planes direction in the xy plane, so using atan2 makes sense enough. 

Here is a clip of the code I am using (I apologize ahead of time, float math is more troublesome then i would like on the propeller chip)::

mx := fm.ffloat(IMU.getMX)

my := fm.ffloat(IMU.getMY)

mz := fm.ffloat(IMU.getMZ)

mv := fm.fsqr(fm.fadd(fm.fadd(fm.fmul(mx,mx),fm.fmul(my,my)),fm.fmul(mz,mz)))

pitchRad := fm.radians(pitch)

rollRad := fm.radians(roll)

mx := fm.fdiv(mx,mv)

my := fm.fdiv(my,mv)

mz := fm.fdiv(mz,mv)

magY := fm.FSub(fm.fadd(fm.fmul(fm.fmul(fm.cos(pitchRad),mx),fm.sin(pitchRad)),fm.fmul(fm.cos(rollRad),my)),fm.fmul(fm.fmul(fm.sin(rollRad),mz),fm.cos(pitchRad)))

//sorry for how ugly this term is ^^^

magX := fm.fadd(fm.fmul(fm.cos(pitchRad),mx),fm.fmul(fm.sin(pitchRad),mz))

yaw := fm.degrees(atan2(magY, magX))

PRI atan2(_y, _x) | _z, atan       //this is my atan2 function

if _x == 0.0

if _y > 0.0

return(PIBY2_FLOAT)

if _y == 0.0

return(0.0)

else

return(fm.fneg(PIBY2_FLOAT))

_z := fm.fdiv(_y, _x)

if fm.fabs(_z) < 1.0

atan := fm.fdiv(_z, fm.fadd(1.0,fm.fmul(fm.fmul(0.28, _z), _z)))

if _x < 0.0

if _y < 0.0

return(fm.fsub(atan, PI_FLOAT))

else

return(fm.fadd(atan, PI_FLOAT))

else

atan := fm.fsub(PIBY2_FLOAT, fm.fdiv(_z , fm.fadd(fm.fmul(_z, _z), 0.28)))

if _y < 0.0

return(fm.fsub(atan, PI_FLOAT))

return(atan)

 

THANKS FOR ANY HELP!!!

pst.png

IMAG0003.jpg

IMAG0004.jpg

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

This reply was deleted.

Activity

DIY Robocars via Twitter
RT @Heavy02011: ⁦@diyrobocars⁩ : A Home-brew computer club* for Connected Autonomous Driving. talk at #piandmore ⁦@PiAndMore⁩ on Jan 23rd h…
22 hours ago
DIY Robocars via Twitter
RT @a1k0n: New blog post! Deep dive into my ceiling light based localization algorithm which runs in 1ms on a Raspberry Pi 3: https://t.co/…
22 hours ago
DIY Robocars via Twitter
Great new guide to using @donkey_car https://custom-build-robots.com/donkey-car-e-book-en
22 hours ago
DIY Robocars via Twitter
RT @chr1sa: The next @DIYRobocars virtual AI race is tomorrow morn at 9:00am PT. You can watch live on Twitch https://www.meetup.com/DIYRobocars/events/275268196/
yesterday
DIY Robocars via Twitter
New version of Intel OpenBot! This resolves many of the issues with the first version, including a much smoother tr… https://twitter.com/i/web/status/1352395636369313798
Thursday
DIY Drones via Twitter
Using ArduRover with an RTK GPS https://ift.tt/2N9I3RO
Tuesday
DIY Drones via Twitter
Monday
DIY Robocars via Twitter
Monday
DIY Robocars via Twitter
Jan 15
DIY Robocars via Twitter
Jan 15
DIY Drones via Twitter
Jan 14
DIY Robocars via Twitter
RT @Heavy02011: @diyrobocars : A Home-brew computer club* for Connected Autonomous Driving on Jan 23rd, 2021 https://www.meetup.com/Connected-Autonomous-Driving/events/275728684/ #Meetu…
Jan 14
DIY Robocars via Twitter
Jan 14
David Hori liked Isabella Domi's profile
Jan 12
DIY Robocars via Twitter
RT @Heavy02011: ⁦@diyrobocars⁩ Autonomous Driving Assembly at #rC3. join us at https://rc3.world/rc3/assembly/diyrobocars-f1tenth/ ⁦@f1tenth⁩ ⁦@DAVGtech⁩ ⁦@DWalmroth⁩…
Jan 11
DIY Robocars via Twitter
RT @chr1sa: New car designs coming for our next @DIYRobocars @donkey_car virtual race on the 23rd. Choose any one you want at race time Le…
Jan 11
More…