/** * @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; }
/** * @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; }
/** * 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; }
/** * 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; }
/** * @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; }
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; }
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; }
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, ®); 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, ®_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; }
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; }
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 */ }
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; }
/** * @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; }
/** 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; }
/** 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; }
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; }