Example #1
0
/**
 *  @brief Set the output data rate for the particular configuration.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *             Config to modify with new ODR.
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param odr
 *             Output data rate in units of 1/1000Hz (mHz).
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int mma8450_set_odr(void *mlsl_handle,
		struct ext_slave_platform_data *pdata,
		struct mma8450_config *config,
		int apply,
		long odr)
{
	unsigned char bits;
	int result = INV_SUCCESS;

	if (odr > 200000) {
		config->odr = 400000;
		bits = 0x00;
	} else if (odr > 100000) {
		config->odr = 200000;
		bits = 0x04;
	} else if (odr > 50000) {
		config->odr = 100000;
		bits = 0x08;
	} else if (odr > 25000) {
		config->odr = 50000;
		bits = 0x0B;
	} else if (odr > 12500) {
		config->odr = 25000;
		bits = 0x40; /* Sleep -> Auto wake mode */
	} else if (odr > 1563) {
		config->odr = 12500;
		bits = 0x10;
	} else if (odr > 0) {
		config->odr = 1563;
		bits = 0x14;
	} else {
		config->ctrl_reg1 = 0; /* Set FS1.FS2 to Standby */
		config->odr = 0;
		bits = 0;
	}

	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x3);
	if (apply) {
		result = inv_serial_single_write(mlsl_handle, pdata->address,
				ACCEL_MMA8450_CTRL_REG1, 0);
		ERROR_CHECK(result);
		result = inv_serial_single_write(mlsl_handle, pdata->address,
				ACCEL_MMA8450_CTRL_REG1, config->ctrl_reg1);
		ERROR_CHECK(result);
		MPL_LOGV("ODR: %d mHz, 0x%02x\n",
			config->odr, (int)config->ctrl_reg1);
	}
	return result;
}
Example #2
0
/**
 *  @brief Set the output data rate for the particular configuration.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *             Config to modify with new ODR.
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param odr
 *             Output data rate in units of 1/1000Hz (mHz).
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int bma250_set_odr(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct bma250_config *config,
			  int apply,
			  long odr)
{
	int result = INV_SUCCESS;
	unsigned char reg_odr;

	/* Table uses bandwidth which is half the sample rate */
	odr = odr >> 1;
	if (odr >= 1000000) {
		reg_odr = 0x0F;
		config->odr = 2000000;
	} else if (odr >= 500000) {
		reg_odr = 0x0E;
		config->odr = 1000000;
	} else if (odr >= 250000) {
		reg_odr = 0x0D;
		config->odr = 500000;
	} else if (odr >= 125000) {
		reg_odr = 0x0C;
		config->odr = 250000;
	} else if (odr >= 62500) {
		reg_odr = 0x0B;
		config->odr = 125000;
	} else if (odr >= 31250) {
		reg_odr = 0x0A;
		config->odr = 62500;
	} else if (odr >= 15630) {
		reg_odr = 0x09;
		config->odr = 31250;
	} else {
		reg_odr = 0x08;
		config->odr = 15630;
	}

	if (apply) {
		MPL_LOGV("ODR: %d\n", config->odr);
		result = inv_serial_single_write(mlsl_handle, pdata->address,
				BMA250_ODR_REG, reg_odr);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
	}

	return result;
}
Example #3
0
/**
 * Sets the IRQ to fire when one of the IRQ events occur.  Threshold and
 * duration will not be used uless the type is MOT or NMOT.
 *
 * @param config configuration to apply to, suspend or resume
 * @param irq_type The type of IRQ.  Valid values are
 * - MPU_SLAVE_IRQ_TYPE_NONE
 * - MPU_SLAVE_IRQ_TYPE_MOTION
 * - MPU_SLAVE_IRQ_TYPE_DATA_READY
 */
static int kxtf9_set_irq(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct kxtf9_config *config,
			int apply,
			long irq_type)
{
	int result = ML_SUCCESS;
	struct kxtf9_private_data *private_data = pdata->private_data;

	config->irq_type = (unsigned char)irq_type;
	config->ctrl_reg1 &= ~0x22;
	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
		config->ctrl_reg1 |= 0x20;
		config->reg_int_cfg1 = 0x38;
		config->reg_int_cfg2 = 0x00;
	} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
		config->ctrl_reg1 |= 0x02;
		if ((unsigned long) config ==
			(unsigned long) &private_data->suspend)
			config->reg_int_cfg1 = 0x34;
		else
			config->reg_int_cfg1 = 0x24;
		config->reg_int_cfg2 = 0xE0;
	} else {
		config->reg_int_cfg1 = 0x00;
		config->reg_int_cfg2 = 0x00;
	}

	if (apply) {
		/* Must clear bit 7 before writing new configuration */
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					KXTF9_CTRL_REG1, 0x40);
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					KXTF9_INT_CTRL_REG1,
					config->reg_int_cfg1);
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					KXTF9_INT_CTRL_REG2,
					config->reg_int_cfg2);
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					KXTF9_CTRL_REG1,
					config->ctrl_reg1);
	}
	MPL_LOGV("CTRL_REG1: %lx, INT_CFG1: %lx, INT_CFG2: %lx\n",
		(unsigned long)config->ctrl_reg1,
		(unsigned long)config->reg_int_cfg1,
		(unsigned long)config->reg_int_cfg2);

	return result;
}
Example #4
0
/**
 * Set the Output data rate for the particular configuration
 *
 * @param config Config to modify with new ODR
 * @param odr Output data rate in units of 1/1000Hz
 */
static int lis331dlh_set_odr(void *mlsl_handle,
			     struct ext_slave_platform_data *pdata,
			     struct lis331dlh_config *config,
			     int apply, long odr)
{
	unsigned char bits;
	int result = INV_SUCCESS;

	if (odr > 400000) {
		config->odr = 1000000;
		bits = 0x38;
	} else if (odr > 100000) {
		config->odr = 400000;
		bits = 0x30;
	} else if (odr > 50000) {
		config->odr = 100000;
		bits = 0x28;
	} else if (odr > 10000) {
		config->odr = 50000;
		bits = 0x20;
	} else if (odr > 5000) {
		config->odr = 10000;
		bits = 0xC0;
	} else if (odr > 2000) {
		config->odr = 5000;
		bits = 0xB0;
	} else if (odr > 1000) {
		config->odr = 2000;
		bits = 0x80;
	} else if (odr > 500) {
		config->odr = 1000;
		bits = 0x60;
	} else if (odr > 0) {
		config->odr = 500;
		bits = 0x40;
	} else {
		config->odr = 0;
		bits = 0;
	}

	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7);
	lis331dlh_set_dur(mlsl_handle, pdata, config, apply, config->dur);
	MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
	if (apply)
		result = inv_serial_single_write(mlsl_handle, pdata->address,
						 LIS331_CTRL_REG1,
						 config->ctrl_reg1);
	return result;
}
/**
 *  @brief Set the output data rate for the particular configuration.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *             Config to modify with new ODR.
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param odr
 *             Output data rate in units of 1/1000Hz (mHz).
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int bma150_set_odr(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma150_config *config,
			int apply,
			long odr)
{
	unsigned char odr_bits = 0;
	unsigned char wup_bits = 0;
	int result = INV_SUCCESS;

	if (odr > 100000) {
		config->odr = 190000;
		odr_bits = 0x03;
	} else if (odr > 50000) {
		config->odr = 100000;
		odr_bits = 0x02;
	} else if (odr > 25000) {
		config->odr = 50000;
		odr_bits = 0x01;
	} else if (odr > 0) {
		config->odr = 25000;
		odr_bits = 0x00;
	} else {
		config->odr = 0;
		wup_bits = 0x00;
	}

	config->int_reg &= BMA150_INT_MASK_WUP;
	config->ctrl_reg &= BMA150_CTRL_MASK_ODR;
	config->ctrl_reg |= odr_bits;

	MPL_LOGV("ODR: %d\n", config->odr);
	if (apply) {
		result = inv_serial_single_write(mlsl_handle, pdata->address,
					BMA150_CTRL_REG, config->ctrl_reg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		result = inv_serial_single_write(mlsl_handle, pdata->address,
					BMA150_INT_REG, config->int_reg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
	}

	return result;
}
Example #6
0
/**
 * Set the Output data rate for the particular configuration
 *
 * @param config Config to modify with new ODR
 * @param odr Output data rate in units of 1/1000Hz
 */
static int lis3dh_set_odr(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct lis3dh_config *config,
			int apply,
			long odr)
{
	unsigned char bits;
	int result = ML_SUCCESS;

	if (odr > 400000) {
		config->odr = 1250000;
		bits = 0x90;
	} else if (odr > 200000) {
		config->odr = 400000;
		bits = 0x70;
	} else if (odr > 100000) {
		config->odr = 200000;
		bits = 0x60;
	} else if (odr > 50000) {
		config->odr = 100000;
		bits = 0x50;
	} else if (odr > 25000) {
		config->odr = 50000;
		bits = 0x40;
	} else if (odr > 10000) {
		config->odr = 25000;
		bits = 0x30;
	} else if (odr > 1000) {
		config->odr = 10000;
		bits = 0x20;
	} else if (odr > 500) {
		config->odr = 1000;
		bits = 0x10;
	} else {
		config->odr = 0;
		bits = 0;
	}

	config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0xf);
	lis3dh_set_dur(mlsl_handle, pdata,
			config, apply, config->dur);
	MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
	if (apply)
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					LIS3DH_CTRL_REG1,
					config->ctrl_reg1);
	return result;
}
/**
 *  @brief Set the output data rate for the particular configuration.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *             Config to modify with new ODR.
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param odr
 *             Output data rate in units of 1/1000Hz (mHz).
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int bma222_set_odr(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct bma222_config *config,
			  int apply,
			  long odr)
{
	int result = INV_SUCCESS;
	unsigned char reg_odr;

	if (odr >= 1000000) {
		reg_odr = 0x0F;
		config->odr = 1000000;
	} else if (odr >= 500000) {
		reg_odr = 0x0E;
		config->odr = 500000;
	} else if (odr >= 250000) {
		reg_odr = 0x0D;
		config->odr = 250000;
	} else if (odr >= 125000) {
		reg_odr = 0x0C;
		config->odr = 125000;
	} else if (odr >= 62500) {
		reg_odr = 0x0B;
		config->odr = 62500;
	} else if (odr >= 32000) {
		reg_odr = 0x0A;
		config->odr = 32000;
	} else if (odr >= 16000) {
		reg_odr = 0x09;
		config->odr = 16000;
	} else {
		reg_odr = 0x08;
		config->odr = 8000;
	}

	if (apply) {
		MPL_LOGV("ODR: %d\n", config->odr);
		result = inv_serial_single_write(mlsl_handle, pdata->address,
					ADXL34X_ODR_REG, reg_odr);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
	}
	return result;
}
/**
 *  @brief Set the output data rate for the particular configuration.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *             Config to modify with new ODR.
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param odr
 *             Output data rate in units of 1/1000Hz (mHz).
 *
 *  @return ML_SUCCESS if successful or a non-zero error code.
 */
static int bma250_set_odr(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct bma250_config *config,
			  int apply,
			  long odr)
{
	int result = ML_SUCCESS;
	unsigned char reg_odr;

	if (odr >= 1000000) {
		reg_odr = 0x0F;
		config->odr = 1000000;
	} else if (odr >= 500000) {
		reg_odr = 0x0E;
		config->odr = 500000;
	} else if (odr >= 250000) {
		reg_odr = 0x0D;
		config->odr = 250000;
	} else if (odr >= 125000) {
		reg_odr = 0x0C;
		config->odr = 125000;
	} else if (odr >= 62500) {
		reg_odr = 0x0B;
		config->odr = 62500;
	} else if (odr >= 31250) {
		reg_odr = 0x0A;
		config->odr = 31250;
	} else if (odr >= 15630) {
		reg_odr = 0x09;
		config->odr = 15630;
	} else {
		reg_odr = 0x08;
		config->odr = 7810;
	}

	if (apply) {
		MPL_LOGV("ODR: %d\n", config->odr);
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				BMA250_ODR_REG, reg_odr);
		ERROR_CHECK(result);
	}

	return result;
}
Example #9
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;
}
static int lis3dh_set_dur(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct lis3dh_config *config, int apply, long dur)
{
	int result = INV_SUCCESS;
	long reg_dur = (dur * config->odr) / 1000000L;
	config->dur = dur;

	if (reg_dur > LIS3DH_MAX_DUR)
		reg_dur = LIS3DH_MAX_DUR;

	config->reg_dur = (unsigned char)reg_dur;
	MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
	if (apply)
		result = inv_serial_single_write(mlsl_handle, pdata->address,
						 LIS3DH_INT1_DURATION,
						 (unsigned char)reg_dur);
	return result;
}
static int lis3dh_set_ths(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct lis3dh_config *config, int apply, long ths)
{
	int result = INV_SUCCESS;
	if ((unsigned int)ths > 1000 * config->fsr)
		ths = (long)1000 * config->fsr;

	if (ths < 0)
		ths = 0;

	config->ths = ths;
	config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
	if (apply)
		result = inv_serial_single_write(mlsl_handle, pdata->address,
						 LIS3DH_INT1_THS,
						 config->reg_ths);
	return result;
}
Example #12
0
/**
 *  @brief read the sensor data from the device.
 *
 *  @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.
 *  @param data
 *             a buffer to store the data read.
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int mma8450_read(void *mlsl_handle,
		 struct ext_slave_descr *slave,
		 struct ext_slave_platform_data *pdata, unsigned char *data)
{
	int result;
	unsigned char local_data[4];	/* Status register + 3 bytes data */
	result = inv_serial_read(mlsl_handle, pdata->address,
				0x00,
				 sizeof(local_data), local_data);
	ERROR_CHECK(result);
	memcpy(data, &local_data[1], (slave->read_len) - 1);
		MPL_LOGV("Data Not Ready: %02x %02x %02x %02x\n",
			local_data[0],
			local_data[1],
			local_data[2],
			local_data[3]);
	if (!(local_data[0] & 0x04))
		result = INV_ERROR_ACCEL_DATA_NOT_READY;
	return result;
}
Example #13
0
static int kxtf9_set_ths(void *mlsl_handle,
			 struct ext_slave_platform_data *pdata,
			 struct kxtf9_config *config, int apply, long ths)
{
	int result = INV_SUCCESS;
	if ((ths * KXTF9_THS_COUNTS_P_G / 1000) > KXTF9_MAX_THS)
		ths = (long)(KXTF9_MAX_THS * 1000) / KXTF9_THS_COUNTS_P_G;

	if (ths < 0)
		ths = 0;

	config->ths = ths;
	config->reg_ths = (unsigned char)
	    ((long)(ths * KXTF9_THS_COUNTS_P_G) / 1000);
	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
	if (apply)
		result = inv_serial_single_write(mlsl_handle, pdata->address,
						 KXTF9_WUF_THRESH,
						 config->reg_ths);
	return result;
}
Example #14
0
static int kxtf9_set_dur(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct kxtf9_config *config,
			int apply,
			long dur)
{
	int result = ML_SUCCESS;
	long reg_dur = (dur * config->odr) / 1000000;
	config->dur = dur;

	if (reg_dur > KXTF9_MAX_DUR)
		reg_dur = KXTF9_MAX_DUR;

	config->reg_dur = (unsigned char) reg_dur;
	MPL_LOGV("DUR: %d, 0x%02x\n", config->dur, (int)config->reg_dur);
	if (apply)
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					KXTF9_WUF_TIMER,
					(unsigned char)reg_dur);
	return result;
}
Example #15
0
static int mpu6050_set_fsr(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct mpu6050_config *config, long apply, long fsr)
{
	unsigned char fsr_mask;
	int result;

	if (fsr <= 2000) {
		config->fsr = 2000;
		fsr_mask = 0x00;
	} else if (fsr <= 4000) {
		config->fsr = 4000;
		fsr_mask = 0x08;
	} else if (fsr <= 8000) {
		config->fsr = 8000;
		fsr_mask = 0x10;
	} else { /* fsr = [8001, oo) */
		config->fsr = 16000;
		fsr_mask = 0x18;
	}

	if (apply) {
		unsigned char reg;
		result = inv_serial_read(mlsl_handle, pdata->address,
					 MPUREG_ACCEL_CONFIG, 1, &reg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		result = inv_serial_single_write(mlsl_handle, pdata->address,
						 MPUREG_ACCEL_CONFIG,
						 reg | fsr_mask);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		MPL_LOGV("FSR: %d\n", config->fsr);
	}
	return 0;
}
/**
 *  @brief Set the full scale range of the accels
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *             pointer to configuration.
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param fsr
 *             requested full scale range.
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int bma150_set_fsr(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct bma150_config *config,
			  int apply,
			  long fsr)
{
	unsigned char fsr_bits;
	int result = INV_SUCCESS;

	if (fsr <= 2048) {
		fsr_bits = 0x00;
		config->fsr = 2048;
	} else if (fsr <= 4096) {
		fsr_bits = 0x08;
		config->fsr = 4096;
	} else {
		fsr_bits = 0x10;
		config->fsr = 8192;
	}

	config->ctrl_reg &= BMA150_CTRL_MASK_FSR;
	config->ctrl_reg |= fsr_bits;

	MPL_LOGV("FSR: %d\n", config->fsr);
	if (apply) {
		result = inv_serial_single_write(mlsl_handle, pdata->address,
				BMA150_CTRL_REG, config->ctrl_reg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		result = inv_serial_single_write(mlsl_handle, pdata->address,
				BMA150_CTRL_REG, config->ctrl_reg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
	}
	return result;
}
Example #17
0
static int lis331dlh_set_ths(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct lis331dlh_config *config,
			int apply,
			long ths)
{
	int result = ML_SUCCESS;
	if ((unsigned int) ths >= config->fsr)
		ths = (long) config->fsr - 1;

	if (ths < 0)
		ths = 0;

	config->ths = ths;
	config->reg_ths = (unsigned char)(long)((ths * 128L) / (config->fsr));
	MPL_LOGV("THS: %d, 0x%02x\n", config->ths, (int)config->reg_ths);
	if (apply)
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					LIS331_INT1_THS,
					config->reg_ths);
	return result;
}
static int mpu6050_set_irq(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct mpu6050_config *config, long apply,
			  long irq_type)
{

	/* HACK, no need for interrupts for MPU6050 accel
		- use of soft interrupt is required */
#if 0
	switch (irq_type) {
	case MPU_SLAVE_IRQ_TYPE_DATA_READY:
		config->irq_type = irq_type;
		reg_int_cfg = BIT_RAW_RDY_EN;
		break;
	/* todo: add MOTION, NO_MOTION, and FREEFALL */
	case MPU_SLAVE_IRQ_TYPE_NONE:
		/* Do nothing, not even set the interrupt because it is
		   shared with the gyro */
		config->irq_type = irq_type;
		return 0;
	default:
		return INV_ERROR_INVALID_PARAMETER;
	}

	if (apply) {
		result = inv_serial_single_write(mlsl_handle, pdata->address,
						 MPUREG_INT_ENABLE,
						 reg_int_cfg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		MPL_LOGV("irq_type: %d\n", config->irq_type);
	}
#endif

	return 0;
}
/**
 *  @brief Set the full scale range of the accels
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *             pointer to configuration.
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param fsr
 *             requested full scale range in milli gees (mg).
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int adxl34x_set_fsr(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct adxl34x_config *config,
			  int apply,
			  long fsr)
{
	int result = INV_SUCCESS;

	if (fsr <= 2000) {
		config->fsr_reg_mask = 0x00;
		config->fsr = 2000;
	} else if (fsr <= 4000) {
		config->fsr_reg_mask = 0x01;
		config->fsr = 4000;
	} else if (fsr <= 8000) {
		config->fsr_reg_mask = 0x02;
		config->fsr = 8000;
	} else { /* 8001 -> oo */
		config->fsr_reg_mask = 0x03;
		config->fsr = 16000;
	}

	if (apply) {
		unsigned char reg_df;
		result = inv_serial_read(mlsl_handle, pdata->address,
				ADXL34X_DATAFORMAT_REG, 1, &reg_df);
		reg_df &= ~ADXL34X_DATAFORMAT_FSR_MASK;
		result = inv_serial_single_write(mlsl_handle, pdata->address,
				ADXL34X_DATAFORMAT_REG,
				reg_df | config->fsr_reg_mask);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		MPL_LOGV("FSR: %d mg\n", config->fsr);
	}
	return result;
}
Example #20
0
/**
 *  @internal
 *  @brief  used to get the FIFO data.
 *  @param  length  
 *              Number of bytes to read from the FIFO.
 *  @param  buffer  
 *              the bytes of FIFO data.
 *              Note that this buffer <b>must</b> be large enough
 *              to store and additional trailing FIFO footer when 
 *              expected.  The callers must make sure enough space
 *              is allocated.
 *  @return number of valid bytes of data.
**/
uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer)
{
    INVENSENSE_FUNC_START;
    inv_error_t result;
    uint_fast16_t inFifo;
    uint_fast16_t toRead;
    int_fast8_t kk;

    toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount;
    /*---- make sure length is correct ----*/
    if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) {
        fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER;
        return 0;
    }

    result = inv_get_fifo_length(&inFifo);
    if (INV_SUCCESS != result) {
        fifo_objHW.fifoError = result;
        return 0;
    }
    // fifo_objHW.fifoCount is the footer size left in the buffer, or 
    //      0 if this is the first time reading the fifo since it was reset
    if (inFifo < length + fifo_objHW.fifoCount) {
        fifo_objHW.fifoError = INV_SUCCESS;
        return 0;
    }
    // if a trailing fifo count is expected - start storing data 2 bytes before
    result =
        inv_read_fifo(fifo_objHW.fifoCount >
                      0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead);
    if (INV_SUCCESS != result) {
        fifo_objHW.fifoError = result;
        return 0;
    }
    // Make sure the fifo didn't overflow before or during the read
    result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
                             MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow);
    if (INV_SUCCESS != result) {
        fifo_objHW.fifoError = result;
        return 0;
    }

    if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) {
        MPL_LOGV("Resetting Fifo : Overflow\n");
        inv_reset_fifo();
        fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW;
        return 0;
    }

    /* Check the Footer value to give us a chance at making sure data 
     * didn't get corrupted */
    for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) {
        if (buffer[kk] != gFifoFooter[kk]) {
            MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n",
                     buffer[0], buffer[1]);
            _fifoDebug(char out[200];
                       MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount);
                       sprintf(out, "0x");
                       for (kk = 0; kk < (int)toRead; kk++) {
                       sprintf(out, "%s%02X", out, buffer[kk]);}
                       MPL_LOGW("%s\n", out);)
                inv_reset_fifo();
            fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER;
            return 0;
        }
Example #21
0
static int bma250_set_irq(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma250_config *config,
			int apply,
			long irq_type)
{
	unsigned char irq_bits = 0;
	int result = ML_SUCCESS;

	
	return ML_SUCCESS;

	if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION)
		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;

	config->irq_type = (unsigned char)irq_type;

	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
		irq_bits = 0x20;
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
	} else {
		irq_bits = 0x00;
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
	}

	config->int_reg &= ACCEL_BOSCH_INT_MASK_IRQ;
	config->int_reg |= irq_bits;

	if (apply) {

#ifndef CONFIG_CIR_ALWAYS_READY
		if (!config->power_mode) {
			
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_REG_SOFT_RESET,
					0xB6);
			ERROR_CHECK(result);
			MLOSSleep(1);
		}

#endif

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_CTRL_REG, config->ctrl_reg);
		ERROR_CHECK(result);

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_INT_REG, config->int_reg);
		ERROR_CHECK(result);

#ifdef CONFIG_CIR_ALWAYS_READY
		if (!config->power_mode && !cir_flag) {
#else
		if (!config->power_mode) {
#endif
			result = MLSLSerialWriteSingle(mlsl_handle,
				pdata->address, BOSCH_PWR_REG, 0x80);
			ERROR_CHECK(result);
			MLOSSleep(1);
		} else {
			result = set_normal_mode(mlsl_handle, pdata);
			ERROR_CHECK(result);
		}
	}
	return result;
}

static int bma250_set_odr(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma250_config *config,
			int apply,
			long odr)
{
	unsigned char odr_bits = 0;
	unsigned char wup_bits = 0;
	unsigned char read_from_chip_bw = 0;
	int result = ML_SUCCESS;

	
	if (odr > 100000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 50000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 20000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 15000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 0) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else {
		config->odr = 0;
		wup_bits = 0x00;
		config->power_mode = 0;
	}

	switch (config->power_mode) {
	case 1:
		config->bw_reg &= BMA250_BW_MASK;
		config->bw_reg |= odr_bits;
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
		break;
	case 0:
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
		config->int_reg |= wup_bits;
		break;
	default:
		break;
	}

	MPL_LOGV("ODR: %d \n", config->odr);
	if (apply) {
#ifndef CONFIG_CIR_ALWAYS_READY
#if 0 
			
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_REG_SOFT_RESET,
					0xB6);
			ERROR_CHECK(result);
			MLOSSleep(1);
#endif
#endif
			result = MLSLSerialRead(mlsl_handle,
					pdata->address, BMA250_BW_REG, 1,
					&read_from_chip_bw);
			ERROR_CHECK(result);

			if (odr_bits != read_from_chip_bw) {
				D("%s: Really set ODR to %d\n", __func__,
					config->odr);
				result = MLSLSerialWriteSingle(mlsl_handle,
						pdata->address, BMA250_BW_REG,
						config->bw_reg);
				ERROR_CHECK(result);
				
				MLOSSleep(25);
			}

			

			if (!config->power_mode) {
				result = MLSLSerialWriteSingle(mlsl_handle,
						pdata->address,	BOSCH_PWR_REG,
						0x80);
				ERROR_CHECK(result);
				MLOSSleep(1);
			} else {
#if 0 
				result = set_normal_mode(mlsl_handle, pdata);
				ERROR_CHECK(result);
#endif
			}
	}

	return result;
}
Example #22
0
static int bma250_set_odr(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma250_config *config,
			int apply,
			long odr)
{
	unsigned char odr_bits = 0;
	unsigned char wup_bits = 0;
	int result = ML_SUCCESS;

	
	if (odr > 100000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 50000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 20000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 15000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 0) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else {
		config->odr = 0;
		wup_bits = 0x00;
		config->power_mode = 0;
	}

	switch (config->power_mode) {
	case 1:
		config->bw_reg &= BMA250_BW_MASK;
		config->bw_reg |= odr_bits;
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
		break;
	case 0:
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
		config->int_reg |= wup_bits;
		break;
	default:
		break;
	}

	MPL_LOGV("ODR: %d \n", config->odr);
	if (apply) {
			
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_REG_SOFT_RESET,
					0xB6);
			ERROR_CHECK(result);
			MLOSSleep(1);

			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_BW_REG,
					config->bw_reg);
			ERROR_CHECK(result);

			

			if (!config->power_mode) {
				result = MLSLSerialWriteSingle(mlsl_handle,
						pdata->address,	BOSCH_PWR_REG,
						0x80);
				ERROR_CHECK(result);
				MLOSSleep(1);
			} else {
				result = set_normal_mode(mlsl_handle, pdata);
				ERROR_CHECK(result);
			}
	}

	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 */
}
Example #24
0
void mldl_print_cfg(struct mldl_cfg *mldl_cfg)
{
	struct mpu_gyro_cfg	*mpu_gyro_cfg	= mldl_cfg->mpu_gyro_cfg;
	struct mpu_offsets	*mpu_offsets	= mldl_cfg->mpu_offsets;
	struct mpu_chip_info	*mpu_chip_info	= mldl_cfg->mpu_chip_info;
	struct inv_mpu_cfg	*inv_mpu_cfg	= mldl_cfg->inv_mpu_cfg;
	struct inv_mpu_state	*inv_mpu_state	= mldl_cfg->inv_mpu_state;
	struct ext_slave_descr	**slave		= mldl_cfg->slave;
	struct mpu_platform_data *pdata		= mldl_cfg->pdata;
	struct ext_slave_platform_data **pdata_slave = mldl_cfg->pdata_slave;
	int ii;

	/* mpu_gyro_cfg */
	MPL_LOGV("int_config     = %02x\n", mpu_gyro_cfg->int_config);
	MPL_LOGV("ext_sync       = %02x\n", mpu_gyro_cfg->ext_sync);
	MPL_LOGV("full_scale     = %02x\n", mpu_gyro_cfg->full_scale);
	MPL_LOGV("lpf            = %02x\n", mpu_gyro_cfg->lpf);
	MPL_LOGV("clk_src        = %02x\n", mpu_gyro_cfg->clk_src);
	MPL_LOGV("divider        = %02x\n", mpu_gyro_cfg->divider);
	MPL_LOGV("dmp_enable     = %02x\n", mpu_gyro_cfg->dmp_enable);
	MPL_LOGV("fifo_enable    = %02x\n", mpu_gyro_cfg->fifo_enable);
	MPL_LOGV("dmp_cfg1       = %02x\n", mpu_gyro_cfg->dmp_cfg1);
	MPL_LOGV("dmp_cfg2       = %02x\n", mpu_gyro_cfg->dmp_cfg2);
	/* mpu_offsets */
	MPL_LOGV("tc[0]      = %02x\n", mpu_offsets->tc[0]);
	MPL_LOGV("tc[1]      = %02x\n", mpu_offsets->tc[1]);
	MPL_LOGV("tc[2]      = %02x\n", mpu_offsets->tc[2]);
	MPL_LOGV("gyro[0]    = %04x\n", mpu_offsets->gyro[0]);
	MPL_LOGV("gyro[1]    = %04x\n", mpu_offsets->gyro[1]);
	MPL_LOGV("gyro[2]    = %04x\n", mpu_offsets->gyro[2]);

	/* mpu_chip_info */
	MPL_LOGV("addr            = %02x\n", mldl_cfg->mpu_chip_info->addr);

	MPL_LOGV("silicon_revision = %02x\n", mpu_chip_info->silicon_revision);
	MPL_LOGV("product_revision = %02x\n", mpu_chip_info->product_revision);
	MPL_LOGV("product_id       = %02x\n", mpu_chip_info->product_id);
	MPL_LOGV("gyro_sens_trim   = %02x\n", mpu_chip_info->gyro_sens_trim);
	MPL_LOGV("accel_sens_trim  = %02x\n", mpu_chip_info->accel_sens_trim);

	MPL_LOGV("requested_sensors = %04x\n", inv_mpu_cfg->requested_sensors);
	MPL_LOGV("ignore_system_suspend= %04x\n",
		inv_mpu_cfg->ignore_system_suspend);
	MPL_LOGV("status = %04x\n", inv_mpu_state->status);
	MPL_LOGV("i2c_slaves_enabled= %04x\n",
		inv_mpu_state->i2c_slaves_enabled);

	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
		if (!slave[ii])
			continue;
		MPL_LOGV("SLAVE %d:\n", ii);
		MPL_LOGV("    suspend  = %02x\n", (int)slave[ii]->suspend);
		MPL_LOGV("    resume   = %02x\n", (int)slave[ii]->resume);
		MPL_LOGV("    read     = %02x\n", (int)slave[ii]->read);
		MPL_LOGV("    type     = %02x\n", slave[ii]->type);
		MPL_LOGV("    reg      = %02x\n", slave[ii]->read_reg);
		MPL_LOGV("    len      = %02x\n", slave[ii]->read_len);
		MPL_LOGV("    endian   = %02x\n", slave[ii]->endian);
		MPL_LOGV("    range.mantissa= %02x\n",
			slave[ii]->range.mantissa);
		MPL_LOGV("    range.fraction= %02x\n",
			slave[ii]->range.fraction);
	}

	for (ii = 0; ii < EXT_SLAVE_NUM_TYPES; ii++) {
		if (!pdata_slave[ii])
			continue;
		MPL_LOGV("PDATA_SLAVE[%d]\n", ii);
		MPL_LOGV("    irq        = %02x\n", pdata_slave[ii]->irq);
		MPL_LOGV("    adapt_num  = %02x\n", pdata_slave[ii]->adapt_num);
		MPL_LOGV("    bus        = %02x\n", pdata_slave[ii]->bus);
		MPL_LOGV("    address    = %02x\n", pdata_slave[ii]->address);
		MPL_LOGV("    orientation=\n"
			"                            %2d %2d %2d\n"
			"                            %2d %2d %2d\n"
			"                            %2d %2d %2d\n",
			pdata_slave[ii]->orientation[0],
			pdata_slave[ii]->orientation[1],
			pdata_slave[ii]->orientation[2],
			pdata_slave[ii]->orientation[3],
			pdata_slave[ii]->orientation[4],
			pdata_slave[ii]->orientation[5],
			pdata_slave[ii]->orientation[6],
			pdata_slave[ii]->orientation[7],
			pdata_slave[ii]->orientation[8]);
	}

	MPL_LOGV("pdata->int_config         = %02x\n", pdata->int_config);
	MPL_LOGV("pdata->level_shifter      = %02x\n", pdata->level_shifter);
	MPL_LOGV("pdata->orientation        =\n"
		 "                            %2d %2d %2d\n"
		 "                            %2d %2d %2d\n"
		 "                            %2d %2d %2d\n",
		 pdata->orientation[0], pdata->orientation[1],
		 pdata->orientation[2], pdata->orientation[3],
		 pdata->orientation[4], pdata->orientation[5],
		 pdata->orientation[6], pdata->orientation[7],
		 pdata->orientation[8]);
}
/**
 * Set the full scale range of the accels
 *
 * @param config pointer to configuration
 * @param fsr requested full scale range
 */
static int bma250_set_fsr(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma250_config *config,
			int apply,
			long fsr)
{
	unsigned char fsr_bits;
	int result = ML_SUCCESS;

	/* TO DO use dynamic range when stability safe */
	/*if (fsr <= 2048) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else if (fsr <= 4096) {
		fsr_bits = 0x05;
		config->fsr = 4096;
	} else if (fsr <= 8192) {
		fsr_bits = 0x08;
		config->fsr = 8192;
	} else if (fsr <= 16384) {
		fsr_bits = 0x0C;
		config->fsr = 16384;
	} else {
		fsr_bits = 0x03;
		config->fsr = 2048;
	}*/
	if (fsr <= 2048) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else if (fsr <= 4096) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else if (fsr <= 8192) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else if (fsr <= 16384) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else {
		fsr_bits = 0x03;
		config->fsr = 2048;
	}

	config->ctrl_reg &= ACCEL_BOSCH_CTRL_MASK_FSR;
	config->ctrl_reg |= fsr_bits;

	MPL_LOGV("FSR: %d \n", config->fsr);
	if (apply) {

		if (!config->power_mode) {
			/* BMA250: Software reset */
			result = MLSLSerialWriteSingle(mlsl_handle,
				pdata->address, BMA250_REG_SOFT_RESET, 0xB6);
			ERROR_CHECK(result);
			MLOSSleep(1);
		}

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_CTRL_REG, config->ctrl_reg);
		ERROR_CHECK(result);

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_CTRL_REG, config->ctrl_reg);
		ERROR_CHECK(result);

		if (!config->power_mode) {
			result = MLSLSerialWriteSingle(mlsl_handle,
				pdata->address,	BOSCH_PWR_REG, 0x80);
			ERROR_CHECK(result);
			MLOSSleep(1);
		}
	}
	return result;
}
/**
 * Set the Output data rate for the particular configuration
 *
 * @param config Config to modify with new ODR
 * @param odr Output data rate in units of 1/1000Hz
 */
static int bma250_set_odr(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma250_config *config,
			int apply,
			long odr)
{
	unsigned char odr_bits = 0;
	unsigned char wup_bits = 0;
	int result = ML_SUCCESS;

	/* TO DO use dynamic bandwidth when stability safe */
	/*if (odr > 100000) {
		config->odr = 125000;
		odr_bits = 0x0C;
		config->power_mode = 1;
	} else if (odr > 50000) {
		config->odr = 62500;
		odr_bits = 0x0B;
		config->power_mode = 1;
	} else if (odr > 20000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 15000) {
		config->odr = 15630;
		odr_bits = 0x09;
		config->power_mode = 1;
	} else if (odr > 0) {
		config->odr = 7810;
		odr_bits = 0x08;
		config->power_mode = 1;
	} else {
		config->odr = 0;
		wup_bits = 0x00;
		config->power_mode = 0;
	}*/
	if (odr > 100000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 50000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 20000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 15000) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else if (odr > 0) {
		config->odr = 31250;
		odr_bits = 0x0A;
		config->power_mode = 1;
	} else {
		config->odr = 0;
		wup_bits = 0x00;
		config->power_mode = 0;
	}

	switch (config->power_mode) {
	case 1:
		config->bw_reg &= BMA250_BW_MASK;
		config->bw_reg |= odr_bits;
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
		break;
	case 0:
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
		config->int_reg |= wup_bits;
		break;
	default:
		break;
	}

	MPL_LOGV("ODR: %d \n", config->odr);
	if (apply) {
			/* BMA250: Software reset */
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_REG_SOFT_RESET,
					0xB6);
			ERROR_CHECK(result);
			MLOSSleep(1);

			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_BW_REG,
					config->bw_reg);
			ERROR_CHECK(result);

			/* TODO Use irq when necessary */
			/*
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address,	BOSCH_INT_REG,
					config->int_reg);
			ERROR_CHECK(result);*/

			if (!config->power_mode) {
				result = MLSLSerialWriteSingle(mlsl_handle,
						pdata->address,	BOSCH_PWR_REG,
						0x80);
				ERROR_CHECK(result);
				MLOSSleep(1);
			}
	}

	return result;
}
Example #27
0
/**
 *  @brief  Get a sample of compass data from the device.
 *  @param  data
 *              the buffer to store the compass raw data for
 *              X, Y, and Z axes.
 *  @return INV_SUCCESS or a non-zero error code.
 */
inv_error_t inv_get_compass_data(long *data)
{
    inv_error_t result;
    int ii;
    struct mldl_cfg *mldl_cfg = inv_get_dl_config();

    unsigned char *tmp = inv_obj.mag->raw_data;

    if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len >
        sizeof(inv_obj.mag->raw_data)) {
        LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
        return INV_ERROR_INVALID_CONFIGURATION;
    }

    if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus ==
        EXT_SLAVE_BUS_PRIMARY ||
        !(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
        /*--- read the compass sensor data.
          The compass read function may return an INV_ERROR_COMPASS_* errors
          when the data is not ready (read/refresh frequency mismatch) or 
          the internal data sampling timing of the device was not respected. 
          Returning the error code will make the sensor fusion supervisor 
          ignore this compass data sample. ---*/
        result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
                                                    inv_get_serial_handle(),
                                                    inv_get_serial_handle(),
                                                    tmp);
        if (result) {
            if (COMPASS_DEBUG) {
                MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
            }
            return result;
        }
        for (ii = 0; ii < 3; ii++) {
            if (EXT_SLAVE_BIG_ENDIAN ==
                mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->endian)
                data[ii] =
                    ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
            else
                data[ii] =
                    ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
        }
    } else {
#if defined CONFIG_MPU_SENSORS_MPU6050A2 ||     \
    defined CONFIG_MPU_SENSORS_MPU6050B1
        result = inv_get_external_sensor_data(data, 3);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
#if defined CONFIG_MPU_SENSORS_MPU6050A2
        {
            static unsigned char first = true;
            // one-off write to AKM
            if (first) {
                unsigned char regs[] = {
                    // beginning Mantis register for one-off slave R/W
                    MPUREG_I2C_SLV4_ADDR,
                    // the slave to write to
                    mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->address,
                    // the register to write to
                    /*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->reg */
                    0x0A,
                    // the value to write
                    /*mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->trigger->value */
                    0x01,
                    // enable the write
                    0xC0
                };
                result =
                    inv_serial_write(inv_get_serial_handle(),
                                     mldl_cfg->mpu_chip_info->addr,
                                     ARRAY_SIZE(regs), regs);
                first = false;
            } else {
                unsigned char regs[] = {
                    MPUREG_I2C_SLV4_CTRL,
                    0xC0
                };
                result =
                    inv_serial_write(inv_get_serial_handle(),
                                     mldl_cfg->mpu_chip_info->addr,
                                     ARRAY_SIZE(regs), regs);
            }
        }
#endif
#else
        return INV_ERROR_INVALID_CONFIGURATION;
#endif                          // CONFIG_MPU_SENSORS_xxxx
    }
    data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]);
    data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]);
    data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]);

    return INV_SUCCESS;
}
Example #28
0
/** Turn on data logging to allow playback of same scenario at a later time.
* @param[in] file File to write to, must be open.
*/
void inv_turn_on_data_logging(FILE *file)
{
    MPL_LOGV("input data logging started\n");
    inv_data_builder.file = file;
    inv_data_builder.debug_mode = RD_RECORD;
}
Example #29
0
/** Turn off data logging to allow playback of same scenario at a later time.
* File passed to inv_turn_on_data_logging() must be closed after calling this.
*/
void inv_turn_off_data_logging()
{
    MPL_LOGV("input data logging stopped\n");
    inv_data_builder.debug_mode = RD_NO_DEBUG;
    inv_data_builder.file = NULL;
}
Example #30
0
static int bma250_set_fsr(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma250_config *config,
			int apply,
			long fsr)
{
	unsigned char fsr_bits;
	int result = ML_SUCCESS;

	
	if (fsr <= 2048) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else if (fsr <= 4096) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else if (fsr <= 8192) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else if (fsr <= 16384) {
		fsr_bits = 0x03;
		config->fsr = 2048;
	} else {
		fsr_bits = 0x03;
		config->fsr = 2048;
	}

	config->ctrl_reg &= ACCEL_BOSCH_CTRL_MASK_FSR;
	config->ctrl_reg |= fsr_bits;

	MPL_LOGV("FSR: %d \n", config->fsr);
	if (apply) {
#ifndef CONFIG_CIR_ALWAYS_READY
		if (!config->power_mode) {
			
			result = MLSLSerialWriteSingle(mlsl_handle,
				pdata->address, BMA250_REG_SOFT_RESET, 0xB6);
			ERROR_CHECK(result);
			MLOSSleep(1);
		}
#endif

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_CTRL_REG, config->ctrl_reg);
		ERROR_CHECK(result);

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_CTRL_REG, config->ctrl_reg);
		ERROR_CHECK(result);

		if (!config->power_mode) {
			result = MLSLSerialWriteSingle(mlsl_handle,
				pdata->address,	BOSCH_PWR_REG, 0x80);
			ERROR_CHECK(result);
			MLOSSleep(1);
		} else {
			result = set_normal_mode(mlsl_handle, pdata);
			ERROR_CHECK(result);
		}
	}
	return result;
}