Problem with tilt compensation

Hey all,

I'm very happy to join DiyDrones community
(Sorry for my bad english)

I'm working on my new project with a drone 3DR frame and 10DOF IMU (MPU6050, HMC5883, MS5611 http://www.drotek.fr/shop/en/home/62-imu-10dof-mpu6050-hmc5883-ms5611.html), 1 or 2 days per month for 3 years so moving very slowly between the IHM and the embedded code! 

I using a MPU6050 and HMC5883 Magnetomter and chipkit pic32 Board !

If using a heading with only magnetometer with this code my yaw axis it's good (if don't move other axis):

My code:

float heading = atan2(mag.y, mag.x); // mag.x and mag.y it's raw value scaled ( raw * 0.92 )  1.3Ga

if (heading < 0)
heading += 2*M_PI;

if (heading > 2*M_PI)
heading -= 2*M_PI;

return heading * toDeg;


Video :

If add a tilt compensation with accelerometer my yaw axis, move with roll and pitch inclination

My code: 

float rollRads = acc.x * toRad; // acc.x = acc angle calculate with acos( accX(G) ) converted to deg

float pitchRads = acc.y * toRad; // acc.y =  acc angle calculate with acos( accY(G) ) converted to deg
float cosRoll = cos(rollRads);
float sinRoll = sin(rollRads);
float cosPitch = cos(pitchRads);
float sinPitch = sin(pitchRads);

float Xh = mag.x * cosPitch + mag.y * sinRoll * sinPitch + mag.z * cosRoll * sinPitch;
float Yh = mag.y * cosRoll - mag.z * sinRoll;

float heading = atan2(Yh, Xh);

if (heading < 0) {
heading += (2*M_PI);
}

if (heading > M_PI) {
heading -= (2*M_PI);
}

return heading * toDeg;

Video:
 

My code is available on github (Check IMU.cpp):
https://github.com/marc-j/NeuroDrone-pic32

Thanks for your help !!

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

Join diydrones

Email me when people reply –

Replies

  • I found the problem after simulation, resolved by this calcul :

    float Xh = (mag.x * cosRoll) + (mag.y * sinPitch * sinRoll) - (mag.z * cosPitch * sinRoll);
    float Yh = (mag.y * cosPitch) - (mag.z * sinPitch);

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…
Saturday
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/…
Saturday
DIY Robocars via Twitter
Great new guide to using @donkey_car https://custom-build-robots.com/donkey-car-e-book-en
Saturday
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/
Friday
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 Robocars via Twitter
Jan 18
DIY Robocars via Twitter
Jan 15
DIY Robocars via Twitter
Jan 15
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
DIY Robocars via Twitter
RT @RoboticMasters: Thanks to @EllerbachMaxime and the Sydney Uni Capstone Students the @donkey_car @diyrobocars simulator is getting a ma…
Jan 11
DIY Robocars via Twitter
Jan 6
DIY Robocars via Twitter
Dec 28, 2020
More…