int ak8975_suspend(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; result = inv_serial_single_write(mlsl_handle, pdata->address, AK8975_REG_CNTL, AK8975_CNTL_MODE_POWER_DOWN); inv_sleep(1); /* wait at least 100us */ ERROR_CHECK(result); return result; }
static int lis3dh_init(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { inv_error_t result; long range; struct lis3dh_private_data *private_data; private_data = (struct lis3dh_private_data *) inv_malloc(sizeof(struct lis3dh_private_data)); if (!private_data) return INV_ERROR_MEMORY_EXAUSTED; pdata->private_data = private_data; private_data->resume.ctrl_reg1 = 0x67; private_data->suspend.ctrl_reg1 = 0x18; private_data->resume.mot_int1_cfg = 0x95; private_data->suspend.mot_int1_cfg = 0x2a; lis3dh_set_odr(mlsl_handle, pdata, &private_data->suspend, FALSE, 0); lis3dh_set_odr(mlsl_handle, pdata, &private_data->resume, FALSE, 200000); range = RANGE_FIXEDPOINT_TO_LONG_MG(slave->range); lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend, FALSE, range); lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume, FALSE, range); lis3dh_set_ths(mlsl_handle, pdata, &private_data->suspend, FALSE, 80); lis3dh_set_ths(mlsl_handle, pdata, &private_data->resume, FALSE, 40); lis3dh_set_dur(mlsl_handle, pdata, &private_data->suspend, FALSE, 1000); lis3dh_set_dur(mlsl_handle, pdata, &private_data->resume, FALSE, 2540); lis3dh_set_irq(mlsl_handle, pdata, &private_data->suspend, FALSE, MPU_SLAVE_IRQ_TYPE_NONE); lis3dh_set_irq(mlsl_handle, pdata, &private_data->resume, FALSE, MPU_SLAVE_IRQ_TYPE_NONE); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG1, 0x07); inv_sleep(6); return INV_SUCCESS; }
int hmc5883_suspend(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; result = inv_serial_single_write(mlsl_handle, pdata->address, HMC_REG_MODE, HMC_MODE_SLEEP); ERROR_CHECK(result); inv_sleep(3); return result; }
int hscdtd004a_suspend(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; /* Power mode: stand-by */ result = inv_serial_single_write(mlsl_handle, pdata->address, COMPASS_HSCDTD004A_CTRL1, 0x00); ERROR_CHECK(result); inv_sleep(1); /* turn-off time */ return result; }
int hscdtd004a_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; /* Soft reset */ result = inv_serial_single_write(mlsl_handle, pdata->address, COMPASS_HSCDTD004A_CTRL3, 0x80); ERROR_CHECK(result); /* Normal state; Power mode: active */ result = inv_serial_single_write(mlsl_handle, pdata->address, COMPASS_HSCDTD004A_CTRL1, 0x82); ERROR_CHECK(result); /* Data ready enable */ result = inv_serial_single_write(mlsl_handle, pdata->address, COMPASS_HSCDTD004A_CTRL2, 0x7C); ERROR_CHECK(result); inv_sleep(1); /* turn-on time */ return result; }
int mantis_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; unsigned char reg; struct mantis_private_data *private_data = (struct mantis_private_data *)pdata->private_data; result = inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1, ®); ERROR_CHECK(result); #if defined(CONFIG_MPU_SENSORS_MPU6050B1) if (reg & BIT_SLEEP) { result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP); ERROR_CHECK(result); } #else if ((reg & BITS_PWRSEL) != BITS_PWRSEL) { result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, reg | BITS_PWRSEL); ERROR_CHECK(result); } #endif inv_sleep(2); result = inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, 1, ®); ERROR_CHECK(result); reg &= ~(BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA); result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, reg); ERROR_CHECK(result); /* settings */ result = mantis_set_fsr(mlsl_handle, pdata, &private_data->resume, TRUE, private_data->resume.fsr); ERROR_CHECK(result); result = mantis_set_odr(mlsl_handle, pdata, &private_data->resume, TRUE, private_data->resume.odr); ERROR_CHECK(result); result = mantis_set_irq(mlsl_handle, pdata, &private_data->resume, TRUE, private_data->resume.irq_type); ERROR_CHECK(result); /* motion, no_motion */ reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_THR, reg); ERROR_CHECK(result); reg = (unsigned char) ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths); result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_ZRMOT_THR, reg); ERROR_CHECK(result); reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_DUR, reg); ERROR_CHECK(result); reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_ZRMOT_DUR, reg); ERROR_CHECK(result); return result; }
static int lis3dh_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { inv_error_t result; unsigned char reg1; unsigned char reg2; struct lis3dh_private_data *private_data = pdata->private_data; result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG1, private_data->resume.ctrl_reg1); ERROR_CHECK(result); inv_sleep(6); /* Full Scale */ reg1 = 0x48; if (private_data->suspend.fsr == 16384) reg1 |= 0x30; else if (private_data->suspend.fsr == 8192) reg1 |= 0x20; else if (private_data->suspend.fsr == 4096) reg1 |= 0x10; else if (private_data->suspend.fsr == 2048) reg1 |= 0x00; result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG4, reg1); ERROR_CHECK(result); /* Configure high pass filter */ result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG2, 0x31); ERROR_CHECK(result); if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { reg1 = 0x10; reg2 = 0x00; } else if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) { reg1 = 0x40; reg2 = private_data->resume.mot_int1_cfg; } else { reg1 = 0x00; reg2 = 0x00; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG3, reg1); ERROR_CHECK(result); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_INT1_THS, private_data->resume.reg_ths); ERROR_CHECK(result); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_INT1_DURATION, private_data->resume.reg_dur); ERROR_CHECK(result); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_INT1_CFG, reg2); ERROR_CHECK(result); result = inv_serial_read(mlsl_handle, pdata->address, LIS3DH_CTRL_REG6, 1, ®1); ERROR_CHECK(result); return result; }
static int ak8975_init(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result; unsigned char serial_data[COMPASS_NUM_AXES]; struct ak8975_private_data *private_data; private_data = (struct ak8975_private_data *) inv_malloc(sizeof(struct ak8975_private_data)); if (!private_data) return INV_ERROR_MEMORY_EXAUSTED; result = inv_serial_single_write(mlsl_handle, pdata->address, AK8975_REG_CNTL, AK8975_CNTL_MODE_POWER_DOWN); ERROR_CHECK(result); /* Wait at least 100us */ #ifdef __KERNEL__ udelay(100); #else inv_sleep(1); #endif result = inv_serial_single_write(mlsl_handle, pdata->address, AK8975_REG_CNTL, AK8975_CNTL_MODE_FUSE_ROM_ACCESS); ERROR_CHECK(result); /* Wait at least 200us */ #ifdef __KERNEL__ udelay(200); #else inv_sleep(1); #endif result = inv_serial_read(mlsl_handle, pdata->address, AK8975_REG_ASAX, COMPASS_NUM_AXES, serial_data); ERROR_CHECK(result); pdata->private_data = private_data; private_data->init.asa[0] = serial_data[0]; private_data->init.asa[1] = serial_data[1]; private_data->init.asa[2] = serial_data[2]; result = inv_serial_single_write(mlsl_handle, pdata->address, AK8975_REG_CNTL, AK8975_CNTL_MODE_POWER_DOWN); ERROR_CHECK(result); #ifdef __KERNEL__ udelay(100); #else inv_sleep(1); #endif return INV_SUCCESS; }
// Main function int main(int argc, char *argv[]) { //unsigned short accelSlaveAddr = ACCEL_SLAVEADDR_INVALID; unsigned short platformId = ID_INVALID; unsigned short accelId = ID_INVALID; unsigned short compassId = ID_INVALID; unsigned char reg[32]; unsigned char *verStr; int result; int key = 0; int interror; struct mpuirq_data **data; const char *ints[] = { "/dev/mpuirq", /* INTSRC_MPU */ //"/dev/accelirq", /* INTSRC_AUX1 */ //"/dev/compassirq", /* INTSRC_AUX2 */ //"/dev/pressureirq", /* INTSRC_AUX3 */ }; int handles[ARRAY_SIZE(ints)]; CALL_N_CHECK( inv_get_version(&verStr) ); printf("%s\n", verStr); if(INV_SUCCESS == MenuHwChoice(&platformId, &accelId, &compassId)) { CALL_CHECK_N_RETURN_ERROR(SetupPlatform(platformId, accelId, compassId)); } CALL_CHECK_N_RETURN_ERROR( inv_serial_start("/dev/mpu") ); IntOpen(ints, handles, ARRAY_SIZE(ints)); if (handles[0] < 0) { printf("IntOpen failed\n"); interror = INV_ERROR; } else { interror = INV_SUCCESS; } CALL_CHECK_N_RETURN_ERROR( inv_dmp_open() ); CALL_CHECK_N_RETURN_ERROR(inv_set_mpu_sensors(INV_NINE_AXIS)); /***********************/ /* advanced algorithms */ /***********************/ /* The Aichi and AKM libraries are only built against the * android tool chain */ CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_no_motion()); CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_from_gravity(true)); CALL_CHECK_N_RETURN_ERROR(inv_enable_bias_from_LPF(true)); CALL_CHECK_N_RETURN_ERROR(inv_set_dead_zone_normal(true)); #ifdef ANDROID { struct mldl_cfg *mldl_cfg = inv_get_dl_config(); if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] && mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id == COMPASS_ID_AK8975) { CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external()); CALL_CHECK_N_RETURN_ERROR(inv_external_slave_akm8975_open()); } else if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] && mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id == COMPASS_ID_AMI306) { CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external()); CALL_CHECK_N_RETURN_ERROR(inv_external_slave_ami306_open()); } else if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] && mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id == COMPASS_ID_MMC328X) { printf("Compass id is %d\n", mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id); CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion_external()); CALL_CHECK_N_RETURN_ERROR(inv_external_slave_mmc3280_open()); }else { CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion()); } } #else CALL_CHECK_N_RETURN_ERROR(inv_enable_9x_fusion()); #endif CALL_CHECK_N_RETURN_ERROR( inv_enable_temp_comp() ); CALL_CHECK_N_RETURN_ERROR( inv_enable_fast_nomot() ); CALL_CHECK_N_RETURN_ERROR( inv_set_motion_callback(onMotion) ); CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_processed_callback(processedData) ); /* Setup FIFO */ CALL_CHECK_N_RETURN_ERROR( inv_send_quaternion(INV_32_BIT) ); CALL_CHECK_N_RETURN_ERROR( inv_send_gyro(INV_ALL,INV_32_BIT) ); CALL_CHECK_N_RETURN_ERROR( inv_send_accel(INV_ALL,INV_32_BIT) ); CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_rate(20) ); /* Check to see if interrupts are available. If so use them */ if (INV_SUCCESS == interror) { CALL_CHECK_N_RETURN_ERROR( inv_set_fifo_interrupt(true) ); CALL_CHECK_N_RETURN_ERROR( inv_set_motion_interrupt(true) ); CALL_CHECK_N_RETURN_ERROR( IntSetTimeout(handles[0], 100) ); MPL_LOGI("Interrupts Configured\n"); flag |= 0x04; } else { MPL_LOGI("Interrupts unavailable on this platform\n"); flag &= ~0x04; } CALL_CHECK_N_RETURN_ERROR( inv_dmp_start() ); //Loop while (1) { usleep(8000); result = ConsoleKbhit(); if (DEBUG_OUT) printf("_kbhit result : %d\n", result); if (result) { key = ConsoleGetChar(); if (DEBUG_OUT) printf("getchar key : %c (%d)\n", key, key); } else{ key = 0; } if (key == 'q') { printf("quit...\n"); break; } else if (key == '0') { printf("flag=0\n"); flag = 0; } else if (key == '1') { if (flag & 1) { MPL_LOGI("flag &= ~1 - who am i\n"); flag &= ~1; } else { MPL_LOGI("flag |= 1 - who am i\n"); flag |= 1; } } else if (key == '2') { if (flag & 2) { MPL_LOGI("flag &= ~2 - inv_update_data()\n"); flag &= ~2; } else { MPL_LOGI("flag |= 2 - inv_update_data()\n"); flag |= 2; } } else if (key == '4') { if (flag & 4) { MPL_LOGI("flag &= ~4 - IntProcess()\n"); flag &= ~4; } else { MPL_LOGI("flag |= 4 - IntProcess()\n"); flag |= 4; } } else if (key == 'a') { if (flag & 0x80) { MPL_LOGI("flag &= ~0x80 - Quaternion\n"); flag &= ~0x80; } else { MPL_LOGI("flag |= 0x80 - Quaternion\n"); flag |= 0x80; } } else if (key == 'b') { if (flag & 0x20) { printf("flag &= ~0x20 - dumpData()\n"); flag &= ~0x20; } else { printf("flag |= 0x20 - dumpData()\n"); flag |= 0x20; } } else if (key == 'S') { MPL_LOGI("run MPU Self-Test...\n"); CALL_CHECK_N_RETURN_ERROR(inv_self_test_run()); inv_sleep(5); continue; } else if (key == 'C') { MPL_LOGI("run MPU Calibration Test...\n"); CALL_CHECK_N_RETURN_ERROR(inv_self_test_calibration_run()); inv_sleep(5); continue; } else if (key == 'Z') { MPL_LOGI("run MPU Self-Test for Accel Z-Axis...\n"); CALL_CHECK_N_RETURN_ERROR(inv_self_test_accel_z_run()); inv_sleep(5); continue; } else if (key == 'h') { printf( "\n\n" "0 - turn all the features OFF\n" "1 - read WHO_AM_I\n" "2 - call inv_update_data()\n" "4 - call IntProcess()\n" "a - print Quaternion data\n" "b - Print raw accel and gyro data\n" "S - interrupt execution, run self-test\n" "C - interrupt execution, run calibration test\n" "Z - interrupt execution, run accel Z-axis test\n" "h - show this help\n" "\n\n" ); } if (flag & 0x01) { if (DEBUG_OUT) printf("inv_serial_readSingle(0x68,0,reg)\n"); CALL_CHECK_N_RETURN_ERROR( inv_serial_read(inv_get_serial_handle(), 0x68, 0, 1, reg) ); printf("\nreg[0]=%02x", reg[0]); } if (flag & 0x02) { CALL_N_CHECK( inv_update_data() ); } if (flag & 0x04) { data = InterruptPoll(handles, ARRAY_SIZE(handles), 0, 500000); InterruptPollDone(data); } if (flag & 0x20) { dumpData(); } } // Close Motion Library CALL_CHECK_N_RETURN_ERROR( inv_dmp_close() ); CALL_CHECK_N_RETURN_ERROR( inv_serial_stop() ); CALL_N_CHECK(IntClose(handles, ARRAY_SIZE(handles))); return INV_SUCCESS; }