Important changes to forums and questions
All forums and questions are now archived. To start a new conversation or read the latest updates go to forums.mbed.com.
10 years, 6 months ago.
Quadrocopter yaw, pitch, roll calculation from 10-DOF sensor
Hello,
I'm trying to build an autonomous quadrocopter, but I'm having some trouble with calculating the yaw, pitch and roll angles from a 10-DOF imu sensor (3-DOF Gyro, 3-DOF accelerometer, 3-DOF magnetometer and a barometer) with my mbed (LPC1768). IMU Link: http://www.dfrobot.com/index.php?route=product/product&product_id=818
I have found some code on mbed.org for a 10-DOF imu sensor wich has basically the same hardware: http://mbed.org/users/pommzorz/code/HK10DOF/ This code uses quaternions to calculate the yaw, pitch and roll.
The code seems to be working fine, except that the calculated yaw angle from the <<HK10DOF::getYawPitchRoll()>>
method seems to return wrong data. The yaw angle value varies between 90-150 degrees while I am turning it 360 degrees around (and holding it horizontally),
I compared this data to the compass data with the <<getHeadingXY()>>
method.
This method returns seemingly correct data. But the data from this method is only reliable when the compas is being held horizontally (cause it doesn't take the Z axis into account). and obviously this isn't always the case in a quadrocopter. So thats why i need the quaternion calculations to work correctly...
I've searched the web for some other algorithms that should calculate the yaw correctly from quaternions, but for some reason I am not getting the right results.
Has anybody got any tips or code that i could try? Or does anybody know what could be wrong with the calculations?
this is the code from the HK10DOF source code that should calculate the yaw pitch and roll values from the quaternion:
getYawPitchRollRad()
void HK10DOF::getYawPitchRollRad(float * ypr) { float q[4]; // quaternion float gx, gy, gz; // estimated gravity direction getQ(q); gx = 2 * (q[1]*q[3] - q[0]*q[2]); gy = 2 * (q[0]*q[1] + q[2]*q[3]); gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1); ypr[1] = atan(gx / sqrt(gy*gy + gz*gz)); ypr[2] = atan(gy / sqrt(gx*gx + gz*gz)); }
4 Answers
10 years, 4 months ago.
Here is a mbed 10 dof solution which does not use quaternions: https://www.youtube.com/watch?v=0z1F3aezO40
10 years, 6 months ago.
Koen,
What you are probably seeing is "gimbal lock". Yes I know quaternions do not suffer from this, but atan and friends do! there are methods that do get around the issue, do a google and find a method that suits you.
Dave.
Atan can give problems, but it should not be possible to actually lock it, since the calculations are done in quaternions, at best it can give unreliable results, but that should be in cases where that is supposed to happen (for example if you have a plane that is going vertical, it has no heading).
@Koen, I do have my own AHRS calculation code, however it has never been tested (I did test it just with an IMU board, but it has never been on board a quadcopter), so I can't really guarantee it won't crash immediatly :P. It also doesn't use quaternions since they gave me a headache, but simply vector operations. Also efficiency wise there is alot to improve. But in my manual tests it looked like it worked, including autocalibration of gyro offset.
posted by 22 Jan 2014See here for an explanation http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles check under "singularities".
Dave.
posted by 22 Jan 2014@Erik, Could you maybe give me a link to your AHRS code? It could probably help me alot :).
posted by 22 Jan 2014@Koen, see: http://mbed.org/users/Sissors/code/IMUCalc/wiki/Homepage. Added also a little bit of info from what I remember how it should work :D. (It actually worked pretty well in my manual tests, with gyroscope drift being calculated and compensated, but then I felt like making the code was more fun than the quadrocopter, and never continued :D).
posted by 22 Jan 201410 years, 6 months ago.
I don't have any good answers for you, but I just wanted to let you know that I'm working on the same exact problem. If I get a solution together I'll be sure to post it here first.
10 years, 6 months ago.
"<<getHeadingXY()>>
"
Take a look LSM303::getTiltHeading(). It calculates correct heading for tilted compass.
https://mbed.org/users/fin4478/code/LSM303DM/file/1052b1b97cc2/LSM303.cpp