/**
* @brief Get a sample of pressure data from the device.
* @param data
* the buffer to store the pressure raw data for
* X, Y, and Z axes.
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_get_pressure_data(long *data)
{
inv_error_t result = INV_SUCCESS;
unsigned char tmp[3];
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
/*--- read the pressure sensor data.
The pressure read function may return an INV_ERROR_PRESSURE_* 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 pressure data sample. ---*/
result = (inv_error_t) inv_mpu_read_pressure(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(), tmp);
if (result) {
_pressureDebug(MPL_LOGV
("inv_mpu_read_pressure returned %d (%s)\n", result,
MLErrorCode(result)));
return result;
}
if (EXT_SLAVE_BIG_ENDIAN ==
mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE]->endian)
data[0] =
(((long)((signed char)tmp[0])) << 16) + (((long)tmp[1]) << 8) +
((long)tmp[2]);
else
data[0] =
(((long)((signed char)tmp[2])) << 16) + (((long)tmp[1]) << 8) +
((long)tmp[0]);
return INV_SUCCESS;
}
inv_error_t inv_compass_check_range(void)
{
struct ext_slave_config config;
unsigned char data[3];
inv_error_t result;
config.key = MPU_SLAVE_RANGE_CHECK;
config.len = 3;
config.apply = TRUE;
config.data = data;
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
if(data[0] || data[1] || data[2]) {
/* some value clipped */
return INV_ERROR_COMPASS_DATA_ERROR;
}
return INV_SUCCESS;
}
/**
* @brief Runs the MPU test at MPL runtime.
* If the DMP is operating, stops the DMP temporarely,
* runs the MPU Self Test, and re-starts the DMP.
*
* @return INV_SUCCESS or a non-zero error code otherwise.
*/
inv_error_t inv_self_test_run(void)
{
#ifdef CONFIG_MPU_SENSORS_MPU3050
return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE);
#else
return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE);
#endif
}
/**
* @brief Get a sample of accel data from the device.
* @param data
* the buffer to store the accel raw data for
* X, Y, and Z axes.
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_get_accel_data(long *data)
{
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
inv_error_t result;
unsigned char raw_data[2 * ACCEL_NUM_AXES];
long tmp[ACCEL_NUM_AXES];
int ii;
signed char *mtx = mldl_cfg->pdata->accel.orientation;
char accelId = mldl_cfg->accel->id;
if (NULL == data)
return INV_ERROR_INVALID_PARAMETER;
if (mldl_cfg->accel->read_len > sizeof(raw_data))
return INV_ERROR_ASSERTION_FAILURE;
result = (inv_error_t) inv_mpu_read_accel(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
raw_data);
if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
return result;
}
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) {
tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256;
tmp[ii] += (long)((unsigned char)raw_data[2 * ii]);
} else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) ||
(EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) {
tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256;
tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]);
if (accelId == ACCEL_ID_KXSD9) {
tmp[ii] = (long)((short)(((unsigned short)tmp[ii])
+ ((unsigned short)0x8000)));
}
} else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) {
tmp[ii] = (long)((signed char)raw_data[ii]) * 256;
} else {
result = INV_ERROR_FEATURE_NOT_IMPLEMENTED;
}
}
for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
data[ii] = ((long)tmp[0] * mtx[3 * ii] +
(long)tmp[1] * mtx[3 * ii + 1] +
(long)tmp[2] * mtx[3 * ii + 2]);
}
//MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]);
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 { /* compass on the 2nd bus or DMP is off */
result = inv_get_external_sensor_data(data, 3);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
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;
}
inv_error_t inv_set_compass_offset(void)
{
struct ext_slave_config config;
unsigned char data[3];
inv_error_t result;
config.key = MPU_SLAVE_OFFSET_VALS;
config.len = 3;
config.apply = TRUE;
config.data = data;
if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) {
/* push stored values */
data[0] = (char)inv_obj.compass_offsets[0];
data[1] = (char)inv_obj.compass_offsets[1];
data[2] = (char)inv_obj.compass_offsets[2];
MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]);
result = inv_mpu_config_compass(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
} else {
/* compute new values and store them */
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]);
if(result == INV_SUCCESS) {
inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1;
inv_obj.compass_offsets[0] = data[0];
inv_obj.compass_offsets[1] = data[1];
inv_obj.compass_offsets[2] = data[2];
}
}
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
/**
* @brief Read values from the compass slave device scale registers,
* regardless of the bus it is connected to and the MPU's configuration.
* @param reg
* the register to read from on the slave compass device.
* @param val
* a buffer of 3 elements to store the values read from the
* compass device.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_compass_read_scale(long *val)
{
struct ext_slave_config config;
unsigned char data[3];
inv_error_t result;
config.key = MPU_SLAVE_READ_SCALE;
config.len = 3;
config.apply = true;
config.data = data;
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
val[0] = ((long)(data[0] - 128) << 22) + (1L << 30);
val[1] = ((long)(data[1] - 128) << 22) + (1L << 30);
val[2] = ((long)(data[2] - 128) << 22) + (1L << 30);
return result;
}
/**
* @brief Read values from the compass slave device registers, regardless
* of the bus it is connected to and the MPU's configuration.
* @param reg
* the register to read from on the slave compass device.
* @param val
* a buffer of 3 elements to store the values read from the
* compass device.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val)
{
struct ext_slave_config config;
unsigned char data[2];
inv_error_t result;
data[0] = reg;
config.key = MPU_SLAVE_READ_REGISTERS;
config.len = 2;
config.apply = true;
config.data = data;
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
*val = data[1];
return result;
}
/**
* @brief Write a single register on the compass slave device, regardless
* of the bus it is connected to and the MPU's configuration.
* @param reg
* the register to write to on the slave compass device.
* @param val
* the value to write.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val)
{
struct ext_slave_config config;
unsigned char data[2];
inv_error_t result;
data[0] = reg;
data[1] = val;
config.key = MPU_SLAVE_WRITE_REGISTERS;
config.len = 2;
config.apply = TRUE;
config.data = data;
result = inv_mpu_config_compass(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
void MPLSensor::handlePowerEvent()
{
VFUNC_LOG;
mpuirq_data irqd;
int fd = (uintptr_t) inv_get_serial_handle();
read(fd, &irqd, sizeof(irqd));
if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) {
//going to sleep
sleepEvent();
} else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) {
//waking up
wakeEvent();
}
ioctl(fd, MPU_PM_EVENT_HANDLED, 0);
}
int MPLSensor::getPowerFd() const
{
int hdl = (uintptr_t) inv_get_serial_handle();
//ALOGV("MPLSensor::getPowerFd returning %d", hdl);
return hdl;
}
/**
* @brief
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_setup_dmp(void)
{
inv_error_t result;
struct mldl_cfg * mldl_cfg = inv_get_dl_config();
inv_set_get_address( inv_setup_dmpGetAddress );
result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_set_full_scale(2000.f);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_load_dmp(dmpMemory, DMP_CODE_SIZE, sConfig);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_set_ignore_system_suspend(false);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
if (mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL] &&
mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]->config) {
struct ext_slave_config config;
long odr;
config.key = MPU_SLAVE_CONFIG_ODR_SUSPEND;
config.len = sizeof(long);
config.apply = false;
config.data = &odr;
odr = 0;
result = inv_mpu_config_accel(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
&config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
odr = 200000;
result = inv_mpu_config_accel(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
&config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
config.key = MPU_SLAVE_CONFIG_IRQ_SUSPEND;
odr = MPU_SLAVE_IRQ_TYPE_NONE;
result = inv_mpu_config_accel(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
&config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
odr = MPU_SLAVE_IRQ_TYPE_NONE;
result = inv_mpu_config_accel(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
&config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
return result;
}
/**
* @brief Runs the MPU test for bias correction only at MPL runtime.
* If the DMP is operating, stops the DMP temporarely,
* runs the bias calculation routines, and re-starts the DMP.
*
* @return INV_SUCCESS or a non-zero error code otherwise.
*/
inv_error_t inv_self_test_bias_only(void)
{
return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE);
}
/**
* @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;
}
/**
* @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;
}
// 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;
}
