C++ MPL_LOGV函数代码示例

/**
 *  @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;
}

/**
 * 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;
}

/**
 * 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;
}

/**
 *  @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;
}

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;
}

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;
}

/**
 *  @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;
        }