i2cdevlib

i2cdevlib copied to clipboard

Hi,

I have mounted the MPU6050 in a vertical orientation, where x is the pitch axis, instead of y:

MPU6050 Horizontal	MPU6050 Vertical
x	z
y	x
z	y

I am using MPU6050_6Axis_MotionApps612 and it is working fine getting the pitch angles, except that on startup, it takes around 3-5 seconds for the DMP output to stabilize. The value starts at pi / 2 and moves towards 0, which is the desired angle at the complete vertical orientation.

I am using the following dmpGetPitch function, which I based off of dmpGetYawPitchRoll:

float dmpGetPitch(Quaternion *q, VectorFloat *gravity) {
    float pitch = 0;
    // pitch: (nose up/down, about accel X axis (-Y axis in robot frame))
    pitch = atan2(gravity->z , sqrt(gravity->x * gravity->x + gravity->y * gravity->y));
    
    if (gravity -> y < 0) {
        if(pitch > 0) {
            pitch = PI - pitch; 
        } else { 
            pitch = -PI - pitch;
        }
    }
    return pitch;
}

I suspect that the DMP states initializes the output quaternion state at 0 degrees, from a horizontal orientation.

I want to initialize the DMP at a custom initial quaternion state. Is this possible? Is there some way I can modify the DMP code or send this data to the unit?

The reason why I want to set custom initial values is that my system might have a watchdog reset and I cannot spare 5 seconds of startup time. It needs to work instantaneously.