storing a matrix in the dmp to swap axes

[mark-hahn]

I may be crazy but I remember seeing code that encodes a 3x3 matrix into a single word and then stores it in the dmp. This matrix was used to swap axes. I remember being dissapointed that the matrix values could only be -1, 0, or +1. The unit matrix of +1's on the diagonal was the default. Does anyone know about this? I have been googling for an hour to find it again with no luck. There is no function in the motionApps files like this.

[ZHomeSlice]

your not,

This is all available from the https://invensense.tdk.com/ website
Get developer access and go to downloads then search for "Embedded MotionDrivers for our 6-axis and 9-axis MPU based products"
You will find their versions of the code 6.12 and an older 5.1.3 versions for the MPU6050 we have recreated in a much simpler way.

This funciton is found in
eMPL_outputs.c

/**
 *  @brief      Body-to-world frame rotation matrix.
 *  @param[out] data        Rotation matrix, q30 fixed point.
 *  @param[out] accuracy    Accuracy of the measurement from 0 (least accurate)
 *                          to 3 (most accurate).
 *  @param[out] timestamp   The time in milliseconds when this sensor was read.
 *  @return     1 if data was updated.
 */
int inv_get_sensor_type_rot_mat(long *data, int8_t *accuracy, inv_time_t *timestamp)
{
    inv_quaternion_to_rotation(eMPL_out.quat, data);
    accuracy[0] = eMPL_out.quat_accuracy;
    timestamp[0] = eMPL_out.nine_axis_timestamp;
    return eMPL_out.nine_axis_status;
}

eMPL_out.quat = the quaternion values
eMPL = Enbedded Motion Procesing Library or as we callit DMP

and the supporting math function:
found in:
ml_math_func.c

/**
 * Converts a quaternion to a rotation matrix.
 * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
 * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
 *             First 3 elements of the rotation matrix, represent
 *             the first row of the matrix. Rotation matrix multiplied
 *             by a 3 element column vector transform a vector from Body
 *             to World.
 */
void inv_quaternion_to_rotation(const long *quat, long *rot)
{
    rot[0] =
        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
                quat[0]) -
        1073741824L;
    rot[1] =
        inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
    rot[2] =
        inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
    rot[3] =
        inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
    rot[4] =
        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
                quat[0]) -
        1073741824L;
    rot[5] =
        inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
    rot[6] =
        inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
    rot[7] =
        inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
    rot[8] =
        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
                quat[0]) -
        1073741824L;
}

and

/** Performs a multiply and shift by 29. These are good functions to write in assembly on
 * with devices with small memory where you want to get rid of the long long which some
 * assemblers don't handle well
 * @param[in] a
 * @param[in] b
 * @return ((long long)a*b)>>29
*/
long inv_q29_mult(long a, long b)
{
#ifdef EMPL_NO_64BIT
    long result;
    result = (long)((float)a * b / (1L << 29));
    return result;
#else
    long long temp;
    long result;
    temp = (long long)a * b;
    result = (long)(temp >> 29);
    return result;
#endif
}

if you come up with a simple way to do this Please share.

Z

[mark-hahn]

I was hoping for something handled inside the dmp so that the quaternion output from the dmp fusion was from the swapped axes. I wanted this so that my chip was effectively tilted +10 degrees off an axis instead of -80. I was hoping the smaller angle would help the fusion inside the dmp stabilize faster than the 7 seconds I'm experiencing. Any processing I do outside the dmp is not going to help the fusion stabilization speed.

So my question is still whether there is a way to put a matrix in the dmp to have it start from swapped axes before it does the fusion.

[ZHomeSlice]

The Gyro auto stabilizes on V6.12 Embedded DMP. Note that my Calibration routine uses a PID routine to find the exact offsets. The gyro calibration only requires the MPU to be still. the accelerometer requires the MPU to be vertical Z pointing UP. Unfortunately, this requires gravity to be below the Z-axis to negate that error. You could modify the calibration routine to accept gravity on other planes.

Z

[mark-hahn]

I just saw this in the discussion on how DMP works (#648). You may have answered this question in that reply to the other discussion. :-)

Orientation Gesture Recognition – 
    Uses sensor data to detect is there is a change in device orientation from 
    portrait, landscape, reverse portrait, and reverse orientation. 
      Very much dependent on the orientation matrix.

This must be the matrix I asked about in this discussion's original post. I'll see if I can find out how to set it.