/** * @cond MPL * @brief inv_get_mag_bias_error_float is used to get an array of three numbers representing * the current estimated error in the compass biases. These numbers are unitless and serve * as rough estimates in which numbers less than 100 typically represent * reasonably well calibrated compass axes. * * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif * must have been called. * * @param data * A pointer to an array to be passed back to the user. * <b>Must be 3 cells long</b>. * * @return INV_SUCCESS if the command is successful; an error code otherwise. * @endcond */ inv_error_t inv_get_mag_bias_error_float(float *data) { INVENSENSE_FUNC_START; inv_error_t result = INV_SUCCESS; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (NULL == data) { return INV_ERROR_INVALID_PARAMETER; } if (inv_obj.adv_fusion->large_field == 0) { data[0] = (float)inv_obj.adv_fusion->compass_bias_error[0]; data[1] = (float)inv_obj.adv_fusion->compass_bias_error[1]; data[2] = (float)inv_obj.adv_fusion->compass_bias_error[2]; } else { data[0] = (float)P_INIT; data[1] = (float)P_INIT; data[2] = (float)P_INIT; } return result; }
/** * @brief inv_set_control_sensitivity is used to set the sensitivity for a control * signal. * * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or * inv_open_low_power_pedometer(). * * @param controlSignal Indicates which control signal is being modified. * Must be one of: * - INV_CONTROL_1, * - INV_CONTROL_2, * - INV_CONTROL_3 or * - INV_CONTROL_4. * * @param sensitivity The sensitivity of the control signal. * * @return error code */ inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, long sensitivity) { INVENSENSE_FUNC_START; unsigned char regs[2]; long finalSens = 0; inv_error_t result; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; finalSens = sensitivity * 100; if (finalSens > 16384) { finalSens = 16384; } regs[0] = (unsigned char)(finalSens / 256); regs[1] = (unsigned char)(finalSens % 256); switch (controlSignal) { case INV_CONTROL_1: result = inv_set_mpu_memory(KEY_D_0_224, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } cntrl_params.sensitivity[0] = (unsigned short)sensitivity; break; case INV_CONTROL_2: result = inv_set_mpu_memory(KEY_D_0_228, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } cntrl_params.sensitivity[1] = (unsigned short)sensitivity; break; case INV_CONTROL_3: result = inv_set_mpu_memory(KEY_D_0_232, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } cntrl_params.sensitivity[2] = (unsigned short)sensitivity; break; case INV_CONTROL_4: result = inv_set_mpu_memory(KEY_D_0_236, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } cntrl_params.sensitivity[3] = (unsigned short)sensitivity; break; default: break; } if (finalSens != sensitivity * 100) { return INV_ERROR_INVALID_PARAMETER; } else { return INV_SUCCESS; } }
/** * Returns the curren compass accuracy. * * - 0: Unknown: The accuracy is unreliable and compass data should not be used * - 1: Low: The compass accuracy is low. * - 2: Medium: The compass accuracy is medium. * - 3: High: The compas acurracy is high and can be trusted * * @param accuracy The accuracy level in the range 0-3 * * @return ML_SUCCESS or non-zero error code */ inv_error_t inv_get_compass_accuracy(int *accuracy) { if (inv_get_state() != INV_STATE_DMP_STARTED) return INV_ERROR_SM_IMPROPER_STATE; *accuracy = inv_obj.adv_fusion->compass_accuracy; return INV_SUCCESS; }
/** * @brief Disables the INV_CONTROL engine. * * @note This function replaces MLDisable(INV_CONTROL) * * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must * have been called. * * @return INV_SUCCESS or non-zero error code */ inv_error_t inv_disable_control(void) { INVENSENSE_FUNC_START; if (inv_get_state() < INV_STATE_DMP_STARTED) return INV_ERROR_SM_IMPROPER_STATE; return INV_SUCCESS; }
/** * @brief inv_set_grid_callback is used to register a callback function that * will trigger when the grid location changes. * inv_set_grid_callback allows a user to define a callback function that will * run when a control signal crosses a grid threshold. * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or * inv_open_low_power_pedometer(). inv_dmp_start must <b>NOT</b> have * been called. * * @param func A user defined callback function * @return Zero if the command is successful; an ML error code otherwise. **/ inv_error_t inv_set_grid_callback(fpGridCb func) { INVENSENSE_FUNC_START; if (inv_get_state() != INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; cntrl_params.gridCallback = func; return INV_SUCCESS; }
/** * @brief Enables the INV_CONTROL engine. * * @note This function replaces MLEnable(INV_CONTROL) * * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must * have been called. * * @return INV_SUCCESS or non-zero error code */ inv_error_t inv_enable_control(void) { INVENSENSE_FUNC_START; if (inv_get_state() != INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; memset(&cntrl_obj, 0, sizeof(cntrl_obj)); inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL); // fixme, someone needs to send control data to the fifo return INV_SUCCESS; }
inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, long *gridChange) { INVENSENSE_FUNC_START; int_fast8_t i = 0; if (inv_get_state() != INV_STATE_DMP_STARTED) return INV_ERROR_SM_IMPROPER_STATE; for (i = 0; i < 4; i++) { controlSignal[i] = cntrl_obj.controlInt[i]; gridNum[i] = cntrl_obj.gridNum[i]; gridChange[i] = cntrl_obj.gridChange[i]; } return INV_SUCCESS; }
/** * @cond MPL * @brief inv_get_mag_scale is used to get magnetometer scale. * * * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif * must have been called. * * @param data * A pointer to an array to be passed back to the user. * <b>Must be 3 cells long at least</b>. * * @return Zero if the command is successful; an ML error code otherwise. * @endcond */ inv_error_t inv_get_mag_scale(long *data) { inv_error_t result = INV_SUCCESS; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (NULL == data) { return INV_ERROR_INVALID_PARAMETER; } data[0] = inv_obj.adv_fusion->compass_scale[0]; data[1] = inv_obj.adv_fusion->compass_scale[1]; data[2] = inv_obj.adv_fusion->compass_scale[2]; return result; }
/** * @cond MPL * @brief inv_get_local_field_float is used to get local magnetic field data. * * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif * must have been called. * * @param data * A pointer to an array to be passed back to the user. * <b>Must be 3 cells long</b>. * * @return INV_SUCCESS if the command is successful; an error code otherwise. * @endcond */ inv_error_t inv_get_local_field_float(float *data) { INVENSENSE_FUNC_START; inv_error_t result = INV_SUCCESS; if (inv_get_state() < INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (NULL == data) { return INV_ERROR_INVALID_PARAMETER; } data[0] = (float)inv_obj.adv_fusion->local_field[0] / 65536.0f; data[1] = (float)inv_obj.adv_fusion->local_field[1] / 65536.0f; data[2] = (float)inv_obj.adv_fusion->local_field[2] / 65536.0f; return result; }
inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset, long *data) { INVENSENSE_FUNC_START; if (inv_get_state() != INV_STATE_DMP_STARTED) return INV_ERROR_SM_IMPROPER_STATE; switch (controlSignal) { case INV_CONTROL_1: *data = cntrl_obj.gridNum[0]; if (reset == INV_RESET) { cntrl_obj.gridNum[0] = 0; } break; case INV_CONTROL_2: *data = cntrl_obj.gridNum[1]; if (reset == INV_RESET) { cntrl_obj.gridNum[1] = 0; } break; case INV_CONTROL_3: *data = cntrl_obj.gridNum[2]; if (reset == INV_RESET) { cntrl_obj.gridNum[2] = 0; } break; case INV_CONTROL_4: *data = cntrl_obj.gridNum[3]; if (reset == INV_RESET) { cntrl_obj.gridNum[3] = 0; } break; default: break; } return INV_SUCCESS; }
/** * @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; }
inv_error_t inv_set_control_data(unsigned short controlSignal, unsigned short parameterArray, unsigned short parameterAxis) { INVENSENSE_FUNC_START; unsigned char regs[2] = { DINA80 + 10, DINA20 }; inv_error_t result; if (inv_get_state() != INV_STATE_DMP_OPENED) return INV_ERROR_SM_IMPROPER_STATE; if (parameterArray == INV_ANGULAR_VELOCITY) { regs[0] = DINA80 + 5; regs[1] = DINA00; } switch (controlSignal) { case INV_CONTROL_1: cntrl_params.parameterArray[0] = parameterArray; switch (parameterAxis) { case INV_PITCH: regs[1] += 0x02; cntrl_params.parameterAxis[0] = 0; break; case INV_ROLL: regs[1] = DINA22; cntrl_params.parameterAxis[0] = 1; break; case INV_YAW: regs[1] = DINA42; cntrl_params.parameterAxis[0] = 2; break; default: return INV_ERROR_INVALID_PARAMETER; } result = inv_set_mpu_memory(KEY_CFG_3, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } break; case INV_CONTROL_2: cntrl_params.parameterArray[1] = parameterArray; switch (parameterAxis) { case INV_PITCH: regs[1] += DINA0E; cntrl_params.parameterAxis[1] = 0; break; case INV_ROLL: regs[1] += DINA2E; cntrl_params.parameterAxis[1] = 1; break; case INV_YAW: regs[1] += DINA4E; cntrl_params.parameterAxis[1] = 2; break; default: return INV_ERROR_INVALID_PARAMETER; } result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } break; case INV_CONTROL_3: cntrl_params.parameterArray[2] = parameterArray; switch (parameterAxis) { case INV_PITCH: regs[1] += DINA0E; cntrl_params.parameterAxis[2] = 0; break; case INV_ROLL: regs[1] += DINA2E; cntrl_params.parameterAxis[2] = 1; break; case INV_YAW: regs[1] += DINA4E; cntrl_params.parameterAxis[2] = 2; break; default: return INV_ERROR_INVALID_PARAMETER; } result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } break; case INV_CONTROL_4: cntrl_params.parameterArray[3] = parameterArray; switch (parameterAxis) { case INV_PITCH: regs[1] += DINA0E; cntrl_params.parameterAxis[3] = 0; break; case INV_ROLL: regs[1] += DINA2E; cntrl_params.parameterAxis[3] = 1; break; case INV_YAW: regs[1] += DINA4E; cntrl_params.parameterAxis[3] = 2; break; default: return INV_ERROR_INVALID_PARAMETER; } result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } break; default: result = INV_ERROR_INVALID_PARAMETER; break; } return result; }