예제 #1
0
/**
 *  @brief one-time device driver initialization function.
 *         If the driver is built as a kernel module, this function will be
 *         called when the module is loaded in the kernel.
 *         If the driver is built-in in the kernel, this function will be
 *         called at boot time.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param slave
 *             a pointer to the slave descriptor data structure.
 *  @param pdata
 *             a pointer to the slave platform data.
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int mma8450_init(void *mlsl_handle,
			  struct ext_slave_descr *slave,
			  struct ext_slave_platform_data *pdata)
{
	struct mma8450_private_data *private_data;
	private_data = (struct mma8450_private_data *)
		inv_malloc(sizeof(struct mma8450_private_data));

	if (!private_data)
		return INV_ERROR_MEMORY_EXAUSTED;

	pdata->private_data = private_data;

	mma8450_set_odr(mlsl_handle, pdata, &private_data->suspend,
			FALSE, 0);
	mma8450_set_odr(mlsl_handle, pdata, &private_data->resume,
			FALSE, 200000);
	mma8450_set_fsr(mlsl_handle, pdata, &private_data->suspend,
			FALSE, 2000);
	mma8450_set_fsr(mlsl_handle, pdata, &private_data->resume,
			FALSE, 2000);
	mma8450_set_irq(mlsl_handle, pdata, &private_data->suspend,
			FALSE,
			MPU_SLAVE_IRQ_TYPE_NONE);
	mma8450_set_irq(mlsl_handle, pdata, &private_data->resume,
			FALSE,
			MPU_SLAVE_IRQ_TYPE_NONE);
	return INV_SUCCESS;
}
예제 #2
0
static int lis3dh_init(void *mlsl_handle,
		       struct ext_slave_descr *slave,
		       struct ext_slave_platform_data *pdata)
{
	inv_error_t result;
	long range;
	struct lis3dh_private_data *private_data;
	private_data = (struct lis3dh_private_data *)
	    inv_malloc(sizeof(struct lis3dh_private_data));

	if (!private_data)
		return INV_ERROR_MEMORY_EXAUSTED;

	pdata->private_data = private_data;

	private_data->resume.ctrl_reg1 = 0x67;
	private_data->suspend.ctrl_reg1 = 0x18;
	private_data->resume.mot_int1_cfg = 0x95;
	private_data->suspend.mot_int1_cfg = 0x2a;

	lis3dh_set_odr(mlsl_handle, pdata, &private_data->suspend, FALSE, 0);
	lis3dh_set_odr(mlsl_handle, pdata, &private_data->resume,
		       FALSE, 200000);

	range = RANGE_FIXEDPOINT_TO_LONG_MG(slave->range);
	lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
			FALSE, range);
	lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume,
			FALSE, range);

	lis3dh_set_ths(mlsl_handle, pdata, &private_data->suspend,
			FALSE, 80);
	lis3dh_set_ths(mlsl_handle, pdata, &private_data->resume,
			FALSE, 40);

	lis3dh_set_dur(mlsl_handle, pdata, &private_data->suspend,
			FALSE, 1000);
	lis3dh_set_dur(mlsl_handle, pdata, &private_data->resume,
			FALSE, 2540);

	lis3dh_set_irq(mlsl_handle, pdata, &private_data->suspend,
			FALSE, MPU_SLAVE_IRQ_TYPE_NONE);
	lis3dh_set_irq(mlsl_handle, pdata, &private_data->resume,
			FALSE, MPU_SLAVE_IRQ_TYPE_NONE);

	result = inv_serial_single_write(mlsl_handle, pdata->address,
					 LIS3DH_CTRL_REG1, 0x07);
	inv_sleep(6);

	return INV_SUCCESS;
}
예제 #3
0
static int mantis_init(void *mlsl_handle,
		       struct ext_slave_descr *slave,
		       struct ext_slave_platform_data *pdata)
{
	struct mantis_private_data *private_data;
	long range;

#ifdef CONFIG_MPU_SENSORS_MPU3050
	(void *)private_data;
	return INV_ERROR_INVALID_MODULE;
#endif

	private_data = (struct mantis_private_data *)
	    inv_malloc(sizeof(struct mantis_private_data));

	if (!private_data)
		return INV_ERROR_MEMORY_EXAUSTED;

	pdata->private_data = private_data;

	mantis_set_odr(mlsl_handle, pdata, &private_data->suspend,
		       FALSE, 0);
	mantis_set_odr(mlsl_handle, pdata, &private_data->resume,
		       FALSE, 200000);

	range = RANGE_FIXEDPOINT_TO_LONG_MG(slave->range);
	mantis_set_fsr(mlsl_handle, pdata, &private_data->suspend,
		       FALSE, range);
	mantis_set_fsr(mlsl_handle, pdata, &private_data->resume,
		       FALSE, range);

	mantis_set_irq(mlsl_handle, pdata, &private_data->suspend, FALSE,
		       MPU_SLAVE_IRQ_TYPE_NONE);
	mantis_set_irq(mlsl_handle, pdata, &private_data->resume, FALSE,
		       MPU_SLAVE_IRQ_TYPE_NONE);

	mantis_set_ths(mlsl_handle, pdata, &private_data->suspend, FALSE, 80);
	mantis_set_ths(mlsl_handle, pdata, &private_data->resume, FALSE, 40);

	mantis_set_dur(mlsl_handle, pdata, &private_data->suspend, FALSE, 1000);
	mantis_set_dur(mlsl_handle, pdata, &private_data->resume, FALSE, 2540);

	return INV_SUCCESS;
}
inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
{
    FILE *fp;
    inv_error_t result = INV_SUCCESS;
    size_t fsize;

    fp = fopen(MLCAL_FILE,"rb");
    if (fp == NULL) {
        MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
        return INV_ERROR_FILE_OPEN;
    }

    // obtain file size
    fseek (fp, 0 , SEEK_END);
    fsize = ftell (fp);
    rewind (fp);
  
    *calData = (unsigned char *)inv_malloc(fsize);
    if (*calData==NULL) {
        MPL_LOGE("Could not allocate buffer of %d bytes - "
                 "aborting\n", fsize);
        fclose(fp);
        return INV_ERROR_MEMORY_EXAUSTED;
    }

    *bytesRead = fread(*calData, 1, fsize, fp);
    if (*bytesRead != fsize) {
        MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
                 *bytesRead, fsize);
        result = INV_ERROR_FILE_READ;
        goto read_cal_end;
    }
    else {
        MPL_LOGI("Bytes read = %d", *bytesRead);
    }

read_cal_end:
    fclose(fp);
    return result;
}
/**
 *  @brief  Store runtime calibration data to a file
 *
 *  @pre    Must be in INV_STATE_DMP_OPENED state.
 *          inv_dmp_open() or inv_dmp_stop() must have been called.
 *          inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
 *          been called.
 *
 *  @return 0 or error code.
 */
inv_error_t inv_store_calibration(void)
{
    unsigned char *calData;
    inv_error_t result;
    size_t length;

    result = inv_get_mpl_state_size(&length);
    calData = (unsigned char *)inv_malloc(length);
    if (!calData) {
        MPL_LOGE("Could not allocate buffer of %d bytes - "
                 "aborting\n", length);
        return INV_ERROR_MEMORY_EXAUSTED;
    }
    else {
        MPL_LOGI("mpl state size = %d", length);
    }

    result = inv_save_mpl_states(calData, length);
    if (result != INV_SUCCESS) {
        MPL_LOGE("Could not save mpl states - "
                 "error %d - aborting\n", result);
        goto free_mem_n_exit;
    }
    else {
        MPL_LOGE("calData from inv_save_mpl_states, size=%d", 
                 strlen((char *)calData));
    }

    result = inv_write_cal(calData, length);
    if (result != INV_SUCCESS) {
        MPL_LOGE("Could not store calibrated data on file - "
                 "error %d - aborting\n", result);
        goto free_mem_n_exit;

    }

free_mem_n_exit:
    inv_free(calData);
    return result;
}
/**
 *  @brief  If requested via inv_test_setup_accel(), test the accelerometer
 *          biases and calculate the necessary bias correction.
 *  @param  mlsl_handle
 *              serial interface handle to allow serial communication with the
 *              device, both gyro and accelerometer.
 *  @param  enable_axis
 *              specify which axis has to be checked and corrected: provides
 *              a switch mode between 3 axis calibration and Z axis only
 *              calibration.
 *  @param  bias
 *              output pointer to store the initial bias calculation provided
 *              by the MPU Self Test.  Requires 3 elements to store accel X, Y,
 *              and Z axis bias.
 *  @param  gravity
 *              The gravity value given the parts' sensitivity: for example
 *              if the accelerometer is set to +/- 2 gee ==> the gravity
 *              value will be 2^14 = 16384.
 *  @param  perform_full_test
 *              If 1:
 *              calculates offsets and noise and compare it against set
 *              thresholds. The final exist status will reflect if any of the
 *              value is outside of the expected range.
 *              When 0;
 *              skip the noise calculation and pass/fail assessment; simply
 *              calculates the accel biases.
 *
 *  @return 0 on success. A non-zero error code on error.
 */
int test_accel(void *mlsl_handle, int enable_axes,
                   short *bias, long gravity,
                   uint_fast8_t perform_full_test)
{
    short *p_vals;
    float avg[3] = {0.f, 0.f, 0.f}, zg = 0.f;
    float rms[3];
    float accel_rms_thresh = 1000000.f; /* enourmous to make the test always
                                           passes - future deployment */
    int accel_error = false;
    const long sample_period = inv_get_sample_step_size_ms() * 1000;
    int ii;

    p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples);

    /* collect the samples  */
    for(ii = 0; ii < test_setup.accel_samples; ii++) {
        unsigned result = INV_ERROR_ACCEL_DATA_NOT_READY;
        int tries = 0;
        long accel_data[3];
        short *vals = &p_vals[3 * ii];

        /* ignore data not ready errors but don't try more than 5 times */
        while (result == INV_ERROR_ACCEL_DATA_NOT_READY && tries++ < 5) {
            result = inv_get_accel_data(accel_data);
            usleep(sample_period);
        }
        if (result || tries >= 5) {
            MPL_LOGV("cannot reliably fetch data from the accelerometer");
            accel_error = true;
            goto accel_early_exit;
        }
        vals[X] = (short)accel_data[X];
        vals[Y] = (short)accel_data[Y];
        vals[Z] = (short)accel_data[Z];
        avg[X] += 1.f * vals[X] / test_setup.accel_samples;
        avg[Y] += 1.f * vals[Y] / test_setup.accel_samples;
        avg[Z] += 1.f * vals[Z] / test_setup.accel_samples;
        if (VERBOSE_OUT)
            MPL_LOGI("Accel         : %+13d %+13d %+13d (LSB)\n",
                     vals[X], vals[Y], vals[Z]);
    }

    if (((enable_axes << 4) & INV_THREE_AXIS_ACCEL) == INV_THREE_AXIS_ACCEL) {
        MPL_LOGI("Accel biases  : %+13.3f %+13.3f %+13.3f (LSB)\n",
                 avg[X], avg[Y], avg[Z]);
        if (VERBOSE_OUT)
            MPL_LOGI("Accel biases  : %+13.3f %+13.3f %+13.3f (gee)\n",
                     avg[X] / gravity, avg[Y] / gravity, avg[Z] / gravity);

        bias[X] = FLOAT_TO_SHORT(avg[X]);
        bias[Y] = FLOAT_TO_SHORT(avg[Y]);
        zg = avg[Z] - g_z_sign * gravity;
        bias[Z] = FLOAT_TO_SHORT(zg);

        MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n",
                 bias[X], bias[Y], bias[Z]);
        if (VERBOSE_OUT)
            MPL_LOGI("Accel correct.: "
                     "%+13.3f %+13.3f %+13.3f (gee)\n",
                     1.f * bias[X] / gravity,
                     1.f * bias[Y] / gravity,
                     1.f * bias[Z] / gravity);

        if (perform_full_test) {
            /* accel RMS - for now the threshold is only indicative */
            for (ii = 0,
                     rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
                 ii <  test_setup.accel_samples; ii++) {
                short *vals = &p_vals[3 * ii];
                rms[X] += (vals[X] - avg[X]) * (vals[X] - avg[X]);
                rms[Y] += (vals[Y] - avg[Y]) * (vals[Y] - avg[Y]);
                rms[Z] += (vals[Z] - avg[Z]) * (vals[Z] - avg[Z]);
            }
            for (ii = 0; ii < 3; ii++) {
                if (rms[ii] >  accel_rms_thresh * accel_rms_thresh
                                * test_setup.accel_samples) {
                    MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold "
                             "(threshold = %.2f)\n", a_name[ii],
                             sqrt(rms[ii] / test_setup.accel_samples),
                             accel_rms_thresh);
                    accel_error = true;
                    goto accel_early_exit;
                }
            }
            MPL_LOGI("Accel RMS     : %+13.3f %+13.3f %+13.3f (LSB-rms)\n",
                     sqrt(rms[X] / DEF_N_ACCEL_SAMPLES),
                     sqrt(rms[Y] / DEF_N_ACCEL_SAMPLES),
                     sqrt(rms[Z] / DEF_N_ACCEL_SAMPLES));
        }
    } else {
        MPL_LOGI("Accel Z bias    : %+13.3f (LSB)\n", avg[Z]);
        if (VERBOSE_OUT)
            MPL_LOGI("Accel Z bias    : %+13.3f (gee)\n", avg[Z] / gravity);

        zg = avg[Z] - g_z_sign * gravity;
        bias[Z] = FLOAT_TO_SHORT(zg);

        MPL_LOGI("Accel Z correct.: %+13d (LSB)\n", bias[Z]);
        if (VERBOSE_OUT)
            MPL_LOGI("Accel Z correct.: "
                     "%+13.3f (gee)\n", 1.f * bias[Z] / gravity);
    }

accel_early_exit:
    if (accel_error) {
        bias[0] = bias[1] = bias[2] = 0;
        return (1);     /* error */
    }
    inv_free(p_vals);

    return (0);         /* success */
}
예제 #7
0
static int ak8975_init(void *mlsl_handle,
			  struct ext_slave_descr *slave,
			  struct ext_slave_platform_data *pdata)
{
	int result;
	unsigned char serial_data[COMPASS_NUM_AXES];

	struct ak8975_private_data *private_data;
	private_data = (struct ak8975_private_data *)
		inv_malloc(sizeof(struct ak8975_private_data));

	if (!private_data)
		return INV_ERROR_MEMORY_EXAUSTED;

	result = inv_serial_single_write(mlsl_handle, pdata->address,
				AK8975_REG_CNTL,
				AK8975_CNTL_MODE_POWER_DOWN);
	ERROR_CHECK(result);

	/* Wait at least 100us */
#ifdef __KERNEL__
	udelay(100);
#else
	inv_sleep(1);
#endif

	result = inv_serial_single_write(mlsl_handle, pdata->address,
				AK8975_REG_CNTL,
				AK8975_CNTL_MODE_FUSE_ROM_ACCESS);
	ERROR_CHECK(result);

	/* Wait at least 200us */
#ifdef __KERNEL__
	udelay(200);
#else
	inv_sleep(1);
#endif

	result = inv_serial_read(mlsl_handle, pdata->address,
				AK8975_REG_ASAX,
				COMPASS_NUM_AXES,
				serial_data);
	ERROR_CHECK(result);

	pdata->private_data = private_data;

	private_data->init.asa[0] = serial_data[0];
	private_data->init.asa[1] = serial_data[1];
	private_data->init.asa[2] = serial_data[2];

	result = inv_serial_single_write(mlsl_handle, pdata->address,
				AK8975_REG_CNTL,
				AK8975_CNTL_MODE_POWER_DOWN);
	ERROR_CHECK(result);

#ifdef __KERNEL__
	udelay(100);
#else
	inv_sleep(1);
#endif
	return INV_SUCCESS;
}