/**
* @brief An MPL wrapper for the main MPU Self Test API inv_factory_calibrate().
* See inv_factory_calibrate() function for more details.
*
* @pre inv_dmp_open() <b>must</b> have been called to populate the mldl_cfg
* data structure.
* On Windows, SetupPlatform() is also a madatory pre condition to
* ensure the accelerometer is properly configured before running the
* test.
*
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @param provide_result
* If 1:
* perform and analyze the offset, drive frequency, and noise
* calculation and compare it against set thresholds. Report
* also the final result using a bit-mask like error code as
* described in the inv_test_gyro_xxxx() functions.
* When 0:
* skip the noise and drive frequency calculation and pass/fail
* assessment. It simply calculates the gyro and accel biases.
* NOTE: for MPU6050 devices, this parameter is currently
* ignored.
*
* @return INV_SUCCESS or first non-zero error code otherwise.
*/
inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle,
unsigned char provide_result)
{
INVENSENSE_FUNC_START;
inv_error_t firstError = INV_SUCCESS;
inv_error_t result;
unsigned char initState = inv_get_state();
if (initState < INV_STATE_DMP_OPENED) {
MPL_LOGE("Self Test cannot run before inv_dmp_open()\n");
return INV_ERROR_SM_IMPROPER_STATE;
}
/* obtain a pointer to the 'struct mldl_cfg' data structure. */
mputestCfgPtr = inv_get_dl_config();
if(initState == INV_STATE_DMP_STARTED) {
result = inv_dmp_stop();
ERROR_CHECK_FIRST(firstError, result);
}
result = inv_factory_calibrate(mlsl_handle, provide_result);
ERROR_CHECK_FIRST(firstError, result);
if(initState == INV_STATE_DMP_STARTED) {
result = inv_dmp_start();
ERROR_CHECK_FIRST(firstError, result);
}
return firstError;
}
inv_error_t inv_compass_check_range(void)
{
struct ext_slave_config config;
unsigned char data[3];
inv_error_t result;
config.key = MPU_SLAVE_RANGE_CHECK;
config.len = 3;
config.apply = TRUE;
config.data = data;
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
if(data[0] || data[1] || data[2]) {
/* some value clipped */
return INV_ERROR_COMPASS_DATA_ERROR;
}
return INV_SUCCESS;
}
/**
* @brief Get a sample of pressure data from the device.
* @param data
* the buffer to store the pressure raw data for
* X, Y, and Z axes.
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_get_pressure_data(long *data)
{
inv_error_t result = INV_SUCCESS;
unsigned char tmp[3];
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
/*--- read the pressure sensor data.
The pressure read function may return an INV_ERROR_PRESSURE_* errors
when the data is not ready (read/refresh frequency mismatch) or
the internal data sampling timing of the device was not respected.
Returning the error code will make the sensor fusion supervisor
ignore this pressure data sample. ---*/
result = (inv_error_t) inv_mpu_read_pressure(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(), tmp);
if (result) {
_pressureDebug(MPL_LOGV
("inv_mpu_read_pressure returned %d (%s)\n", result,
MLErrorCode(result)));
return result;
}
if (EXT_SLAVE_BIG_ENDIAN ==
mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE]->endian)
data[0] =
(((long)((signed char)tmp[0])) << 16) + (((long)tmp[1]) << 8) +
((long)tmp[2]);
else
data[0] =
(((long)((signed char)tmp[2])) << 16) + (((long)tmp[1]) << 8) +
((long)tmp[0]);
return INV_SUCCESS;
}
inv_error_t inv_check_max_gyro_bias(short *offset)
{
long tmp1;
long tmp2;
int ii;
long sf;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (sgb.maxGyroBias == 0) {
return INV_SUCCESS;
}
tmp2 = sgb.maxGyroBias* 65536L;
if (mldl_cfg->mpu_chip_info->gyro_sens_trim != 0) {
sf = 2000 * 131 / mldl_cfg->mpu_chip_info->gyro_sens_trim;
} else {
sf = 2000;
}
for (ii = 0; ii < GYRO_NUM_AXES; ii++) {
tmp1 = -offset[ii]*sf;
if (ABS(tmp1) > ABS(tmp2)) {
return INV_WARNING_GYRO_MAG;
}
}
return INV_SUCCESS;
}
/** Converts from internal DMP gyro bias registers to external hardware gyro bias by
* applying scaling and transformation.
*/
void inv_convert_bias(const unsigned char *regs, short *bias)
{
long biasTmp2[3], biasTmp[3], biasPrev[3];
int i;
int sf;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->mpu_chip_info->gyro_sens_trim != 0) {
sf = 2000 * 131 / mldl_cfg->mpu_chip_info->gyro_sens_trim;
} else {
sf = 2000;
}
for (i = 0; i < 3; i++) {
biasTmp2[i] = inv_big8_to_int32(®s[i * 4]);
}
// Rotate bias vector by the transpose of the orientation matrix
for (i = 0; i < 3; ++i) {
biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.calmat->gyro_orient[i]) +
inv_q30_mult(biasTmp2[1], inv_obj.calmat->gyro_orient[i + 3]) +
inv_q30_mult(biasTmp2[2], inv_obj.calmat->gyro_orient[i + 6]);
}
for (i = 0; i < GYRO_NUM_AXES; i++) {
biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf);
biasPrev[i] = (long)mldl_cfg->mpu_offsets->gyro[i];
if (biasPrev[i] > 32767)
biasPrev[i] -= 65536L;
}
for (i = 0; i < GYRO_NUM_AXES; i++) {
bias[i] = (short)(biasPrev[i] - biasTmp[i]);
}
}
/**
* @brief Query the pressure slave address.
* @return The 7-bit pressure slave address.
*/
unsigned char inv_get_pressure_slave_addr(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (NULL != mldl_cfg->pdata)
return mldl_cfg->pdata->pressure.address;
else
return 0;
}
/**
* @brief Get the ID of the pressure in use.
* @return ID of the pressure in use.
*/
unsigned short inv_get_pressure_id(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE]) {
return mldl_cfg->slave[EXT_SLAVE_TYPE_PRESSURE]->id;
}
return ID_INVALID;
}
/**
* @brief Query the pressure slave address.
* @return The 7-bit pressure slave address.
*/
unsigned char inv_get_pressure_slave_addr(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE])
return mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_PRESSURE]->address;
else
return 0;
}
/**
* @brief Get the ID of the compass in use.
* @return ID of the compass in use.
*/
unsigned short inv_get_compass_id(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (NULL != mldl_cfg->compass) {
return mldl_cfg->compass->id;
}
return ID_INVALID;
}
/**
* @brief Get the ID of the compass in use.
* @return ID of the compass in use.
*/
unsigned short inv_get_compass_id(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]) {
return mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->id;
}
return ID_INVALID;
}
/**
* @brief Query the compass slave address.
* @return The 7-bit compass slave address.
*/
unsigned char inv_get_compass_slave_addr(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS])
return mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->address;
else
return 0;
}
/**
* @brief Get the ID of the pressure in use.
* @return ID of the pressure in use.
*/
unsigned short inv_get_pressure_id(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (NULL != mldl_cfg->pressure) {
return mldl_cfg->pressure->id;
}
return ID_INVALID;
}
/**
* @brief Get a sample of accel data from the device.
* @param data
* the buffer to store the accel raw data for
* X, Y, and Z axes.
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_get_accel_data(long *data)
{
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
inv_error_t result;
unsigned char raw_data[2 * ACCEL_NUM_AXES];
long tmp[ACCEL_NUM_AXES];
int ii;
signed char *mtx = mldl_cfg->pdata->accel.orientation;
char accelId = mldl_cfg->accel->id;
if (NULL == data)
return INV_ERROR_INVALID_PARAMETER;
if (mldl_cfg->accel->read_len > sizeof(raw_data))
return INV_ERROR_ASSERTION_FAILURE;
result = (inv_error_t) inv_mpu_read_accel(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
raw_data);
if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
return result;
}
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) {
tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256;
tmp[ii] += (long)((unsigned char)raw_data[2 * ii]);
} else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) ||
(EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) {
tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256;
tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]);
if (accelId == ACCEL_ID_KXSD9) {
tmp[ii] = (long)((short)(((unsigned short)tmp[ii])
+ ((unsigned short)0x8000)));
}
} else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) {
tmp[ii] = (long)((signed char)raw_data[ii]) * 256;
} else {
result = INV_ERROR_FEATURE_NOT_IMPLEMENTED;
}
}
for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
data[ii] = ((long)tmp[0] * mtx[3 * ii] +
(long)tmp[1] * mtx[3 * ii + 1] +
(long)tmp[2] * mtx[3 * ii + 2]);
}
//MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]);
return result;
}
/**
* @brief Get a sample of compass data from the device.
* @param data
* the buffer to store the compass raw data for
* X, Y, and Z axes.
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_get_compass_data(long *data)
{
inv_error_t result;
int ii;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
unsigned char *tmp = inv_obj.mag->raw_data;
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len >
sizeof(inv_obj.mag->raw_data)) {
LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
return INV_ERROR_INVALID_CONFIGURATION;
}
if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus ==
EXT_SLAVE_BUS_PRIMARY ||
!(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
/*--- read the compass sensor data.
The compass read function may return an INV_ERROR_COMPASS_* errors
when the data is not ready (read/refresh frequency mismatch) or
the internal data sampling timing of the device was not respected.
Returning the error code will make the sensor fusion supervisor
ignore this compass data sample. ---*/
result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
tmp);
if (result) {
if (COMPASS_DEBUG) {
MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
}
return result;
}
for (ii = 0; ii < 3; ii++) {
if (EXT_SLAVE_BIG_ENDIAN ==
mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->endian)
data[ii] =
((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
else
data[ii] =
((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
}
} else { /* compass on the 2nd bus or DMP is off */
result = inv_get_external_sensor_data(data, 3);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]);
data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]);
data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]);
return INV_SUCCESS;
}
/**
* @brief Used to determine if a compass is
* configured and used by the MPL.
* @return INV_SUCCESS if the compass is present.
*/
unsigned char inv_compass_present(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS] &&
mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->resume &&
mldl_cfg->inv_mpu_cfg->requested_sensors & INV_THREE_AXIS_COMPASS)
return true;
else
return false;
}
/**
* @brief Is a pressure configured and used by MPL?
* @return INV_SUCCESS if the pressure is present.
*/
unsigned char inv_pressure_present(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (NULL != mldl_cfg->pressure &&
NULL != mldl_cfg->pressure->resume &&
mldl_cfg->requested_sensors & INV_THREE_AXIS_PRESSURE)
return TRUE;
else
return FALSE;
}
/**
* @brief Used to determine if an accel is configured and
* used by the MPL.
* @return INV_SUCCESS if the accel is present.
*/
unsigned char inv_accel_present(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (NULL != mldl_cfg->accel &&
NULL != mldl_cfg->accel->resume &&
mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL)
return TRUE;
else
return FALSE;
}
/**
* @brief Used to determine if a compass is
* configured and used by the MPL.
* @return INV_SUCCESS if the compass is present.
*/
unsigned char inv_compass_present(void)
{
INVENSENSE_FUNC_START;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (NULL != mldl_cfg->compass &&
NULL != mldl_cfg->compass->resume &&
mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS)
return TRUE;
else
return FALSE;
}
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;
}
/**
* handle the motion/no motion output from the MPL.
*/
void MPLSensor::cbOnMotion(uint16_t val)
{
FUNC_LOG;
//after the first no motion, the gyro should be calibrated well
if (val == 2) {
if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) {
//if gyros are on and we got a no motion, set a flag
// indicating that the cal file can be written.
mHaveGoodMpuCal = true;
}
}
return;
}
/**
* @brief umplStartMPU kicks starts the uMPL state machine. This function
* in turn calls the MPL functions like inv_dmp_open() and inv_dmp_start() which starts
* the motion processing algorithms. This function also enables the required features
* such as turning on the bias trackers and temperature compensation.
* This function enables the required type of data to be put in FIFO.
*
*
*
* @pre umplInit() must have been called.
*
* @return INV_SUCCESS if successful, a non-zero error code otherwise.
*/
inv_error_t umplStartMPU(void)
{
inv_error_t result;
if (umplState == UMPL_STOP)
{
MPL_LOGV("UMPL_STOP to UMPL_RUN\n");
result = inv_dmp_open();
if (result != INV_SUCCESS) return result;
result = umplDmpSetup();
if (result != INV_SUCCESS) return result;
result = inv_dmp_start();
if (result != INV_SUCCESS) return result;
#ifndef UMPL_DISABLE_LOAD_CAL
result = inv_uload_calibration();
if (result != INV_SUCCESS)
MPL_LOGE("inv_uload_calibration failed with %d in umplStartMPU\n",result);
#endif
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_ACCEL_ONLY )
{
struct mldl_cfg * mldl_cfg = inv_get_dl_config();
MPL_LOGD("UMPL_ACCEL_ONLY (or UMPL_LPACCEL_ONLY) to UMPL_RUN\n");
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]) {
inv_set_mpu_sensors( INV_NINE_AXIS );
} else {
inv_set_mpu_sensors( INV_SIX_AXIS_GYRO_ACCEL );
}
inv_set_fifo_rate(fifo_rate);
umplSetState(UMPL_RUN);
return INV_SUCCESS;
}
else if( umplState == UMPL_LPACCEL_ONLY )
{
umplStartAccelOnly(0.0);
umplStartMPU();
}
return INV_ERROR_SM_IMPROPER_STATE;
}
/**
* @brief Sets the compass bias.
* @param bias
* Compass bias, length 3. Scale is micro Tesla's * 2^16.
* Frame is mount frame which may be different from body frame.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_set_compass_bias(long *bias)
{
inv_error_t result = INV_SUCCESS;
long biasC[3];
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
inv_obj.mag->bias[0] = bias[0];
inv_obj.mag->bias[1] = bias[1];
inv_obj.mag->bias[2] = bias[2];
// Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
biasC[0] = (long) (bias[0] * (1LL << 30) / inv_obj.mag->sens);
biasC[1] = (long) (bias[1] * (1LL << 30) / inv_obj.mag->sens);
biasC[2] = (long) (bias[2] * (1LL << 30) / inv_obj.mag->sens);
if (IS_INV_ADVFEATURES_ENABLED(inv_obj)) {
biasC[0] += inv_obj.adv_fusion->init_compass_bias[0] * (1LL << 16);
biasC[1] += inv_obj.adv_fusion->init_compass_bias[1] * (1LL << 16);
biasC[2] += inv_obj.adv_fusion->init_compass_bias[2] * (1LL << 16);
}
if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
unsigned char reg[4];
long biasB[3];
signed char *orC =
mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->orientation;
// Now transform to body frame
biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8];
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
inv_int32_to_big8(biasB[0], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
inv_int32_to_big8(biasB[1], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
inv_int32_to_big8(biasB[2], reg));
}
return result;
}
/**
* @brief Sets the compass bias.
* @param bias
* Compass bias, length 3. Scale is micro Tesla's * 2^16.
* Frame is mount frame which may be different from body frame.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_set_compass_bias(long *bias)
{
inv_error_t result = INV_SUCCESS;
long biasC[3];
struct mldl_cfg *mldlCfg = inv_get_dl_config();
inv_obj.compass_bias[0] = bias[0];
inv_obj.compass_bias[1] = bias[1];
inv_obj.compass_bias[2] = bias[2];
// Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
biasC[0] =
(long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) +
inv_obj.init_compass_bias[0] * (1L << 16);
biasC[1] =
(long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) +
inv_obj.init_compass_bias[1] * (1L << 16);
biasC[2] =
(long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) +
inv_obj.init_compass_bias[2] * (1L << 16);
if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
unsigned char reg[4];
long biasB[3];
signed char *orC = mldlCfg->pdata->compass.orientation;
// Now transform to body frame
biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8];
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
inv_int32_to_big8(biasB[0], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
inv_int32_to_big8(biasB[1], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
inv_int32_to_big8(biasB[2], reg));
}
return result;
}
/**
* @brief Sets the compass bias on the DMP. Only compatible with MPU6050B1.
* @param bias
* Compass bias, length 3. Scale is chip units * 2^16.
* Frame is mount frame which may be different from body frame.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_set_new_compass_bias(long *bias)
{
inv_error_t result = INV_SUCCESS;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
unsigned char reg[4];
long biasB[3];
signed char *orC =
mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->orientation;
// Now transform to body frame
biasB[0] = bias[0] * orC[0] + bias[1] * orC[1] + bias[2] * orC[2];
biasB[1] = bias[0] * orC[3] + bias[1] * orC[4] + bias[2] * orC[5];
biasB[2] = bias[0] * orC[6] + bias[1] * orC[7] + bias[2] * orC[8];
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
inv_int32_to_big8(biasB[0], reg));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
inv_int32_to_big8(biasB[1], reg));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
inv_int32_to_big8(biasB[2], reg));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
} else {
return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
}
return INV_SUCCESS;
}
/**
* @brief Sets the compass bias.
* @param bias
* Compass bias, length 3. Scale is chip units * 2^16.
* Frame is mount frame which may be different from body frame.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_set_compass_bias(struct compass_obj_t *obj, long *bias)
{
inv_error_t result = INV_SUCCESS;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
extern struct compass_obj_t inv_compass_obj;
if (obj == NULL) {
obj = &inv_compass_obj;
}
obj->bias[0] = bias[0];
obj->bias[1] = bias[1];
obj->bias[2] = bias[2];
inv_obj.mag->bias[0] = inv_q30_mult(bias[0], inv_obj.mag->sens);
inv_obj.mag->bias[1] = inv_q30_mult(bias[1], inv_obj.mag->sens);
inv_obj.mag->bias[2] = inv_q30_mult(bias[2], inv_obj.mag->sens);
if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
unsigned char reg[4];
long biasB[3];
signed char *orC =
mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->orientation;
// Now transform to body frame
biasB[0] = bias[0] * orC[0] + bias[1] * orC[1] + bias[2] * orC[2];
biasB[1] = bias[0] * orC[3] + bias[1] * orC[4] + bias[2] * orC[5];
biasB[2] = bias[0] * orC[6] + bias[1] * orC[7] + bias[2] * orC[8];
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
inv_int32_to_big8(biasB[0], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
inv_int32_to_big8(biasB[1], reg));
result =
inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
inv_int32_to_big8(biasB[2], reg));
}
return result;
}
/**
* @brief Read values from the compass slave device scale registers,
* regardless of the bus it is connected to and the MPU's configuration.
* @param reg
* the register to read from on the slave compass device.
* @param val
* a buffer of 3 elements to store the values read from the
* compass device.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_compass_read_scale(long *val)
{
struct ext_slave_config config;
unsigned char data[3];
inv_error_t result;
config.key = MPU_SLAVE_READ_SCALE;
config.len = 3;
config.apply = true;
config.data = data;
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
val[0] = ((long)(data[0] - 128) << 22) + (1L << 30);
val[1] = ((long)(data[1] - 128) << 22) + (1L << 30);
val[2] = ((long)(data[2] - 128) << 22) + (1L << 30);
return result;
}
/**
* @brief Read values from the compass slave device registers, regardless
* of the bus it is connected to and the MPU's configuration.
* @param reg
* the register to read from on the slave compass device.
* @param val
* a buffer of 3 elements to store the values read from the
* compass device.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val)
{
struct ext_slave_config config;
unsigned char data[2];
inv_error_t result;
data[0] = reg;
config.key = MPU_SLAVE_READ_REGISTERS;
config.len = 2;
config.apply = true;
config.data = data;
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
*val = data[1];
return result;
}
/**
* @brief Write a single register on the compass slave device, regardless
* of the bus it is connected to and the MPU's configuration.
* @param reg
* the register to write to on the slave compass device.
* @param val
* the value to write.
* @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
*/
inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val)
{
struct ext_slave_config config;
unsigned char data[2];
inv_error_t result;
data[0] = reg;
data[1] = val;
config.key = MPU_SLAVE_WRITE_REGISTERS;
config.len = 2;
config.apply = TRUE;
config.data = data;
result = inv_mpu_config_compass(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
/** Records gyro biases
* @param[in] bias Bias where 1dps is 2^16. In chip frame.
*/
inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode)
{
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
int sf, i;
long biasTmp;
short offset[3];
inv_error_t result;
if (mldl_cfg->mpu_chip_info->gyro_sens_trim != 0) {
sf = 2000 * 131 / mldl_cfg->mpu_chip_info->gyro_sens_trim;
} else {
sf = 2000;
}
for (i = 0; i < GYRO_NUM_AXES; i++) {
biasTmp = -bias[i] / sf;
if (biasTmp < 0)
biasTmp += 65536L;
offset[i] = (short)biasTmp;
}
result = inv_set_gyro_bias_in_hw_unit(offset, mode);
return result;
}
int MPLSensor::update_delay()
{
FUNC_LOG;
int rv = 0;
bool irq_set[5];
pthread_mutex_lock(&mMplMutex);
if (mEnabled) {
uint64_t wanted = -1LLU;
for (int i = 0; i < numSensors; i++) {
if (mEnabled & (1 << i)) {
uint64_t ns = mDelays[i];
wanted = wanted < ns ? wanted : ns;
}
}
//Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns
if (wanted < 10000000LLU) {
wanted = 10000000LLU;
}
int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1
: 0); //mpu fifo rate is in increments of 5ms
if (rate == 0) //KLP disallow fifo rate 0
rate = 1;
if (rate != mCurFifoRate) {
//ALOGD("set fifo rate: %d %llu", rate, wanted);
inv_error_t res; // = inv_dmp_stop();
res = inv_set_fifo_rate(rate);
ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate");
//res = inv_dmp_start();
//ALOGE_IF(res != INV_SUCCESS, "error re-starting DMP");
mCurFifoRate = rate;
rv = (res == INV_SUCCESS);
}
if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) {
if (mUseTimerirq) {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0);
clearIrqData(irq_set);
if (inv_get_dl_config()->requested_sensors
== INV_THREE_AXIS_COMPASS) {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
(unsigned long) (wanted / 1000000LLU));
ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
(int) (wanted / 1000000LLU));
} else {
ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START,
(unsigned long) inv_get_sample_step_size_ms());
ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d",
(int) inv_get_sample_step_size_ms());
}
}
}
}
pthread_mutex_unlock(&mMplMutex);
return rv;
}
