J'avais vue cela effectivement, mais les articles que j'avais lu parlaient de modifier le firmware du bordel pour pouvoir récupérer en i2c ces infos.
Je viens de lire ce que tu as posté, apparemment les calculs pitch/roll/yaw sont bien effectués dans la bibliothèque, et non pas dans le DMP, voir par exemple ici:
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
// 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)
data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
// roll: (tilt left/right, about X axis)
data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
return 0;
}
Et ici le fait qu'il écrit dans la mémoire au démarrage du mpu (a mon avis le bout de programme qui créé ses fameux quaternions):
const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
Et encore, j'ai un doute que les quaternions soient faits dans le DMP:
class Quaternion {
public:
float w;
float x;
float y;
float z;
Quaternion() {
w = 1.0f;
x = 0.0f;
y = 0.0f;
z = 0.0f;
}
Quaternion(float nw, float nx, float ny, float nz) {
w = nw;
x = nx;
y = ny;
z = nz;
}
Quaternion getProduct(Quaternion q) {
// Quaternion multiplication is defined by:
// (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
// (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
// (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
// (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
return Quaternion(
w*q.w - x*q.x - y*q.y - z*q.z, // new w
w*q.x + x*q.w + y*q.z - z*q.y, // new x
w*q.y - x*q.z + y*q.w + z*q.x, // new y
w*q.z + x*q.y - y*q.x + z*q.w); // new z
}
Quaternion getConjugate() {
return Quaternion(w, -x, -y, -z);
}
float getMagnitude() {
return sqrt(w*w + x*x + y*y + z*z);
}
void normalize() {
float m = getMagnitude();
w /= m;
x /= m;
y /= m;
z /= m;
}
Quaternion getNormalized() {
Quaternion r(w, x, y, z);
r.normalize();
return r;
}
};
class VectorInt16 {
public:
int16_t x;
int16_t y;
int16_t z;
VectorInt16() {
x = 0;
y = 0;
z = 0;
}
VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
x = nx;
y = ny;
z = nz;
}
float getMagnitude() {
return sqrt(x*x + y*y + z*z);
}
void normalize() {
float m = getMagnitude();
x /= m;
y /= m;
z /= m;
}
VectorInt16 getNormalized() {
VectorInt16 r(x, y, z);
r.normalize();
return r;
}
void rotate(Quaternion *q) {
// http://www.cprogramming.com/tutorial/3d/quaternions.html
// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
// ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
// P_out = q * P_in * conj(q)
// - P_out is the output vector
// - q is the orientation quaternion
// - P_in is the input vector (a*aReal)
// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
Quaternion p(0, x, y, z);
// quaternion multiplication: q * p, stored back in p
p = q -> getProduct(p);
// quaternion multiplication: p * conj(q), stored back in p
p = p.getProduct(q -> getConjugate());
// p quaternion is now [0, x', y', z']
x = p.x;
y = p.y;
z = p.z;
}
VectorInt16 getRotated(Quaternion *q) {
VectorInt16 r(x, y, z);
r.rotate(q);
return r;
}
};
class VectorFloat {
public:
float x;
float y;
float z;
VectorFloat() {
x = 0;
y = 0;
z = 0;
}
VectorFloat(float nx, float ny, float nz) {
x = nx;
y = ny;
z = nz;
}
float getMagnitude() {
return sqrt(x*x + y*y + z*z);
}
void normalize() {
float m = getMagnitude();
x /= m;
y /= m;
z /= m;
}
VectorFloat getNormalized() {
VectorFloat r(x, y, z);
r.normalize();
return r;
}
void rotate(Quaternion *q) {
Quaternion p(0, x, y, z);
// quaternion multiplication: q * p, stored back in p
p = q -> getProduct(p);
// quaternion multiplication: p * conj(q), stored back in p
p = p.getProduct(q -> getConjugate());
// p quaternion is now [0, x', y', z']
x = p.x;
y = p.y;
z = p.z;
}
VectorFloat getRotated(Quaternion *q) {
VectorFloat r(x, y, z);
r.rotate(q);
return r;
}
};
Car encore une fois le datasheet ne mentionne pas du tout cela, car si c'était le cas j'aurais inclu pitch/roll/yaw à ma classe mpu6050 de mon système: http://sylvainmahe.xyz/howToProgramWidthModule.html
Car c'est quelque chose que pas mal de gens me demandent, mais je ne sais pas faire puisque le datasheet n'indique rien la dessus. J'ai l'impression que cela ressemble à une sorte de "hack" de la mémoire du mpu pour permettre cela.
