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