Jump to content
I2Cdevlib Forums

beng27

Members
  • Posts

    1
  • Joined

  • Last visited

Everything posted by beng27

  1. HI, I've been working on this and I offer a better coding of the conversion function in MPU6050_6Axis_MotionApps20.h starting on line 645. This gives the correct readings of +- 180 degrees in all 3 axes in the stock demo code This was figured out using info from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm and crucially the discussion at http://stackoverflow.com/questions/3755059/3d-accelerometer-calculate-the-orientation Please note: 1) Normally I develop Filemaker Databases and I really don't understand quaternions either! 2) I've only been using the Ardiono for a week or so and never seen C++ before, it may be that my code is not efficient. However I think the logic is sound. Ben _______ int8_t sign; uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { // p & r need to check for >90 rotation and correct for gravity polarity reversal if (gravity -> z > 0) {sign = 1;} else {sign = -1;} // yaw: (about Z axis) data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // pitch: (nose up/down, about Y axis) // adding tiny fraction of x2 or y2 avoids gimbal lock at cost of slight accuracy data[1] = atan2(gravity -> x , sign * sqrt(0.001*gravity -> y*gravity -> y + gravity -> z*gravity -> z)); // roll: (tilt left/right, about X axis) data[2] = atan2(gravity -> y , sign * sqrt(0.001*gravity -> x*gravity -> x + gravity -> z*gravity -> z)); return 0; }
×
×
  • Create New...