Hi there i want to calculate the vertical velocity from accelerometer but firt i want to poject the global z vector from body frame ???
is therer a techniqu to do that ?? i use mpu6050.
I'm working on the same I have modified this to handle additional angles but I haven't tested it completely. Difficult to test velocity while tethered with usb.
Usage with DMP values retrieved from FIFO Buffer of the MPU6050
if (USE_DMP == 1)
{
long quat[4];
float quaternion[4];
short sensors;
if (0 == (ret = dmp_read_fifo(gyro, accel, quat, ×tamp, &sensors, &more)))
{
if (!more)
{
_dataReady = 0;
}
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
q.setQuaternion((float)quat[QUAT_W] / 16384.0f, (float)quat[QUAT_X] / 16384.0f, (float)quat[QUAT_Y] / 16384.0f, (float)quat[QUAT_Z] / 16384.0f);
q.normalize();
//aa.setVectorInt16((short)accel[VEC_X], (short)accel[VEC_Y] , (short)accel[VEC_Z]);
//aa.normalize();
getGravity(&gravity, &q);
//getYawPitchRoll(ypr, &q, &gravity);
getYawPitchRollInDeg(ypr, &q, &gravity);
//getEuler(euler, &q);
//getgetEulerInDeg(euler, &q);
//getLinearAccel(&aaReal, &aa, &gravity);
//getLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print(ypr[0]); Serial.print(F(" "));
Serial.print(ypr[1]); Serial.print(F(" "));
Serial.print(ypr[2]); Serial.print(F(" "));
// Serial.print(euler[0]); Serial.print(F(" "));
// Serial.print(euler[1]); Serial.print(F(" "));
// Serial.print(euler[2]); Serial.print(F(" "));
// Serial.print(aaReal.x); Serial.print(F(" "));
// Serial.print(aaReal.y); Serial.print(F(" "));
// Serial.print(aaReal.z); Serial.print(F(" "));
// Serial.print(aaWorld.x); Serial.print(F(" "));
// Serial.print(aaWorld.y); Serial.print(F(" "));
// Serial.print(aaWorld.z); Serial.print(F(" "));
//Serial.print(gyro[VEC_X]);Serial.print(F(" "));
//Serial.print(gyro[VEC_Y]);Serial.print(F(" "));
//Serial.print(gyro[VEC_Z]);Serial.print(F(" "));
//Serial.print(accel[VEC_X]);Serial.print(F(" "));
//Serial.print(accel[VEC_Y]);Serial.print(F(" "));
//Serial.print(accel[VEC_Z]);
Serial.println();
}
else
{
Serial.print(F("1 Error: ")); Serial.println(ret);
}
}
helper_3dmath.h with some additions at the bottom attached
Let me know if you need help with the retrieval of the MPU6050 DMP values
Z
helper_3dmath.h (8.22 KB)