MPL Questions about Compass Calibration

Justin,

Thanks for your response. I tried using inv_got_compass_bias(), but it always stays equal to zero, no matter how many figure-eights I do. I have called the following to enable 9-axis fusion and compass calibration:
inv_enable_quaternion();
inv_enable_9x_sensor_fusion();
inv_enable_vector_compass_cal();
inv_enable_magnetic_disturbance();

Any other ideas?

Austin

Justin,

Yes, I am reading the compass and loading it into the MPL using the inv_build_compass() functions. Below is the code that runs in my main loop that I copied from the example application. See anything I'm missing?

Austin

{

    uint32_t timestamp;

    uint32_t sensorTimestamp = 0;

    bool newDataToProcess = false;
    /* Don't worry about overflow for timestamps. 32-bit counter won't

       overflow for 49 days.

    */

    timestamp = Global_tickCounterMsec;
    /* Temperature data doesn't need to be read with every gyro sample.

     * Let's make them timer-based like the compass reads.

     */

    if (timestamp > _nextTempMsec)

    {

        _nextTempMsec = timestamp + TEMP_READ_MS;

        _newTempDataAvailable = true;

    }
    if (_newGyroAccelDataAvailable)

    {

        bool triggerCompassRead = false;

        short gyro[3], accel_short[3], sensors;

        unsigned char more;

        long accel[3], quat[4], temperature;
        /* We're not using a data ready interrupt for the compass, so we'll

         * make our compass reads timer-based instead.

         */

        if (timestamp >= _nextCompassMsec)

        {

            /* Compass reads are synced with FIFO reads. Shorten the time

               interval to ensure a compass read occurs with the closest

               FIFO read.

            */

            _nextCompassMsec = timestamp + (COMPASS_READ_MS - 2);

            triggerCompassRead = true;

        }
        /* This function gets new data from the FIFO when the DMP is in

         * use. The FIFO can contain any combination of gyro, accel,

         * quaternion, and gesture data. The sensors parameter tells the

         * caller which data fields were actually populated with new data.

         * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then

         * the FIFO isn't being filled with accel data.

         * The driver parses the gesture data to determine if a gesture

         * event has occurred; on an event, the application will be notified

         * via a callback (assuming that a callback function was properly

         * registered). The more parameter is non-zero if there are

         * leftover packets in the FIFO.

         */

        dmp_read_fifo(gyro, accel_short, quat, &sensorTimestamp, &sensors, &more);
        if (!more)

        {

            /* No more data, clear flag */

            _newGyroAccelDataAvailable = false;

        }
        if ((sensors & INV_XYZ_GYRO) > 0)

        {

            /* Push the new data to the MPL. */

            inv_build_gyro(gyro, sensorTimestamp);

            newDataToProcess = true;
            if (_newTempDataAvailable)

            {

                uint32_t tempTimestamp = 0;
                _newTempDataAvailable = false;
                /* Temperature only used for gyro temp comp. */

                mpu_get_temperature(&temperature, &tempTimestamp);

                inv_build_temp(temperature, tempTimestamp);

            }

        }
        if ((sensors & INV_XYZ_ACCEL) > 0)

        {

            accel[0] = (long)accel_short[0];

            accel[1] = (long)accel_short[1];

            accel[2] = (long)accel_short[2];

            inv_build_accel(accel, 0, sensorTimestamp);

            newDataToProcess = true;

        }
        if ((sensors & INV_WXYZ_QUAT) > 0)

        {

            inv_build_quat(quat, 0, sensorTimestamp);

            newDataToProcess = true;

        }
        if (triggerCompassRead)

        {

            short compass_short[3];

            long compass[3];
            /* For any MPU device with an AKM on the auxiliary I2C bus, the raw

             * magnetometer registers are copied to special gyro registers.

             */

            if (!mpu_get_compass_reg(compass_short, &sensorTimestamp))

            {

                compass[0] = (long)compass_short[0];

                compass[1] = (long)compass_short[1];

                compass[2] = (long)compass_short[2];
                /* NOTE: If using a third-party compass calibration library,

                 * pass in the compass data in uT * 2^16 and set the second

                 * parameter to INV_CALIBRATED | acc, where acc is the

                 * accuracy from 0 to 3.

                 */

                inv_build_compass(compass, 0, sensorTimestamp);

            }
            newDataToProcess = true;

        }
        if (newDataToProcess)

        {

            inv_execute_on_data();
            if (triggerCompassRead)

            {

                _compassDataWasProcessed = true;

            }

        }

    }

}

Justin,

I'm back to looking into this issue of how to determine if the magnetometer is calibrated. I tried inv_got_compass_bias() and inv_get_compass_state() both functions always report zero, before or after some figure-eights.

Below is my IMU init code. Please review. Thanks.

Austin

{

    uint32_t errorCode = 0;

    inv_error_t result;

    uint8_t accel_fsr;

    uint16_t gyro_rate;

    uint16_t gyro_fsr;

#ifdef COMPASS_ENABLED

    uint16_t compass_fsr;

#endif
    /* Analog power off */

    NRF_GPIO->OUTCLR = (1 << 4);
    /* Initialize GPIO */

    /* Inputs */

    NRF_GPIO->DIRCLR = (1 << 1) |   // SCL

                       (1 << 2) |   // INT

                       (1 << 3);    // SDA

    /* Output */

    NRF_GPIO->DIRSET = (1 << 4);    // analog power
    errorCode = nrf_drv_twi_init(&Global_imuI2c, NULL, NULL);

    APP_ERROR_CHECK(errorCode);
    nrf_drv_twi_enable(&Global_imuI2c);

    nrf_delay_ms(20);
    /* Analog power on */

    NRF_GPIO->OUTSET = (1 << 4);

    nrf_delay_ms(200);
#if 0

    whoami = 0;

    result = mpu_read_reg(117, &whoami);

    APP_ERROR_CHECK(result);
    if (whoami == 0x71)

    {

        ++Global_tickCounterMsec;

    }

#endif
    /* Init the MPU */

    result = mpu_init(NULL);

    APP_ERROR_CHECK(result);
    /* Configure IMU interrupt pin.

       Don't enable until done with pairing.

    */

    nrf_drv_gpiote_in_init(2, &_imuIntPinConfig, _ImuIntHandler);
    /* Init the MPL */

    result = inv_init_mpl();

    APP_ERROR_CHECK(result);
    /* Compute 6-axis and 9-axis quaternions. */

    inv_enable_quaternion();

    inv_enable_9x_sensor_fusion();

    /* The MPL expects compass data at a constant rate (matching the rate

     * passed to inv_set_compass_sample_rate). If this is an issue for your

     * application, call this function, and the MPL will depend on the

     * timestamps passed to inv_build_compass instead.

     *

     * inv_9x_fusion_use_timestamps(1);

     */
    /* Update gyro biases when not in motion.

     * WARNING: These algorithms are mutually exclusive.

     */

    inv_enable_fast_nomot();

    /* inv_enable_motion_no_motion(); */

    /* inv_set_no_motion_time(1000); */
    /* Update gyro biases when temperature changes. */

    inv_enable_gyro_tc();
    /* This algorithm updates the accel biases when in motion. A more accurate

     * bias measurement can be made when running the self-test (see case 't' in

     * handle_input), but this algorithm can be enabled if the self-test can't

     * be executed in your application.

     *

     * inv_enable_in_use_auto_calibration();

     */

#ifdef COMPASS_ENABLED

    /* Compass calibration algorithms. */

    inv_enable_vector_compass_cal();

    inv_enable_magnetic_disturbance();

#endif

    /* If you need to estimate your heading before the compass is calibrated,

     * enable this algorithm. It becomes useless after a good figure-eight is

     * detected, so we'll just leave it out to save memory.

     */

#if 0

    /* Enabled for operation before compass is calibrated */

    inv_enable_heading_from_gyro();

#endif
    inv_enable_eMPL_outputs();
    result = inv_start_mpl();

    APP_ERROR_CHECK(result);
    /* Get/set hardware configuration. Start gyro. */

    /* Wake up all sensors. */

#ifdef COMPASS_ENABLED

    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL | INV_XYZ_COMPASS);

#else

    mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);

#endif

    /* Push both gyro and accel data into the FIFO. */

    mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);

    mpu_set_sample_rate(DEFAULT_MPU_HZ);

#ifdef COMPASS_ENABLED

    /* The compass sampling rate can be less than the gyro/accel sampling rate.

     * Use this function for proper power management.

     */

    mpu_set_compass_sample_rate(1000 / COMPASS_READ_MS);

#endif

    /* Read back configuration in case it was set improperly. */

    mpu_get_sample_rate(&gyro_rate);

    mpu_get_gyro_fsr(&gyro_fsr);

    mpu_get_accel_fsr(&accel_fsr);

#ifdef COMPASS_ENABLED

    mpu_get_compass_fsr(&compass_fsr);

#endif

    /* Sync driver configuration with MPL. */

    /* Sample rate expected in microseconds. */

    inv_set_gyro_sample_rate(1000000L / gyro_rate);

    inv_set_accel_sample_rate(1000000L / gyro_rate);

#ifdef COMPASS_ENABLED

    /* The compass rate is independent of the gyro and accel rates. As long as

     * inv_set_compass_sample_rate is called with the correct value, the 9-axis

     * fusion algorithm's compass correction gain will work properly.

     */

    inv_set_compass_sample_rate(COMPASS_READ_MS * 1000L);

#endif
    /* Set chip-to-body orientation matrix.

     * Set hardware units to dps/g's/degrees scaling factor.

     */

    inv_set_gyro_orientation_and_scale(

            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),

            (long)gyro_fsr<<15);

    inv_set_accel_orientation_and_scale(

            inv_orientation_matrix_to_scalar(gyro_pdata.orientation),

            (long)accel_fsr<<15);

#ifdef COMPASS_ENABLED

    inv_set_compass_orientation_and_scale(

            inv_orientation_matrix_to_scalar(compass_pdata.orientation),

            (long)compass_fsr<<15);

#endif
    /* To initialize the DMP:

     * 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in

     *    inv_mpu_dmp_motion_driver.h into the MPU memory.

     * 2. Push the gyro and accel orientation matrix to the DMP.

     * 3. Register gesture callbacks. Don't worry, these callbacks won't be

     *    executed unless the corresponding feature is enabled.

     * 4. Call dmp_enable_feature(mask) to enable different features.

     * 5. Call dmp_set_fifo_rate(freq) to select a DMP output rate.

     * 6. Call any feature-specific control functions.

     *

     * To enable the DMP, just call mpu_set_dmp_state(1). This function can

     * be called repeatedly to enable and disable the DMP at runtime.

     *

     * The following is a short summary of the features supported in the DMP

     * image provided in inv_mpu_dmp_motion_driver.c:

     * DMP_FEATURE_LP_QUAT: Generate a gyro-only quaternion on the DMP at

     * 200Hz. Integrating the gyro data at higher rates reduces numerical

     * errors (compared to integration on the MCU at a lower sampling rate).

     * DMP_FEATURE_6X_LP_QUAT: Generate a gyro/accel quaternion on the DMP at

     * 200Hz. Cannot be used in combination with DMP_FEATURE_LP_QUAT.

     * DMP_FEATURE_TAP: Detect taps along the X, Y, and Z axes.

     * DMP_FEATURE_ANDROID_ORIENT: Google's screen rotation algorithm. Triggers

     * an event at the four orientations where the screen should rotate.

     * DMP_FEATURE_GYRO_CAL: Calibrates the gyro data after eight seconds of

     * no motion.

     * DMP_FEATURE_SEND_RAW_ACCEL: Add raw accelerometer data to the FIFO.

     * DMP_FEATURE_SEND_RAW_GYRO: Add raw gyro data to the FIFO.

     * DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot

     * be used in combination with DMP_FEATURE_SEND_RAW_GYRO.

     */

    dmp_load_motion_driver_firmware();

    dmp_set_orientation(

        inv_orientation_matrix_to_scalar(gyro_pdata.orientation));

    dmp_register_tap_cb(NULL);

    dmp_register_android_orient_cb(NULL);

    /*

     * Known Bug -

     * DMP when enabled will sample sensor data at 200Hz and output to FIFO at the rate

     * specified in the dmp_set_fifo_rate API. The DMP will then sent an interrupt once

     * a sample has been put into the FIFO. Therefore if the dmp_set_fifo_rate is at 25Hz

     * there will be a 25Hz interrupt from the MPU device.

     *

     * There is a known issue in which if you do not enable DMP_FEATURE_TAP

     * then the interrupts will be at 200Hz even if fifo rate

     * is set at a different rate. To avoid this issue include the DMP_FEATURE_TAP

     *

     * DMP sensor fusion works only with gyro at +-2000dps and accel +-2G

     */

    dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT | DMP_FEATURE_TAP);
    dmp_set_fifo_rate(DEFAULT_MPU_HZ);

    inv_set_quat_sample_rate(1000000L / DEFAULT_MPU_HZ);
    mpu_set_dmp_state(1);

}

Justin,

Thanks for reviewing my code and for the tips on how to perform good figure-eights. That did it. The accuracy value went to 3 and the return value of inv_got_compass_bias() became 1.

Austin

Handi,

I don't have any more information for you other than to make sure you are doing good figure-eights. The figure-eights should be performed in several different planes and don't go super fast. I think it took a bit longer than I expected as well. That's all I can tell you.

Austin