void PrintPedometerMenu(void) {
MPL_LOGI("\n\n");
MPL_LOGI(" g) Increase Step Buffer : %d\n", minSteps);
MPL_LOGI(" G) Decrease Step Buffer\n");
MPL_LOGI(" v) Increase Step Buffer Time: %d\n", minStepsTime);
MPL_LOGI(" V) Decrease Step Buffer Time\n");
}
/**
* @internal
* @brief Retrieve the unique MPU device identifier from the internal OTP
* bank 0 memory.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @return 0 on success, a non-zero error code from the serial layer on error.
*/
static inv_error_t get_mpu_unique_id(void *mlsl_handle)
{
inv_error_t result;
unsigned char otp0[8];
result =
inv_serial_read_mem(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
(BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 |
0x00, 6, otp0);
if (result)
goto close;
MPL_LOGI("\n");
MPL_LOGI("DIE_ID : %06X\n",
((int)otp0[1] << 8 | otp0[0]) & 0x1fff);
MPL_LOGI("WAFER_ID : %06X\n",
(((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5);
MPL_LOGI("A_LOT_ID : %06X\n",
( ((int)otp0[4] << 16 | (int)otp0[3] << 8 |
otp0[2]) & 0x3ffff) >> 2);
MPL_LOGI("W_LOT_ID : %06X\n",
( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2);
MPL_LOGI("WP_ID : %06X\n",
( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7);
MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2);
MPL_LOGI("\n");
close:
result =
inv_serial_single_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_BANK_SEL, 0x00);
return result;
}
inv_error_t inv_write_cal(unsigned char *cal, size_t len)
{
FILE *fp;
int bytesWritten;
inv_error_t result = INV_SUCCESS;
if (len <= 0) {
MPL_LOGE("Nothing to write");
return INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("cal data size to write = %d", len);
}
fp = fopen(MLCAL_FILE,"wb");
if (fp == NULL) {
MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE);
return INV_ERROR_FILE_OPEN;
}
bytesWritten = fwrite(cal, 1, len, fp);
if (bytesWritten != len) {
MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
bytesWritten, len);
result = INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("Bytes written = %d", bytesWritten);
}
fclose(fp);
return result;
}
inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len)
{
inv_error_t result = INV_SUCCESS;
int bytesWritten = 0;
if (len <= 0) {
MPL_LOGE("Nothing to write");
return INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("dmp firmware size to write = %d", len);
}
if ( fd == NULL ) {
return INV_ERROR_FILE_OPEN;
}
bytesWritten = fwrite(dmp, 1, len, fd);
if (bytesWritten != len) {
MPL_LOGE("bytes written (%d) don't match requested length (%d)\n",
bytesWritten, len);
result = INV_ERROR_FILE_WRITE;
}
else {
MPL_LOGI("Bytes written = %d", bytesWritten);
}
return result;
}
void TransitionToFullPower(int *handles, int numHandles)
{
CHECK_WARNING(MLDmpPedometerStandAloneClose());
InitPedFullPower();
CHECK_WARNING(MLSetPedometerFullPowerStepCount(stepCount));
CHECK_WARNING(MLSetPedometerFullPowerWalkTime(walkTime));
CHECK_WARNING(MLSetPedometerFullPowerParams(¶ms));
CHECK_WARNING(MLDmpStart());
MPL_LOGI("\n");
MPL_LOGI("**** MPU FULL POWER ****\n");
}
static void OnStep(unsigned long step, unsigned long time)
{
MPL_LOGI("Step %6lu, Time %8.3f\n",
step, (float) time / 1000.);
stepCount = step;
walkTime = time;
}
static int ProcessKbhit(void)
{
int exit = FALSE;
tMLError result = ML_ERROR;
/* Dynamic keyboard processing */
char ch = ConsoleGetChar();
result = GestureMenuProcessChar(&gestureParams, ch);
if (ML_SUCCESS == result || /*ch == ' ' ||*/ ch == '\n' || ch == '\r') {
exit = FALSE;
} else if (ch == 'b') {
flag ^= 0x20;
} else if (ch == 'h') {
PrintPedometerMenu();
} else if (ch == 'g') {
result = MLSetPedometerFullPowerStepBuffer(++minSteps);
MPL_LOGI("MLSetPedometerFullPowerStepBuffer(%d)\n", minSteps);
params.minSteps = minSteps;
} else if (ch == 'G') {
--minSteps;
if (minSteps < 0)
minSteps = 0;
result = MLSetPedometerFullPowerStepBuffer(minSteps);
MPL_LOGI("MLSetPedometerFullPowerStepBuffer(%d)\n", minSteps);
params.minSteps = minSteps;
} else if (ch == 'v') {
minStepsTime += 200;
MLSetPedometerFullPowerStepBufferResetTime(minStepsTime);
MPL_LOGI("MLSetPedometerFullPowerStepBufferResetTime(%d)\n", minStepsTime);
params.maxStepBufferTime = minStepsTime / 5;
} else if (ch == 'V') {
minStepsTime -= 200;
if (minStepsTime < 0)
minStepsTime = 0;
MLSetPedometerFullPowerStepBufferResetTime(minStepsTime);
MPL_LOGI("MLSetPedometerFullPowerStepBufferResetTime(%d)\n", minStepsTime);
params.maxStepBufferTime = minStepsTime / 5;
} else {
exit = TRUE;
}
return exit;
}
int mpu3050_suspend(struct mldl_cfg *mldl_cfg,
void *mlsl_handle,
void *accel_handle,
void *compass_handle,
void *pressure_handle,
bool suspend_gyro,
bool suspend_accel,
bool suspend_compass,
bool suspend_pressure)
{
int result;
unsigned long sensors = (ML_THREE_AXIS_GYRO |
ML_THREE_AXIS_ACCEL |
ML_THREE_AXIS_COMPASS |
ML_THREE_AXIS_PRESSURE);
unsigned long requested = mldl_cfg->requested_sensors;
if (suspend_gyro)
sensors &= ~ML_THREE_AXIS_GYRO;
if (suspend_accel)
sensors &= ~ML_THREE_AXIS_ACCEL;
if (suspend_compass)
sensors &= ~ML_THREE_AXIS_COMPASS;
if (suspend_pressure)
sensors &= ~ML_THREE_AXIS_PRESSURE;
MPL_LOGI("%s: suspending sensors to %04lx\n", __func__, sensors);
mldl_cfg->requested_sensors = sensors;
result = ioctl((int)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
ERROR_CHECK(result);
result = ioctl((int)mlsl_handle, MPU_SUSPEND, NULL);
ERROR_CHECK(result);
result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
ERROR_CHECK(result);
mldl_cfg->requested_sensors = requested;
MPL_LOGI("%s: Will resume next to %04lx\n", __func__, requested);
return result;
}
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;
}
void processedData(void)
{
if (flag & 0x80) {
float checksum = 0.0;
float quat[4];
int i;
CALL_N_CHECK( inv_get_quaternion_float(quat) );
for(i = 0; i < 4; i++)
checksum += (quat[i] * quat[i]);
MPL_LOGI("%12.4f %12.4f %12.4f %12.4f -(%12.4f)\n",
quat[0], quat[1], quat[2], quat[3], sqrtf(checksum));
}
}
void TransitionToLowPower(int *handles,
int numHandles)
{
struct mpuirq_data **data;
CHECK_WARNING(MLDmpClose());
InitPedLowPower();
CHECK_WARNING(MLPedometerStandAloneSetParams(¶ms));
CHECK_WARNING(MLDmpPedometerStandAloneStart());
CHECK_WARNING(MLPedometerStandAloneSetNumOfSteps(stepCount));
CHECK_WARNING(MLPedometerStandAloneSetWalkTime(walkTime));
data = InterruptPoll(handles, numHandles, 0, 0);
#ifdef LINUX
if (data[4]->interruptcount) {
ioctl(handles[4], MPU_PM_EVENT_HANDLED, 0);
}
#endif
InterruptPollDone(data);
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
MPL_LOGI("\n");
MPL_LOGI("**** MPU LOW POWER ****\n");
}
/**
* Prints the new or current orientation using MPL_LOGI and remembers the last
* orientation to print orientation flips.
*
* @param orientation the new or current orientation. 0 to reset.
*/
void PrintOrientation(unsigned short orientation)
{
// Determine if it was a flip
static int sLastOrientation = 0;
int flip = orientation | sLastOrientation;
if ((ML_X_UP | ML_X_DOWN) == flip) {
MPL_LOGI("Flip about the X Axis: \n");
} else if ((ML_Y_UP | ML_Y_DOWN) == flip) {
MPL_LOGI("Flip about the Y axis: \n");
} else if ((ML_Z_UP | ML_Z_DOWN) == flip) {
MPL_LOGI("Flip about the Z axis: \n");
}
sLastOrientation = orientation;
switch (orientation) {
case ML_X_UP:
MPL_LOGI("X Axis is up\n");
break;
case ML_X_DOWN:
MPL_LOGI("X Axis is down\n");
break;
case ML_Y_UP:
MPL_LOGI("Y Axis is up\n");
break;
case ML_Y_DOWN:
MPL_LOGI("Y Axis is down\n");
break;
case ML_Z_UP:
MPL_LOGI("Z Axis is up\n");
break;
case ML_Z_DOWN:
MPL_LOGI("Z Axis is down\n");
break;
case 0:
break; /* Not an error. Resets sLastOrientation */
default:
MPL_LOGE("%s: Unreconized orientation %hx\n", __func__, orientation);
break;
}
}
void DumpDataLowPower(void)
{
if (flag & 0x20) {
int ii;
float data[6];
long fixedData[6];
FIFOGetAccel(fixedData);
for (ii = 0; ii < 6; ii++) {
data[ii] = fixedData[ii] / 65536.0f;
}
MPL_LOGI("A: %12.4f %12.4f %12.4f\n",
data[0], data[1], data[2]);
}
}
/**
* @brief Stores a set of calibration data.
* It generates a binary data set containing calibration data.
* The binary data set is intended to be stored into a file.
*
* @pre inv_dmp_open()
*
* @param calData
* A pointer to an array of bytes to be stored.
* @param length
* The amount of bytes available in the array.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t inv_store_cal(unsigned char *calData, size_t length)
{
inv_error_t res = 0;
size_t size;
STORECAL_LOG("Entering inv_store_cal\n");
inv_get_mpl_state_size(&size);
MPL_LOGI("inv_get_mpl_state_size() : size=%d", size);
/* store data */
res = inv_save_mpl_states(calData, size);
if(res != 0)
{
MPL_LOGE("inv_save_mpl_states() failed");
}
STORECAL_LOG("Exiting inv_store_cal\n");
return INV_SUCCESS;
}
inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead)
{
FILE *fp;
inv_error_t result = INV_SUCCESS;
size_t fsize;
fp = fopen(MLCAL_FILE,"rb");
if (fp == NULL) {
MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE);
return INV_ERROR_FILE_OPEN;
}
// obtain file size
fseek (fp, 0 , SEEK_END);
fsize = ftell (fp);
rewind (fp);
*calData = (unsigned char *)inv_malloc(fsize);
if (*calData==NULL) {
MPL_LOGE("Could not allocate buffer of %d bytes - "
"aborting\n", fsize);
fclose(fp);
return INV_ERROR_MEMORY_EXAUSTED;
}
*bytesRead = fread(*calData, 1, fsize, fp);
if (*bytesRead != fsize) {
MPL_LOGE("bytes read (%d) don't match file size (%d)\n",
*bytesRead, fsize);
result = INV_ERROR_FILE_READ;
goto read_cal_end;
}
else {
MPL_LOGI("Bytes read = %d", *bytesRead);
}
read_cal_end:
fclose(fp);
return result;
}
/**
* @brief Store runtime calibration data to a file
*
* @pre Must be in INV_STATE_DMP_OPENED state.
* inv_dmp_open() or inv_dmp_stop() must have been called.
* inv_dmp_start() and inv_dmp_close() must have <b>NOT</b>
* been called.
*
* @return 0 or error code.
*/
inv_error_t inv_store_calibration(void)
{
unsigned char *calData;
inv_error_t result;
size_t length;
result = inv_get_mpl_state_size(&length);
calData = (unsigned char *)inv_malloc(length);
if (!calData) {
MPL_LOGE("Could not allocate buffer of %d bytes - "
"aborting\n", length);
return INV_ERROR_MEMORY_EXAUSTED;
}
else {
MPL_LOGI("mpl state size = %d", length);
}
result = inv_save_mpl_states(calData, length);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not save mpl states - "
"error %d - aborting\n", result);
goto free_mem_n_exit;
}
else {
MPL_LOGE("calData from inv_save_mpl_states, size=%d",
strlen((char *)calData));
}
result = inv_write_cal(calData, length);
if (result != INV_SUCCESS) {
MPL_LOGE("Could not store calibrated data on file - "
"error %d - aborting\n", result);
goto free_mem_n_exit;
}
free_mem_n_exit:
inv_free(calData);
return result;
}
void dumpData(void)
{
if (flag & 0x20) {
int ii;
float data[6];
float compData[4];
long fixedData[6];
FIFOGetAccel(fixedData);
FIFOGetGyro(&fixedData[3]);
for (ii = 0; ii < 6; ii++) {
data[ii] = fixedData[ii] / 65536.0f;
}
MLGetFloatArray(ML_MAGNETOMETER,compData);
MPL_LOGI("A: %12.4f %12.4f %12.4f "
"G: %12.4f %12.4f %12.4f "
"C: %12.4f %12.4f %12.4f \n",
data[0], data[1], data[2],
data[3], data[4], data[5],
compData[0], compData[1], compData[2]);
}
}
int mpu3050_resume(struct mldl_cfg* mldl_cfg,
void *mlsl_handle,
void *accel_handle,
void *compass_handle,
void *pressure_handle,
bool resume_gyro,
bool resume_accel,
bool resume_compass,
bool resume_pressure)
{
int result;
//mpu_print_cfg(mldl_cfg);
result = ioctl((int)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg);
ERROR_CHECK(result);
result = ioctl((int)mlsl_handle, MPU_RESUME, NULL);
ERROR_CHECK(result);
result = ioctl((int)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg);
ERROR_CHECK(result);
MPL_LOGI("%s: Resuming to %04lx\n", __func__, mldl_cfg->requested_sensors);
return result;
}
/**
* Record the odr for use in computing duration values.
*
* @param config Config to set, suspend or resume structure
* @param odr output data rate in 1/1000 hz
*/
static int mantis_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mantis_config *config, long apply, long odr)
{
int result;
int base, divider;
struct mantis_private_data *private_data =
(struct mantis_private_data *)pdata->private_data;
struct mldl_cfg *mldl_cfg_ref =
(struct mldl_cfg *)private_data->mldl_cfg_ref;
if (!mldl_cfg_ref
|| (mldl_cfg_ref->lpf > 0 && mldl_cfg_ref->lpf < 7))
base = 1000000;
else
base = 8000000;
if (odr != 0) {
divider = base / odr;
config->odr = base / divider;
} else {
config->odr = 0;
return INV_SUCCESS;
}
/* if the DMP and/or gyros are on, don't set the ODR =>
the DMP/gyro mldl_cfg->divider setting will handle it */
if (apply
&& (mldl_cfg_ref
&& !(mldl_cfg_ref->requested_sensors & INV_DMP_PROCESSOR))) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_SMPLRT_DIV, divider - 1);
ERROR_CHECK(result);
MPL_LOGI("ODR : %d mHz\n", config->odr);
}
return INV_SUCCESS;
}
/**
* Record the odr for use in computing duration values.
*
* @param config Config to set, suspend or resume structure
* @param odr output data rate in 1/1000 hz
*/
static int mpu6050_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mpu6050_config *config, long apply, long odr)
{
int result;
unsigned char b;
unsigned char lpa_freq = 1; /* Default value */
long base;
int total_divider;
struct mpu6050_private_data *private_data =
(struct mpu6050_private_data *)pdata->private_data;
struct mldl_cfg *mldl_cfg_ref =
(struct mldl_cfg *)private_data->mldl_cfg_ref;
if (mldl_cfg_ref) {
base = 1000 *
inv_mpu_get_sampling_rate_hz(mldl_cfg_ref->mpu_gyro_cfg)
* (mldl_cfg_ref->mpu_gyro_cfg->divider + 1);
} else {
/* have no reference to mldl_cfg => assume base rate is 1000 */
base = 1000000L;
}
if (odr != 0) {
total_divider = (base / odr) - 1;
/* final odr MAY be different from requested odr due to
integer truncation */
config->odr = base / (total_divider + 1);
} else {
config->odr = 0;
return 0;
}
/* if the DMP and/or gyros are on, don't set the ODR =>
the DMP/gyro mldl_cfg->divider setting will handle it */
if (apply
&& (mldl_cfg_ref &&
!(mldl_cfg_ref->inv_mpu_cfg->requested_sensors &
INV_DMP_PROCESSOR))) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_SMPLRT_DIV,
(unsigned char)total_divider);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
MPL_LOGI("ODR : %d mHz\n", config->odr);
}
/* Decide whether to put accel in LP mode or pull out of LP mode
based on the odr. */
switch (odr) {
case 1000:
lpa_freq = BITS_LPA_WAKE_1HZ;
break;
case 2000:
lpa_freq = BITS_LPA_WAKE_2HZ;
break;
case 10000:
lpa_freq = BITS_LPA_WAKE_10HZ;
break;
case 40000:
lpa_freq = BITS_LPA_WAKE_40HZ;
break;
default:
inv_serial_read(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_1, 1, &b);
b &= BIT_CYCLE;
if (b == BIT_CYCLE) {
MPL_LOGI(" Accel LP - > FP mode. \n ");
mpu6050_set_fp_mode(mlsl_handle, pdata);
}
}
/* If lpa_freq default value was changed, set into LP mode */
if (lpa_freq != 1) {
MPL_LOGI(" Accel FP - > LP mode. \n ");
mpu6050_set_lp_mode(mlsl_handle, pdata, lpa_freq);
}
return 0;
}
/**
* @brief The main entry point of the MPU Self Test, triggering the run of
* the single tests, for gyros and accelerometers.
* Prepares the MPU for the test, taking the device out of low power
* state if necessary, switching the MPU secondary I2C interface into
* bypass mode and restoring the original power state at the end of
* the test.
* This function is also responsible for encoding the output of each
* test in the correct format as it is stored on the file/medium of
* choice (according to inv_serial_write_cal() function).
* The format needs to stay perfectly consistent with the one expected
* by the corresponding loader in ml_stored_data.c; currectly the
* loaded in use is inv_load_cal_V1 (record type 1 - initial
* calibration).
*
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param perform_full_test
* If 1:
* Complete calibration test:
* Calculate offset, drive frequency, and noise and compare it
* against set thresholds.
* When 0:
* Skip the noise and drive frequency calculation,
* simply calculate the gyro biases.
* @param provide_result
* If 1:
* Report the final result using a bit-mask like error code as
* described in the test_gyro() function.
*
* @return 0 on success. A non-zero error code on error.
* Propagates the errors from the tests up to the caller.
*/
int inv_device_test(void *mlsl_handle,
uint_fast8_t sensor_mask,
uint_fast8_t perform_full_test,
uint_fast8_t provide_result)
{
int result = 0, gyro_test_result = 0, accel_test_result = 0;
short temp_avg = 0;
short gyro_biases[3] = {0, 0, 0};
short accel_biases[3] = {0, 0, 0};
long accel_sens[3] = {0};
unsigned long saved_sensor_mask;
unsigned char saved_state = inv_get_state();
mldl_cfg = inv_get_dl_config();
saved_sensor_mask = mldl_cfg->inv_mpu_cfg->requested_sensors;
if (sensor_mask & (INV_THREE_AXIS_GYRO & ~INV_DMP_PROCESSOR)) {
result = inv_set_mpu_sensors(INV_THREE_AXIS_GYRO & ~INV_DMP_PROCESSOR);
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
if (saved_state < INV_STATE_DMP_STARTED) {
result = inv_dmp_start();
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
}
#ifdef TRACK_IDS
result = get_mpu_unique_id(mlsl_handle);
if (result != INV_SUCCESS) {
MPL_LOGI("Could not read the device's unique ID\n");
MPL_LOGI("\n");
return -1;
}
#endif
MPL_LOGI("Collecting one group of %d ms samples for each axis\n",
(perform_full_test ? DEF_PERIOD_CAL : DEF_PERIOD_SELF));
MPL_LOGI("\n");
/* adjust the gyro sensitivity according to the gyro_sens_trim value */
adj_gyro_sens = test_setup.gyro_sens *
mldl_cfg->mpu_chip_info->gyro_sens_trim / 131.072f;
test_setup.gyro_fs = (int)(32768.f / adj_gyro_sens);
/* collect gyro and temperature data, test gyro, report result */
gyro_test_result = test_gyro(mlsl_handle,
gyro_biases, &temp_avg, perform_full_test);
MPL_LOGI("\n");
if (gyro_test_result == 0) {
MPL_LOGI_IF(provide_result, "Test : PASSED\n");
} else {
MPL_LOGI_IF(provide_result, "Test : FAILED %d/%04X - Biases NOT stored\n",
gyro_test_result, gyro_test_result);
goto gyro_test_failed;
}
MPL_LOGI_IF(provide_result, "\n");
}
if (sensor_mask & INV_THREE_AXIS_ACCEL) {
if (!mldl_cfg->slave[EXT_SLAVE_TYPE_ACCEL]) {
MPL_LOGI("\n");
MPL_LOGI("No accelerometer configured\n");
} else {
result = inv_set_mpu_sensors(INV_THREE_AXIS_ACCEL);
if (result)
return -1;
if (inv_get_state() < INV_STATE_DMP_STARTED) {
result = inv_dmp_start();
if (result)
return -1;
}
get_accel_sensitivity(accel_sens);
/* collect accel data. if this step is skipped,
ensure the array still contains zeros. */
accel_test_result =
test_accel(mlsl_handle, sensor_mask >> 4,
accel_biases, accel_sens[Z],
perform_full_test);
if (accel_test_result)
goto accel_test_failed;
/* if only Z accel is requested,
clear out the biases from the other 2 axes */
if ((sensor_mask & INV_THREE_AXIS_ACCEL) == INV_Z_ACCEL)
accel_biases[X] = accel_biases[Y] = 0;
}
}
ALOGE("in %s: accel_bias is %d %d %d", __func__, accel_biases[0],
accel_biases[1], accel_biases[2]);
result = populate_data_store(sensor_mask, temp_avg, gyro_biases,
accel_biases, accel_sens);
if (result)
return -1;
result = inv_serial_write_cal(data_store, ML_INIT_CAL_LEN);
if (result) {
MPL_LOGI("Error : cannot write calibration on file - error %d\n",
result);
LOG_RESULT_LOCATION(result);
return -1;
}
gyro_test_failed:
accel_test_failed:
/* restore the setting had at the beginning */
mldl_cfg->inv_mpu_state->status |= MPU_GYRO_NEEDS_CONFIG;
result = inv_set_mpu_sensors(saved_sensor_mask);
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
/* turn off only if it was off when the function was called */
if (saved_state < INV_STATE_DMP_STARTED) {
result = inv_dmp_stop();
if (result) {
LOG_RESULT_LOCATION(result);
return -1;
}
}
if (gyro_test_result)
return gyro_test_result;
if (accel_test_result)
return accel_test_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 */
}
/**
* @brief
*/
int populate_data_store(unsigned long sensor_mask,
short temp_avg, short gyro_biases[],
short accel_biases[], long accel_sens[])
{
int ptr = 0;
int tmp;
long long lltmp;
uint32_t chk;
/* total len of factory cal */
data_store[ptr++] = 0;
data_store[ptr++] = 0;
data_store[ptr++] = 0;
data_store[ptr++] = ML_INIT_CAL_LEN;
/* record type 5 - initial calibration */
data_store[ptr++] = 0;
data_store[ptr++] = 5;
if (sensor_mask & INV_THREE_AXIS_GYRO) {
/* temperature */
tmp = temp_avg;
if (tmp < 0)
tmp += 2 << 16;
inv_int16_to_big8(tmp, &data_store[ptr]);
ptr += 2;
/* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */
/* x gyro avg */
lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
/* y gyro avg */
lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
/* z gyro avg */
lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs;
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
ptr += 4;
} else {
ptr += (2 + 4 + 4 + 4);
}
if (sensor_mask & INV_THREE_AXIS_ACCEL) {
signed char *mtx =
mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL]->orientation;
short tmp[3];
int ii;
/* need store the biases in chip frame - that is,
inverting the rotation applied in inv_get_accel_data */
memcpy(tmp, accel_biases, sizeof(tmp));
for (ii = 0; ii < ARRAY_SIZE(accel_biases); ii++) {
accel_biases[ii] = tmp[0] * mtx[3 * 0 + ii] +
tmp[1] * mtx[3 * 1 + ii] +
tmp[2] * mtx[3 * 2 + ii];
}
/*
if (sensor_mask & INV_X_ACCEL) {
// x accel avg
lltmp = (long)accel_biases[0] * 65536L / accel_sens[0];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
*/
ptr += 4;
if (sensor_mask & INV_Y_ACCEL) {
/* y accel avg */
lltmp = (long)accel_biases[1] * 65536L / accel_sens[1];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
ptr += 4;
/* z accel avg */
if (sensor_mask & INV_Z_ACCEL) {
lltmp = (long)accel_biases[2] * 65536L / accel_sens[2];
if (lltmp < 0)
lltmp += 1LL << 32;
inv_int32_to_big8((uint32_t)lltmp, &data_store[ptr]);
}
ptr += 4;
} else {
ptr += 12;
}
/* add a checksum for data */
chk = inv_checksum(
data_store + INV_CAL_HDR_LEN,
ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN);
inv_int32_to_big8(chk, &data_store[ptr]);
ptr += 4;
if (ptr != ML_INIT_CAL_LEN) {
MPL_LOGI("Invalid calibration data length: exp %d, got %d\n",
ML_INIT_CAL_LEN, ptr);
return -1;
}
return 0;
}
void PrintGesture(tGesture* gesture)
{
float speed;
char type[1024];
switch (gesture->type)
{
case ML_TAP:
{
if (gesture->meta < 0) {
snprintf(type,sizeof(type),"-");
} else {
snprintf(type,sizeof(type),"+");
}
switch (ABS(gesture->meta))
{
case 1:
strcat(type,"X");
break;
case 2:
strcat(type,"Y");
break;
case 3:
strcat(type,"Z");
break;
default:
strcat(type,"ERROR");
break;
};
MPL_LOGI("TAP: %s %2d, X: %6d Y: %6d Z: %6d XY: %6.2f, YZ: %6.2f, XZ: %6.2f\n",
type,
gesture->num,
gesture->strength,
gesture->speed,
gesture->reserved,
(180 / M_PI) * atan2(
(float)gesture->strength, (float)gesture->speed),
(180 / M_PI) * atan2(
(float)gesture->speed, (float)gesture->reserved),
(180 / M_PI) * atan2(
(float)gesture->strength, (float)gesture->reserved)
);
}
break;
case ML_ROLL_SHAKE:
case ML_PITCH_SHAKE:
case ML_YAW_SHAKE:
{
if (gesture->strength){
snprintf(type, sizeof(type), "Snap : ");
} else {
snprintf(type, sizeof(type), "Shake: ");
}
if (gesture->meta==0) {
strcat(type, "+");
} else {
strcat(type, "-");
}
if (gesture->type == ML_ROLL_SHAKE) {
strcat(type, "Roll ");
} else if (gesture->type == ML_PITCH_SHAKE) {
strcat(type, "Pitch ");
} else if (gesture->type == ML_YAW_SHAKE) {
strcat(type, "Yaw ");
}
speed = (float)gesture->speed +
(float)(gesture->reserved / (float)(1 << 16));
MPL_LOGI("%s:%3d (speed: %8.2f)\n",type, gesture->num, speed);
}
break;
case ML_YAW_IMAGE_ROTATE:
{
if (gesture->meta == 0) {
snprintf(type, sizeof(type), "Positive ");
} else {
snprintf(type, sizeof(type), "Negative ");
}
MPL_LOGI("%s Yaw Image Rotation\n", type);
}
break;
default:
MPL_LOGE("Unknown Gesture received\n");
break;
}
}
/**
* Prints the menu with the current thresholds
*
* @param params The parameters to print
*/
void PrintGestureMenu(tGestureMenuParams const * const params)
{
MPL_LOGI("Press h at any time to re-display this menu\n");
MPL_LOGI("TAP PARAMETERS:\n");
MPL_LOGI(" Use LEFT and RIGHT arrows to adjust Tap Time \n\n");
MPL_LOGI(" j : Increase X threshold : %5d\n",
params->xTapThreshold);
MPL_LOGI(" J (Shift-j): Decrease X threshold\n");
MPL_LOGI(" k : Increase Y threshold : %5d\n",
params->yTapThreshold);
MPL_LOGI(" K (Shift-k): Decrease Y threshold\n");
MPL_LOGI(" i : Increase Z threshold : %5d\n",
params->zTapThreshold);
MPL_LOGI(" I (Shift-i): Decrease Z threshold\n");
MPL_LOGI(" l : Increase tap time : %5d\n",
params->tapTime);
MPL_LOGI(" L (Shift-l): Decrease tap time\n");
MPL_LOGI(" o : Increase next tap time : %5d\n",
params->nextTapTime);
MPL_LOGI(" O (Shift-o): Increase next tap time\n");
MPL_LOGI(" u : Increase max Taps : %5d\n",
params->maxTaps);
MPL_LOGI(" U (Shift-u): Increase max Taps\n");
MPL_LOGI("SHAKE PARAMETERS:\n");
MPL_LOGI(" x : Increase X threshold : %5d\n",
params->xShakeThresh);
MPL_LOGI(" X (Shift-x): Decrease X threshold\n");
MPL_LOGI(" y : Increase Y threshold : %5d\n",
params->yShakeThresh);
MPL_LOGI(" Y (Shift-y): Decrease Y threshold\n");
MPL_LOGI(" z : Increase Z threshold : %5d\n",
params->zShakeThresh);
MPL_LOGI(" Z (Shift-z): Decrease Z threshold\n");
MPL_LOGI(" s : Toggle Shake Function : %5d\n",
params->shakeFunction);
MPL_LOGI(" t : Increase Shake Time : %5d\n",
params->shakeTime);
MPL_LOGI(" T (Shift-T): Decrease Shake Time\n");
MPL_LOGI(" n : Increase Next Shake Time : %5d\n",
params->nextShakeTime);
MPL_LOGI(" N (Shift-n): Decrease Next Shake Time\n");
MPL_LOGI(" m : Increase max Shakes : %5d\n",
params->maxShakes);
MPL_LOGI(" M (Shift-m): Decrease max Shakes\n");
MPL_LOGI("SNAP PARAMETERS:\n");
MPL_LOGI(" p : Increase Pitch threshold : %5d\n",
params->xSnapThresh);
MPL_LOGI(" P (Shift-p): Decrease Pitch threshold\n");
MPL_LOGI(" r : Increase Roll threshold : %5d\n",
params->ySnapThresh);
MPL_LOGI(" R (Shift-r): Decrease Roll threshold\n");
MPL_LOGI(" a : Increase yAw threshold : %5d\n",
params->zSnapThresh);
MPL_LOGI(" A (Shift-a): Decrease yAw threshold\n");
MPL_LOGI("YAW ROTATION PARAMETERS:\n");
MPL_LOGI(" e : Increase yaW Rotate time : %5d\n",
params->yawRotateTime);
MPL_LOGI(" E (Shift-r): Decrease yaW Rotate time\n");
MPL_LOGI(" w : Increase yaW Rotate threshold : %5d\n",
params->yawRotateThreshold);
MPL_LOGI(" W (Shift-w): Decrease yaW Rotate threshold\n");
MPL_LOGI("ORIENTATION PARAMETER:\n");
MPL_LOGI(" d : Increase orientation angle threshold : %5f\n",
params->orientationThreshold);
MPL_LOGI(" D (Shift-d): Decrease orientation angle threshold\n");
MPL_LOGI("FIFO RATE:\n");
MPL_LOGI(" f : Increase fifo divider : %5d\n",
MLGetFIFORate());
MPL_LOGI(" F (Shift-f): Decrease fifo divider\n");
MPL_LOGI("REQUESTED SENSORS:\n");
MPL_LOGI(" S (Shift-s): Toggle in use sensors : %s\n",
sensors_string[params->sensorsIndex]);
MPL_LOGI(" F (Shift-f): Decrease fifo divider\n");
/* V,v, B,b, Q,q, C,c, G,g, are available letters upper and lowercase */
/* S is available */
MPL_LOGI("\n\n");
}
// 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;
}
/**
* Runs the pedometer by polling the step counter.
*/
void PedometerLpExample(unsigned short platform,
unsigned short accelid,
unsigned short compassid,
int *handles,
int numHandles)
{
unsigned long lastStep = 0;
unsigned long lastWalkTime = 0;
unsigned int state = PED_POWER_LOW;
unsigned int pre_freeze_state = state;
int mode;
int exit = FALSE;
tMLError result;
struct mpuirq_data **data;
lastStep = 0;
stepCount = 0;
walkTime = 0;
MPL_LOGI("\n\nPerforming Pedometer\n\n");
MPL_LOGI("You should see:\n"
"\t"
"pedometer step count incrementing as steps are taken\n"
"\n");
MPL_LOGI("Loading example...\n");
MPL_LOGI("Select sensitivity:\n");
MPL_LOGI(" 0) Low (less steps)\n");
MPL_LOGI(" 1) Normal (more steps)\n");
MPL_LOGI(" 2) High (most steps)\n");
scanf("%d", &mode);
PrintPedometerMenu();
InitPedLowPower();
if (mode == 0) {
params.threshold = 30000000L;
params.minUpTime = 320; // 3.125 Hz maximum
params.maxUpTime = 1200; // 0.833 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x10000000L;
params.maxStepBufferTime = 2000;
params.clipThreshold = 0x06000000L;
} else if (mode == 1) {
params.threshold = 25000000L;
params.minUpTime = 320; // 3.125 Hz maximum
params.maxUpTime = 1200; // 0.833 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x0d000000L;
params.maxStepBufferTime = 2500;
params.clipThreshold = 0x06000000L;
} else {
params.threshold = 20000000L;
params.minUpTime = 200; // 5.00 Hz maximum
params.maxUpTime = 1500; // 0.66 Hz minimum
params.minSteps = 5;
params.minEnergy = 0x0a000000L;
params.maxStepBufferTime = 3000;
params.clipThreshold = 0x06000000L;
}
CHECK_WARNING(MLPedometerStandAloneSetParams(¶ms));
MPL_LOGI("Example Loaded\n");
MPL_LOGI("\n");
CHECK_WARNING(MLPedometerStandAloneSetNumOfSteps(0));
CHECK_WARNING(MLDmpPedometerStandAloneStart());
MPL_LOGI("\n");
MPL_LOGI("**** MPU LOW POWER ****\n");
state = PED_POWER_LOW;
pre_freeze_state = state;
//Loop until a key is hit
while (!exit) {
if (state == PED_POWER_LOW) {
CHECK_WARNING(MLPedometerStandAloneGetWalkTime(&walkTime));
result = MLPedometerStandAloneGetNumOfSteps(&stepCount);
if (result == ML_SUCCESS) {
if ((walkTime != lastWalkTime) || (stepCount != lastStep)) {
lastStep = stepCount;
lastWalkTime = walkTime;
MPL_LOGI("Step %6lu, Time %8.3f s\n",
lastStep,
(float)walkTime / 1000.);
}
}
// No Motion tracking
if (0) {
unsigned char data[16];
CHECK_WARNING(MLSLSerialRead(MLSerialGetHandle(),
0x68, MPUREG_23_RSVD, 6, data));
MPL_LOGI("%02x%02x, %02x%02x, %02x%02x\n",
data[0], data[1],
data[2], data[3],
data[4], data[5]);
}
}
data = InterruptPoll(handles, numHandles, 2, 50000);
/* handle system suspend/resume */
#ifdef LINUX
if (data[4]->interruptcount) {
unsigned long power_state = *((unsigned long *)data[4]->data);
if (power_state == MPU_PM_EVENT_SUSPEND_PREPARE &&
state == PED_POWER_FULL) {
pre_freeze_state = PED_POWER_FULL;
TransitionToLowPower(handles, numHandles);
} else if (power_state == MPU_PM_EVENT_POST_SUSPEND &&
pre_freeze_state == PED_POWER_FULL) {
TransitionToFullPower(handles, numHandles);
}
ioctl(handles[4], MPU_PM_EVENT_HANDLED, 0);
}
#endif
InterruptPollDone(data);
if (state == PED_POWER_SLEEP && MLDLGetIntTrigger(INTSRC_AUX1)) {
MPL_LOGI("\n");
MPL_LOGI("**** MPU LOW POWER ****\n");
CHECK_WARNING(MLPedometerStandAloneSetNumOfSteps(stepCount));
CHECK_WARNING(MLPedometerStandAloneSetWalkTime(walkTime));
CHECK_WARNING(MLDmpPedometerStandAloneStart());
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
state = PED_POWER_LOW;
pre_freeze_state = state;
}
if (state == PED_POWER_LOW && MLDLGetIntTrigger(INTSRC_AUX1)) {
CHECK_WARNING(MLPedometerStandAloneGetWalkTime(&walkTime));
CHECK_WARNING(MLPedometerStandAloneGetNumOfSteps(&stepCount));
MPL_LOGI("\n");
MPL_LOGI("**** MPU SLEEP POWER ****\n");
CHECK_WARNING(MLDmpPedometerStandAloneStop());
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
state = PED_POWER_SLEEP;
pre_freeze_state = state;
}
if (state == PED_POWER_LOW && MLDLGetIntTrigger(INTSRC_MPU)) {
CHECK_WARNING(MLUpdateData());
DumpDataLowPower();
MLDLClearIntTrigger(INTSRC_MPU);
}
if (state == PED_POWER_FULL &&
(MLDLGetIntTrigger(INTSRC_MPU) || MLDLGetIntTrigger(INTSRC_AUX1))) {
CHECK_WARNING(MLUpdateData());
dumpData();
MLDLClearIntTrigger(INTSRC_AUX1);
MLDLClearIntTrigger(INTSRC_MPU);
}
if(ConsoleKbhit()) {
char ch;
switch (state) {
case PED_POWER_SLEEP:
case PED_POWER_LOW:
/* Transition to full power */
ch = ConsoleGetChar();
switch (ch) {
case 'q':
exit = TRUE;
break;
case 'h':
PrintPedometerMenu();
break;
case 'g':
minSteps += 2;
case 'G':
--minSteps;
if (minSteps < 0)
minSteps = 0;
result = MLPedometerStandAloneSetStepBuffer(minSteps);
MPL_LOGI("MLPedometerStandAloneSetStepBuffer(%d)\n",
minSteps);
break;
case 'v':
minStepsTime += 400;
case 'V':
minStepsTime -= 200;
if (minStepsTime < 0)
minStepsTime = 0;
MLPedometerStandAloneSetStepBufferResetTime(minStepsTime);
MPL_LOGI(
"MLPedometerStandAloneSetStepBufferResetTime(%d)\n",
minStepsTime);
break;
case 'b':
flag ^= 0x20;
if (flag & 0x20) {
FIFOSendAccel(ML_ELEMENT_1 |
ML_ELEMENT_2 |
ML_ELEMENT_3,
ML_32_BIT);
MLSetFifoInterrupt(TRUE);
} else {
FIFOSendAccel(ML_ELEMENT_1 |
ML_ELEMENT_2 |
ML_ELEMENT_3,
0);
MLSetFifoInterrupt(FALSE);
}
break;
case '\n':
case '\r':
/* Ignore carrage return and line feeds */
break;
default:
TransitionToFullPower(handles,numHandles);
state = PED_POWER_FULL;
pre_freeze_state = state;
break;
};
break;
case PED_POWER_FULL:
default:
/* Transition to low power */
exit = ProcessKbhit();
if (exit) {
TransitionToLowPower(handles, numHandles);
state = PED_POWER_LOW;
pre_freeze_state = state;
exit = FALSE;
}
break;
};
}
}
if (state == PED_POWER_LOW) {
CHECK_WARNING(MLDmpPedometerStandAloneClose());
} else if (PED_POWER_FULL == state) {
CHECK_WARNING(MLDmpClose());
}
}
/**
* Handles a keyboard input and updates an appropriate threshold, prints then
* menu or returns false if the character is not processed.
*
* @param params The parameters to modify if the thresholds are updated
* @param ch The input character
*
* @return TRUE if the character was processed, FALSE otherwise
*/
tMLError GestureMenuProcessChar(tGestureMenuParams * const params, char ch)
{
int result = ML_SUCCESS;
/* Dynamic keyboard processing */
switch (ch) {
case 'j':
params->xTapThreshold += 20;
// Intentionally fall through
case 'J': {
params->xTapThreshold -= 10;
if (params->xTapThreshold < 0) params->xTapThreshold = 0;
result = MLSetTapThreshByAxis(ML_TAP_AXIS_X, params->xTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThresh returned :%d\n", result);
}
MPL_LOGI("MLSetTapThreshByAxis(ML_TAP_AXIS_X, %d)\n",
params->xTapThreshold);
} break;
case 'k':
params->yTapThreshold += 20;
// Intentionally fall through
case 'K': {
params->yTapThreshold -= 10;
if (params->yTapThreshold < 0) params->yTapThreshold = 0;
result = MLSetTapThreshByAxis(ML_TAP_AXIS_Y, params->yTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThresh returned :%d\n", result);
}
MPL_LOGI("MLSetTapThreshByAxis(ML_TAP_AXIS_Y, %d)\n",
params->yTapThreshold);
} break;
case 'i':
params->zTapThreshold += 20;
// Intentionally fall through
case 'I': {
params->zTapThreshold -= 10;
if (params->zTapThreshold < 0) params->zTapThreshold = 0;
result = MLSetTapThreshByAxis(ML_TAP_AXIS_Z, params->zTapThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapThresh returned :%d\n", result);
}
MPL_LOGI("MLSetTapThreshByAxis(ML_TAP_AXIS_Z, %d)\n",
params->zTapThreshold);
} break;
case 'l':
params->tapTime += 20;
// Intentionally fall through
case 'L': {
params->tapTime -= 10;
if (params->tapTime < 0) params->tapTime = 0;
result = MLSetTapTime(params->tapTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapTime returned :%d\n", result);
}
MPL_LOGI("MLSetTapTime(%d)\n", params->tapTime);
} break;
case 'o':
params->nextTapTime += 20;
// Intentionally fall through
case 'O': {
params->nextTapTime -= 10;
if (params->nextTapTime < 0) params->nextTapTime = 0;
result = MLSetNextTapTime(params->nextTapTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetNextTapTime returned :%d\n", result);
}
MPL_LOGI("MLSetNextTapTime(%d)\n", params->nextTapTime);
} break;
case 'u':
params->maxTaps += 2;
// Intentionally fall through
case 'U': {
params->maxTaps -= 1;
if (params->maxTaps < 0) params->maxTaps = 0;
result = MLSetMaxTaps(params->maxTaps);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetMaxTaps returned :%d\n", result);
}
MPL_LOGI("MLSetMaxTaps(%d)\n", params->maxTaps);
} break;
case 's': {
int shakeParam;
params->shakeFunction = (params->shakeFunction + 1) % 2;
switch (params->shakeFunction)
{
case 0:
shakeParam = ML_NO_RETRACTION;
MPL_LOGE("MLSetShakeFunc(ML_NO_RETRACTION)\n");
break;
case 1:
shakeParam = ML_RETRACTION;
MPL_LOGI("MLSetShakeFunc(ML_RETRACTION)\n");
break;
};
result = MLSetShakeFunc(shakeParam);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeFunc returned :%d\n", result);
}
} break;
case 'x':
params->xShakeThresh += 200;
// Intentionally fall through
case 'X': {
params->xShakeThresh -= 100;
result = MLSetShakeThresh(ML_PITCH_SHAKE, params->xShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetShakeThresh(ML_PITCH_SHAKE, %d)\n", params->xShakeThresh);
} break;
case 'y':
params->yShakeThresh += 200;
// Intentionally fall through
case 'Y': {
params->yShakeThresh -= 100;
result = MLSetShakeThresh(ML_ROLL_SHAKE, params->yShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetShakeThresh(ML_ROLL_SHAKE, %d)\n", params->yShakeThresh);
} break;
case 'z':
params->zShakeThresh += 200;
// Intentionally fall through
case 'Z':{
params->zShakeThresh -= 100;
result = MLSetShakeThresh(ML_YAW_SHAKE, params->zShakeThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetShakeThresh(ML_YAW_SHAKE, %d)\n",params->zShakeThresh);
} break;
case 'r':
params->ySnapThresh += 20;
// Intentionally fall through
case 'R': {
params->ySnapThresh -= 10;
result = MLSetHardShakeThresh(ML_ROLL_SHAKE, params->ySnapThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetHardShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetHardShakeThresh(ML_ROLL_SHAKE, %d)\n",params->ySnapThresh);
} break;
case 'p':
params->xSnapThresh += 20;
// Intentionally fall through
case 'P': {
params->xSnapThresh -= 10;
result = MLSetHardShakeThresh(ML_PITCH_SHAKE, params->xSnapThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetHardShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetHardShakeThresh(ML_PITCH_SHAKE, %d)\n",
params->xSnapThresh);
} break;
case 'a':
params->zSnapThresh += 20;
case 'A': {
params->zSnapThresh -= 10;
result = MLSetHardShakeThresh(ML_YAW_SHAKE, params->zSnapThresh);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetHardShakeThresh returned :%d\n", result);
}
MPL_LOGI("MLSetHardShakeThresh(ML_YAW_SHAKE, %d)\n",params->zSnapThresh);
} break;
case 't':
params->shakeTime += 20;
case 'T':{
params->shakeTime -= 10;
result = MLSetShakeTime(params->shakeTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetShakeTime returned :%d\n", result);
}
MPL_LOGI("MLSetShakeTime(%d)\n", params->shakeTime);
} break;
case 'n':
params->nextShakeTime += 20;
case 'N':{
params->nextShakeTime -= 10;
result = MLSetNextShakeTime(params->nextShakeTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetNextShakeTime returned :%d\n", result);
}
MPL_LOGI("MLSetNextShakeTime(%d)\n", params->nextShakeTime);
} break;
case 'm':
params->maxShakes += 2;
case 'M':{
params->maxShakes -= 1;
result = MLSetMaxShakes(ML_SHAKE_ALL, params->maxShakes);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetMaxShakes returned :%d\n", result);
}
MPL_LOGI("MLSetMaxShakes(%d)\n", params->maxShakes);
} break;
case 'e':
params->yawRotateTime += 20;
case 'E':{
params->yawRotateTime -= 10;
result = MLSetYawRotateTime(params->yawRotateTime);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetYawRotateTime returned :%d\n", result);
}
MPL_LOGI("MLSetYawRotateTime(%d)\n", params->yawRotateTime);
} break;
case 'w':
params->yawRotateThreshold += 2;
case 'W':{
params->yawRotateThreshold -= 1;
result = MLSetYawRotateThresh(params->yawRotateThreshold);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetYawRotateThresh returned :%d\n", result);
}
MPL_LOGI("MLSetYawRotateThresh(%d)\n", params->yawRotateThreshold);
} break;
case 'c':
params->shakeRejectValue += 0.20f;
case 'C':{
params->shakeRejectValue -= 0.10f;
result = MLSetTapShakeReject(params->shakeRejectValue);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetTapShakeReject returned :%d\n", result);
}
MPL_LOGI("MLSetTapShakeReject(%f)\n", params->shakeRejectValue);
} break;
case 'd':
params->orientationThreshold += 10;
case 'D':{
params->orientationThreshold -= 5;
if (params->orientationThreshold > 90) {
params->orientationThreshold = 90;
}
if (params->orientationThreshold < 0 ) {
params->orientationThreshold = 0;
}
result = MLSetOrientationThreshold(params->orientationThreshold,
5, 80,
ML_X_AXIS | ML_Y_AXIS | ML_Z_AXIS);
if (ML_SUCCESS != result) {
MPL_LOGE("MLSetOrientationThreshold returned :%d\n", result);
}
MPL_LOGI("MLSetOrientationThreshold(%f, %d, %d,"
" ML_X_AXIS | ML_Y_AXIS | ML_Z_AXIS)\n",
params->orientationThreshold, 5, 80);
} break;
case 'f':
result = MLSetFIFORate(MLGetFIFORate() + 1);
MPL_LOGI("MLSetFIFORate(%d)\n",MLGetFIFORate());
break;
case 'F':
{
unsigned short newRate = MLGetFIFORate();
if (newRate > 0)
newRate--;
result = MLSetFIFORate(newRate);
MPL_LOGI("MLSetFIFORate(%d)\n",MLGetFIFORate());
break;
}
case 'S':
params->sensorsIndex++;
if (params->sensorsIndex >= DIM(sensors)) {
params->sensorsIndex = 0;
}
result = MLSetMPUSensors(sensors[params->sensorsIndex]);
ERROR_CHECK(result);
MPL_LOGI("%d = MLSetMPUSensors(%s)\n", result,
sensors_string[params->sensorsIndex]);
break;
case 'h':
case 'H': {
PrintGestureMenu(params);
} break;
default: {
result = ML_ERROR;
} break;
};
return result;
}
/**
* @brief Test the gyroscope sensor.
* Implements the core logic of the MPU Self Test.
* Produces the PASS/FAIL result. Loads the calculated gyro biases
* and temperature datum into the corresponding pointers.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param gyro_biases
* output pointer to store the initial bias calculation provided
* by the MPU Self Test. Requires 3 elements for gyro X, Y,
* and Z.
* @param temp_avg
* output pointer to store the initial average temperature as
* provided by the MPU Self Test.
* @param perform_full_test
* If 1:
* Complete calibration test:
* Calculate offset, drive frequency, and noise and compare it
* against set thresholds.
* When 0:
* Skip the noise and drive frequency calculation,
* simply calculate the gyro biases.
*
* @return 0 on success.
* On error, the return value is a bitmask representing:
* 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively
* (decimal values will be 1, 2, 4 respectively).
* 3, 4, 5 Excessive offset with X, Y, Z gyros respectively
* (decimal values will be 8, 16, 32 respectively).
* 6, 7, 8 Excessive noise with X, Y, Z gyros respectively
* (decimal values will be 64, 128, 256 respectively).
* 9 If any of the RMS noise values is zero, it may be
* due to a non-functional gyro or FIFO/register failure.
* (decimal value will be 512).
*/
int test_gyro(void *mlsl_handle,
short gyro_biases[3], short *temp_avg,
uint_fast8_t perform_full_test)
{
int ret_val = 0;
inv_error_t result;
int total_count = 0;
int total_count_axis[3] = {0, 0, 0};
int packet_count;
short x[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
short y[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
short z[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0};
int temperature = 0;
float avg[3];
float rms[3];
unsigned long test_start = inv_get_tick_count();
int i, j, tmp;
char tmpStr[200];
unsigned char regs[7] = {0};
/* make sure the DMP is disabled first */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* reset the gyro offset values */
regs[0] = MPUREG_XG_OFFS_USRH;
result = inv_serial_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
6, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* sample rate */
if (perform_full_test) {
/* = 8ms */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_SMPLRT_DIV, 0x07);
test_setup.bias_thresh = (int)(
DEF_BIAS_THRESH_CAL * test_setup.gyro_sens);
} else {
/* = 1ms */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_SMPLRT_DIV, 0x00);
test_setup.bias_thresh = (int)(
DEF_BIAS_THRESH_SELF * test_setup.gyro_sens);
}
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */
switch (test_setup.gyro_fs) {
case 2000:
regs[0] |= 0x18;
break;
case 1000:
regs[0] |= 0x10;
break;
case 500:
regs[0] |= 0x08;
break;
case 250:
default:
regs[0] |= 0x00;
break;
}
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_CONFIG, regs[0]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_INT_ENABLE, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* 1st, timing test */
for (j = 0; j < 3; j++) {
MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]);
/* turn on all gyros, use gyro X for clocking
Set to Y and Z for 2nd and 3rd iteration */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_PWR_MGMT_1, j + 1);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* wait for 2 ms after switching clock source */
usleep(2000);
/* enable & reset FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
tmp = test_setup.tests_per_axis;
while (tmp-- > 0) {
const unsigned char fifo_en_reg = MPUREG_FIFO_EN;
/* enable XYZ gyro in FIFO and nothing else */
result = inv_serial_single_write(mlsl_handle,
mldl_cfg->mpu_chip_info->addr, fifo_en_reg,
BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* wait one period for data */
if (perform_full_test)
usleep(DEF_PERIOD_CAL * 1000);
else
usleep(DEF_PERIOD_SELF * 1000);
/* stop storing gyro in the FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
fifo_en_reg, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Getting number of bytes in FIFO */
result = inv_serial_read(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_FIFO_COUNTH, 2, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* number of 6 B packets in the FIFO */
packet_count = inv_big8_to_int16(dataout) / 6;
sprintf(tmpStr, "Packet Count: %d - ", packet_count);
if (abs(packet_count - test_setup.packet_thresh)
<= /* Within total_timing_tol % range, rounded up */
(int)(test_setup.total_timing_tol *
test_setup.packet_thresh + 1)) {
for (i = 0; i < packet_count; i++) {
/* getting FIFO data */
result = inv_serial_read_fifo(mlsl_handle,
mldl_cfg->mpu_chip_info->addr, 6, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
x[total_count + i] = inv_big8_to_int16(&dataout[0]);
y[total_count + i] = inv_big8_to_int16(&dataout[2]);
z[total_count + i] = inv_big8_to_int16(&dataout[4]);
if (VERBOSE_OUT) {
MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n",
total_count + i, x[total_count + i],
y[total_count + i], z[total_count + i]);
}
}
total_count += packet_count;
total_count_axis[j] += packet_count;
sprintf(tmpStr, "%sOK", tmpStr);
} else {
ret_val |= 1 << j;
sprintf(tmpStr, "%sNOK - samples ignored", tmpStr);
}
MPL_LOGI("%s\n", tmpStr);
}
/* remove gyros from FIFO */
result = inv_serial_single_write(
mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_FIFO_EN, 0x00);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Read Temperature */
result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_TEMP_OUT_H, 2, dataout);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
temperature += (short)inv_big8_to_int16(dataout);
}
MPL_LOGI("\n");
MPL_LOGI("Total %d samples\n", total_count);
MPL_LOGI("\n");
/* 2nd, check bias from X, Y, and Z PLL clock source */
tmp = total_count != 0 ? total_count : 1;
for (i = 0, avg[X] = .0f, avg[Y] = .0f, avg[Z] = .0f;
i < total_count; i++) {
avg[X] += 1.f * x[i] / tmp;
avg[Y] += 1.f * y[i] / tmp;
avg[Z] += 1.f * z[i] / tmp;
}
MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n",
avg[X], avg[Y], avg[Z]);
if (VERBOSE_OUT) {
MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n",
avg[X] / adj_gyro_sens,
avg[Y] / adj_gyro_sens,
avg[Z] / adj_gyro_sens);
}
for (j = 0; j < 3; j++) {
if (fabs(avg[j]) > test_setup.bias_thresh) {
MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold "
"(threshold = %d)\n",
a_name[j], avg[j], test_setup.bias_thresh);
ret_val |= 1 << (3+j);
}
}
/* 3rd, check RMS for dead gyros
If any of the RMS noise value returns zero,
then we might have dead gyro or FIFO/register failure,
the part is sleeping, or the part is not responsive */
for (i = 0, rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f;
i < total_count; i++) {
rms[X] += (x[i] - avg[X]) * (x[i] - avg[X]);
rms[Y] += (y[i] - avg[Y]) * (y[i] - avg[Y]);
rms[Z] += (z[i] - avg[Z]) * (z[i] - avg[Z]);
}
if (rms[X] == 0 || rms[Y] == 0 || rms[Z] == 0) {
ret_val |= 1 << 9;
}
/* 4th, temperature average */
temperature /= 3;
if (VERBOSE_OUT)
MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n",
(float)inv_decode_temperature(temperature) / (1L << 16),
"", "");
/* load into final storage */
*temp_avg = (short)temperature;
gyro_biases[X] = FLOAT_TO_SHORT(avg[X]);
gyro_biases[Y] = FLOAT_TO_SHORT(avg[Y]);
gyro_biases[Z] = FLOAT_TO_SHORT(avg[Z]);
MPL_LOGI("\n");
MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - test_start);
return ret_val;
}
int find_name_by_sensor_type(const char *sensor_type, const char *type, char *sensor_name)
{
const struct dirent *ent;
int number, numstrlen;
FILE *nameFile;
DIR *dp;
char *filename;
dp = opendir(iio_dir);
if (dp == NULL) {
MPL_LOGE("No industrialio devices available");
return -ENODEV;
}
while (ent = readdir(dp), ent != NULL) {
if (strcmp(ent->d_name, ".") != 0 &&
strcmp(ent->d_name, "..") != 0 &&
strlen(ent->d_name) > strlen(type) &&
strncmp(ent->d_name, type, strlen(type)) == 0) {
numstrlen = sscanf(ent->d_name + strlen(type),
"%d",
&number);
/* verify the next character is not a colon */
if (strncmp(ent->d_name + strlen(type) + numstrlen,
":",
1) != 0) {
filename = malloc(strlen(iio_dir)
+ strlen(type)
+ numstrlen
+ 6
+ strlen(sensor_type));
if (filename == NULL)
return -ENOMEM;
sprintf(filename, "%s%s%d/%s",
iio_dir,
type,
number,
sensor_type);
nameFile = fopen(filename, "r");
MPL_LOGI("sensor type path: %s\n", filename);
free(filename);
//fscanf(nameFile, "%s", thisname);
//if (strcmp(name, thisname) == 0) {
if(nameFile == NULL) {
MPL_LOGI("keeps searching");
continue;
} else{
MPL_LOGI("found directory");
}
filename = malloc(strlen(iio_dir)
+ strlen(type)
+ numstrlen
+ 6);
sprintf(filename, "%s%s%d/name",
iio_dir,
type,
number);
nameFile = fopen(filename, "r");
MPL_LOGI("name path: %s\n", filename);
free(filename);
if (!nameFile)
continue;
fscanf(nameFile, "%s", sensor_name);
MPL_LOGI("name found: %s now test for mpuxxxx", sensor_name);
if( !strncmp("mpu",sensor_name, 3) ) {
char secondaryFileName[200];
sprintf(secondaryFileName, "%s%s%d/secondary_name",
iio_dir,
type,
number);
nameFile = fopen(secondaryFileName, "r");
MPL_LOGI("name path: %s\n", secondaryFileName);
if(!nameFile)
continue;
fscanf(nameFile, "%s", sensor_name);
MPL_LOGI("secondary name found: %s\n", sensor_name);
}
else {
fscanf(nameFile, "%s", sensor_name);
MPL_LOGI("name found: %s\n", sensor_name);
}
return 0;
//}
fclose(nameFile);
}
}
}
return -ENODEV;
}
