/**
* @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;
}
