Пример #1
0
/**
 *  @brief  An MPL wrapper for the main MPU Self Test API inv_factory_calibrate().
 *          See inv_factory_calibrate() function for more details.
 *
 *  @pre    inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg
 *          data structure.
 *          On Windows, SetupPlatform() is also a madatory pre condition to
 *          ensure the accelerometer is properly configured before running the
 *          test.
 *
 *  @param  mlsl_handle
 *              serial interface handle to allow serial communication with the
 *              device, both gyro and accelerometer.
 *  @param  provide_result
 *              If 1:
 *              perform and analyze the offset, drive frequency, and noise
 *              calculation and compare it against set thresholds. Report
 *              also the final result using a bit-mask like error code as
 *              described in the inv_test_gyro_xxxx() functions.
 *              When 0:
 *              skip the noise and drive frequency calculation  and pass/fail
 *              assessment. It simply calculates the gyro and accel biases.
 *              NOTE: for MPU6050 devices, this parameter is currently
 *              ignored.
 *
 *  @return INV_SUCCESS or first non-zero error code otherwise.
 */
inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle,
        unsigned char provide_result)
{
    INVENSENSE_FUNC_START;
    inv_error_t firstError = INV_SUCCESS;
    inv_error_t result;
    unsigned char initState = inv_get_state();

    if (initState < INV_STATE_DMP_OPENED) {
        MPL_LOGE("Self Test cannot run before inv_dmp_open()\n");
        return INV_ERROR_SM_IMPROPER_STATE;
    }

    /* obtain a pointer to the 'struct mldl_cfg' data structure. */
    mputestCfgPtr = inv_get_dl_config();

    if(initState == INV_STATE_DMP_STARTED) {
        result = inv_dmp_stop();
        ERROR_CHECK_FIRST(firstError, result);
    }

    result = inv_factory_calibrate(mlsl_handle, provide_result);
    ERROR_CHECK_FIRST(firstError, result);

    if(initState == INV_STATE_DMP_STARTED) {
        result = inv_dmp_start();
        ERROR_CHECK_FIRST(firstError, result);
    }

    return firstError;
}
Пример #2
0
/**
 *  @brief  umplStartMPU kicks starts the uMPL state machine. This function
 *          in turn calls the MPL functions like inv_dmp_open() and inv_dmp_start() which starts 
 *          the motion processing algorithms. This function also enables the required features
 *          such as turning on the bias trackers and temperature compensation.
 *          This function enables the required type of data to be put in FIFO.
 *            
 *
 *
 *  @pre    umplInit() must have been called.
 *
 *  @return INV_SUCCESS if successful, a non-zero error code otherwise.
 */
inv_error_t umplStartMPU(void)
{
	inv_error_t result;

    if (umplState == UMPL_STOP)
    {
        MPL_LOGV("UMPL_STOP to UMPL_RUN\n");
        result = inv_dmp_open();
        if (result != INV_SUCCESS) return result;

        result = umplDmpSetup();
        if (result != INV_SUCCESS) return result;

        result = inv_dmp_start();
        if (result != INV_SUCCESS) return result;

#ifndef UMPL_DISABLE_LOAD_CAL
        result = inv_uload_calibration();
        if (result != INV_SUCCESS)
            MPL_LOGE("inv_uload_calibration failed with %d in umplStartMPU\n",result);
#endif
        umplSetState(UMPL_RUN);
        return INV_SUCCESS;
    }
    else if( umplState == UMPL_ACCEL_ONLY )
    {
        struct mldl_cfg * mldl_cfg = inv_get_dl_config();
        MPL_LOGD("UMPL_ACCEL_ONLY (or UMPL_LPACCEL_ONLY) to UMPL_RUN\n");
        if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]) {
            inv_set_mpu_sensors( INV_NINE_AXIS );
        } else {
            inv_set_mpu_sensors( INV_SIX_AXIS_GYRO_ACCEL );
        }
        inv_set_fifo_rate(fifo_rate);
        umplSetState(UMPL_RUN);
        return INV_SUCCESS;
    }
    else if( umplState == UMPL_LPACCEL_ONLY )
    {
        umplStartAccelOnly(0.0);
        umplStartMPU();
    }
    return INV_ERROR_SM_IMPROPER_STATE;
}
/* set the power states of the various sensors based on the bits set in the
 * enabled_sensors parameter.
 * this function modifies globalish state variables.  It must be called with the mMplMutex held. */
void MPLSensor::setPowerStates(int enabled_sensors)
{
    FUNC_LOG;
    bool irq_set[5] = { false, false, false, false, false };

    //ALOGV(" setPowerStates: %d dmp_started: %d", enabled_sensors, mDmpStarted);

    do {

        if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) {
            mLocalSensorMask = ALL_MPL_SENSORS_NP;
            break;
        }

        if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) {
            mLocalSensorMask = 0;
            break;
        }

        if (GY_ENABLED) {
            mLocalSensorMask |= INV_THREE_AXIS_GYRO;
        } else {
            mLocalSensorMask &= ~INV_THREE_AXIS_GYRO;
        }

        if (A_ENABLED) {
            mLocalSensorMask |= (INV_THREE_AXIS_ACCEL);
        } else {
            mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL);
        }

        if (M_ENABLED) {
            mLocalSensorMask |= INV_THREE_AXIS_COMPASS;
        } else {
            mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS);
        }

    } while (0);

    //record the new sensor state
    inv_error_t rv;

    long sen_mask = mLocalSensorMask & mMasterSensorMask;

    bool changing_sensors = ((inv_get_dl_config()->requested_sensors
            != sen_mask) && (sen_mask != 0));
    bool restart = (!mDmpStarted) && (sen_mask != 0);

    if (changing_sensors || restart) {

        ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart);

        if (mDmpStarted) {
            inv_dmp_stop();
            clearIrqData(irq_set);
            mDmpStarted = false;
        }

        if (sen_mask != inv_get_dl_config()->requested_sensors) {
            //ALOGV("setPowerStates: %lx", sen_mask);
            rv = inv_set_mpu_sensors(sen_mask);
            ALOGE_IF(rv != INV_SUCCESS,
                    "error: unable to set MPL sensor power states (sens=%ld retcode = %d)",
                    sen_mask, rv);
        }

        if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS))
                || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL)))
                && ((sen_mask & INV_DMP_PROCESSOR) == 0)) {
            ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ");
            mUseTimerirq = true;
        } else {
            if (mUseTimerirq) {
                ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
                clearIrqData(irq_set);
            }
            ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ");
            mUseTimerirq = false;
        }

        if (!mDmpStarted) {
            if (mHaveGoodMpuCal || mHaveGoodCompassCal) {
                rv = inv_store_calibration();
                ALOGE_IF(rv != INV_SUCCESS,
                        "error: unable to store MPL calibration file");
                mHaveGoodMpuCal = false;
                mHaveGoodCompassCal = false;
            }
            //ALOGV("Starting DMP");
            rv = inv_dmp_start();
            ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp");
            mDmpStarted = true;
        }
    }

    //check if we should stop the DMP
    if (mDmpStarted && (sen_mask == 0)) {
        //ALOGV("Stopping DMP");
        rv = inv_dmp_stop();
        ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)",
                rv);
        if (mUseTimerirq) {
            ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
        }
        clearIrqData(irq_set);

        mDmpStarted = false;
        mPollTime = -1;
        mCurFifoRate = -1;
    }

}
/**
 *  @brief  The main entry point of the MPU Self Test, triggering the run of
 *          the single tests, for gyros and accelerometers.
 *          Prepares the MPU for the test, taking the device out of low power
 *          state if necessary, switching the MPU secondary I2C interface into
 *          bypass mode and restoring the original power state at the end of
 *          the test.
 *          This function is also responsible for encoding the output of each
 *          test in the correct format as it is stored on the file/medium of
 *          choice (according to inv_serial_write_cal() function).
 *          The format needs to stay perfectly consistent with the one expected
 *          by the corresponding loader in ml_stored_data.c; currectly the
 *          loaded in use is inv_load_cal_V1 (record type 1 - initial
 *          calibration).
 *
 *  @param  mlsl_handle
 *              serial interface handle to allow serial communication with the
 *              device, both gyro and accelerometer.
 *  @param  perform_full_test
 *              If 1:
 *              Complete calibration test:
 *              Calculate offset, drive frequency, and noise and compare it
 *              against set thresholds.
 *              When 0:
 *              Skip the noise and drive frequency calculation,
 *              simply calculate the gyro biases.
 *  @param  provide_result
 *              If 1:
 *              Report the final result using a bit-mask like error code as
 *              described in the test_gyro() function.
 *
 *  @return 0 on success.  A non-zero error code on error.
 *          Propagates the errors from the tests up to the caller.
 */
int inv_device_test(void *mlsl_handle,
                    uint_fast8_t sensor_mask,
                    uint_fast8_t perform_full_test,
                    uint_fast8_t provide_result)
{
    int result = 0, gyro_test_result = 0, accel_test_result = 0;
    short temp_avg = 0;
    short gyro_biases[3] = {0, 0, 0};
    short accel_biases[3] = {0, 0, 0};
    long accel_sens[3] = {0};
    unsigned long saved_sensor_mask;
    unsigned char saved_state = inv_get_state();

    mldl_cfg = inv_get_dl_config();
    saved_sensor_mask = mldl_cfg->inv_mpu_cfg->requested_sensors;

    if (sensor_mask & (INV_THREE_AXIS_GYRO & ~INV_DMP_PROCESSOR)) {
        result = inv_set_mpu_sensors(INV_THREE_AXIS_GYRO & ~INV_DMP_PROCESSOR);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return -1;
        }
        if (saved_state < INV_STATE_DMP_STARTED) {
            result = inv_dmp_start();
            if (result) {
                LOG_RESULT_LOCATION(result);
                return -1;
            }
        }
#ifdef TRACK_IDS
        result = get_mpu_unique_id(mlsl_handle);
        if (result != INV_SUCCESS) {
            MPL_LOGI("Could not read the device's unique ID\n");
            MPL_LOGI("\n");
            return -1;
        }
#endif
        MPL_LOGI("Collecting one group of %d ms samples for each axis\n",
            (perform_full_test ? DEF_PERIOD_CAL : DEF_PERIOD_SELF));
        MPL_LOGI("\n");

        /* adjust the gyro sensitivity according to the gyro_sens_trim value */
        adj_gyro_sens = test_setup.gyro_sens *
            mldl_cfg->mpu_chip_info->gyro_sens_trim / 131.072f;
        test_setup.gyro_fs = (int)(32768.f / adj_gyro_sens);

        /* collect gyro and temperature data, test gyro, report result */
        gyro_test_result = test_gyro(mlsl_handle,
                                gyro_biases, &temp_avg, perform_full_test);
        MPL_LOGI("\n");
        if (gyro_test_result == 0) {
            MPL_LOGI_IF(provide_result, "Test : PASSED\n");
        } else {
            MPL_LOGI_IF(provide_result, "Test : FAILED %d/%04X - Biases NOT stored\n",
                        gyro_test_result, gyro_test_result);
            goto gyro_test_failed;
        }
        MPL_LOGI_IF(provide_result, "\n");
    }

    if (sensor_mask & INV_THREE_AXIS_ACCEL) {
        if (!mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]) {
            MPL_LOGI("\n");
            MPL_LOGI("No accelerometer configured\n");
        } else {
            result = inv_set_mpu_sensors(INV_THREE_AXIS_ACCEL);
            if (result)
                return -1;
            if (inv_get_state() < INV_STATE_DMP_STARTED) {
                result = inv_dmp_start();
                if (result)
                    return -1;
            }

            get_accel_sensitivity(accel_sens);
            /* collect accel data.  if this step is skipped,
               ensure the array still contains zeros. */
            accel_test_result =
                test_accel(mlsl_handle, sensor_mask >> 4,
                           accel_biases, accel_sens[Z],
                           perform_full_test);
            if (accel_test_result)
                goto accel_test_failed;

            /* if only Z accel is requested,
               clear out the biases from the other 2 axes */
            if ((sensor_mask & INV_THREE_AXIS_ACCEL) == INV_Z_ACCEL)
                    accel_biases[X] = accel_biases[Y] = 0;
        }
    }

	
	ALOGE("in %s: accel_bias is %d %d %d", __func__, accel_biases[0],
			accel_biases[1], accel_biases[2]);
    result = populate_data_store(sensor_mask, temp_avg, gyro_biases,
                                     accel_biases, accel_sens);
    if (result)
        return -1;
    result = inv_serial_write_cal(data_store, ML_INIT_CAL_LEN);
    if (result) {
        MPL_LOGI("Error : cannot write calibration on file - error %d\n",
            result);
        LOG_RESULT_LOCATION(result);
        return -1;
    }

gyro_test_failed:
accel_test_failed:
    /* restore the setting had at the beginning */
    mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
    result = inv_set_mpu_sensors(saved_sensor_mask);
    if (result) {
        LOG_RESULT_LOCATION(result);
        return -1;
    }
    /* turn off only if it was off when the function was called */
    if (saved_state < INV_STATE_DMP_STARTED) {
        result = inv_dmp_stop();
        if (result) {
            LOG_RESULT_LOCATION(result);
            return -1;
        }
    }

    if (gyro_test_result)
        return gyro_test_result;
    if (accel_test_result)
        return accel_test_result;

    return result;
}
// Main function
int main(int argc, char *argv[])
{            
    //unsigned short accelSlaveAddr = ACCEL_SLAVEADDR_INVALID;
    unsigned short platformId = ID_INVALID;
    unsigned short accelId = ID_INVALID;
    unsigned short compassId = ID_INVALID;
    unsigned char reg[32];
    unsigned char *verStr;
    int result;
    int key = 0;
    int interror;
    struct mpuirq_data **data;
    const char *ints[] = { "/dev/mpuirq",        /* INTSRC_MPU  */
                           //"/dev/accelirq",    /* INTSRC_AUX1 */
                           //"/dev/compassirq",  /* INTSRC_AUX2 */
                           //"/dev/pressureirq", /* INTSRC_AUX3 */
    };
    int handles[ARRAY_SIZE(ints)];

    CALL_N_CHECK( inv_get_version(&verStr) );
    printf("%s\n", verStr);

    if(INV_SUCCESS == MenuHwChoice(&platformId, &accelId, &compassId)) {
        CALL_CHECK_N_RETURN_ERROR(SetupPlatform(platformId,
                                                accelId, compassId));
    }

    CALL_CHECK_N_RETURN_ERROR( inv_serial_start("/dev/mpu") );
    
    IntOpen(ints, handles, ARRAY_SIZE(ints));
    if (handles[0] < 0) {
        printf("IntOpen failed\n");
        interror = INV_ERROR;
    } else {
        interror = INV_SUCCESS;
    }

    CALL_CHECK_N_RETURN_ERROR( inv_dmp_open() );
    CALL_CHECK_N_RETURN_ERROR(inv_set_mpu_sensors(INV_NINE_AXIS));

    /***********************/
    /* advanced algorithms */
    /***********************/
    /* The Aichi and AKM libraries are only built against the
     * android tool chain */
    CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_no_motion());
    CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_from_gravity(true));
    CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_from_LPF(true));
    CALL_CHECK_N_RETURN_ERROR(inv_set_dead_zone_normal(true));
#ifdef ANDROID
    {
        struct mldl_cfg *mldl_cfg = inv_get_dl_config();
        if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] &&
            mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id == COMPASS_ID_AK8975) {
            CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external());
            CALL_CHECK_N_RETURN_ERROR(inv_external_slave_akm8975_open());
        } else if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] &&
                   mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id ==
                   COMPASS_ID_AMI306) {
            CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external());
            CALL_CHECK_N_RETURN_ERROR(inv_external_slave_ami306_open());
        } else if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] &&
                   mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id ==
                   COMPASS_ID_MMC328X) {
              
	 printf("Compass id is %d\n", mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id);
            CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external());
            CALL_CHECK_N_RETURN_ERROR(inv_external_slave_mmc3280_open());
       }else {
            CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion());
        }
    }
#else
    CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion());
#endif
    CALL_CHECK_N_RETURN_ERROR( inv_enable_temp_comp() );
    CALL_CHECK_N_RETURN_ERROR( inv_enable_fast_nomot() );

    CALL_CHECK_N_RETURN_ERROR( inv_set_motion_callback(onMotion) );
    CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_processed_callback(processedData) );

    /* Setup FIFO */
    CALL_CHECK_N_RETURN_ERROR( inv_send_quaternion(INV_32_BIT) );
    CALL_CHECK_N_RETURN_ERROR( inv_send_gyro(INV_ALL,INV_32_BIT) );
    CALL_CHECK_N_RETURN_ERROR( inv_send_accel(INV_ALL,INV_32_BIT) );
    CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_rate(20) );

    /* Check to see if interrupts are available.  If so use them */
    if (INV_SUCCESS  ==  interror) {
        CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_interrupt(true) );
        CALL_CHECK_N_RETURN_ERROR( inv_set_motion_interrupt(true) );
        CALL_CHECK_N_RETURN_ERROR( IntSetTimeout(handles[0], 100) );
        MPL_LOGI("Interrupts Configured\n");
        flag |= 0x04;
    } else {
        MPL_LOGI("Interrupts unavailable on this platform\n");
        flag &= ~0x04;
    }

    CALL_CHECK_N_RETURN_ERROR( inv_dmp_start() );

    //Loop  
    while (1) {

        usleep(8000);

        result = ConsoleKbhit();
        if (DEBUG_OUT)
            printf("_kbhit result : %d\n", result);
        if (result) {
            key = ConsoleGetChar();
            if (DEBUG_OUT)
                printf("getchar key : %c (%d)\n", key, key);
        } else{
            key = 0; 
        } 

        if (key == 'q') {
            printf("quit...\n");
            break;
        } else if (key == '0') {
            printf("flag=0\n");
            flag = 0; 
        } else if (key == '1') {
            if (flag & 1) {
                MPL_LOGI("flag &= ~1 - who am i\n");
                flag &= ~1;
            } else {
                MPL_LOGI("flag |= 1 - who am i\n");
                flag |= 1;
            }
        } else if (key == '2') {
            if (flag & 2) {
                MPL_LOGI("flag &= ~2 - inv_update_data()\n");
                flag &= ~2;
            } else {
                MPL_LOGI("flag |= 2 - inv_update_data()\n");
                flag |= 2;
            }
        } else if (key == '4') {
            if (flag & 4) {
                MPL_LOGI("flag &= ~4 - IntProcess()\n");
                flag &= ~4;
            } else {
                MPL_LOGI("flag |= 4 - IntProcess()\n");
                flag |= 4;
            }
        } else if (key == 'a') {
            if (flag & 0x80) {
                MPL_LOGI("flag &= ~0x80 - Quaternion\n");
                flag &= ~0x80;
            } else {
                MPL_LOGI("flag |= 0x80  - Quaternion\n");
                flag |= 0x80;
            }
        } else if (key == 'b') {
            if (flag & 0x20) {
                printf("flag &= ~0x20 - dumpData()\n");
                flag &= ~0x20;
            } else {
                printf("flag |= 0x20 - dumpData()\n");
                flag |= 0x20;
            }
        } else if (key == 'S') {
            MPL_LOGI("run MPU Self-Test...\n");
            CALL_CHECK_N_RETURN_ERROR(inv_self_test_run());
            inv_sleep(5);
            continue;
        } else if (key == 'C') {
            MPL_LOGI("run MPU Calibration Test...\n");
            CALL_CHECK_N_RETURN_ERROR(inv_self_test_calibration_run());
            inv_sleep(5);
            continue;
        } else if (key == 'Z') {
            MPL_LOGI("run MPU Self-Test for Accel Z-Axis...\n");
            CALL_CHECK_N_RETURN_ERROR(inv_self_test_accel_z_run());
            inv_sleep(5);
            continue;
        } else if (key == 'h') {
            printf(
                "\n\n"
                "0   -   turn all the features OFF\n"
                "1   -   read WHO_AM_I\n"
                "2   -   call inv_update_data()\n"
                "4   -   call IntProcess()\n"
                "a   -   print Quaternion data\n"
                "b   -   Print raw accel and gyro data\n"
                "S   -   interrupt execution, run self-test\n"
                "C   -   interrupt execution, run calibration test\n"
                "Z   -   interrupt execution, run accel Z-axis test\n"
                "h   -   show this help\n"
                "\n\n"
                );
        } 

        if (flag & 0x01) {
            if (DEBUG_OUT)
                printf("inv_serial_readSingle(0x68,0,reg)\n");
            CALL_CHECK_N_RETURN_ERROR( 
                inv_serial_read(inv_get_serial_handle(), 0x68, 0, 1, reg) );
            printf("\nreg[0]=%02x", reg[0]);
        }
        if (flag & 0x02) {
            CALL_N_CHECK( inv_update_data() );
        }
        if (flag & 0x04) {
            data = InterruptPoll(handles, ARRAY_SIZE(handles), 0, 500000);
            InterruptPollDone(data);
        }
        if (flag & 0x20) {
            dumpData();
        }
    }    

    // Close Motion Library
    CALL_CHECK_N_RETURN_ERROR( inv_dmp_close() );
    CALL_CHECK_N_RETURN_ERROR( inv_serial_stop() );

    CALL_N_CHECK(IntClose(handles, ARRAY_SIZE(handles)));

    return INV_SUCCESS;
}