/**
* @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;
}
}
/**
* @brief inv_set_control_func allows the user to choose how the sensor data will
* be processed in order to provide a control parameter.
* inv_set_control_func allows the user to choose which control functions
* will be incorporated in the sensor data processing.
* The control functions are:
* - INV_GRID
* Indicates that the user will be controlling a system that
* has discrete steps, such as icons, menu entries, pixels, etc.
* - INV_SMOOTH
* Indicates that noise from unintentional motion should be filtered out.
* - INV_DEAD_ZONE
* Indicates that a dead zone should be used, below which sensor
* data is set to zero.
* - INV_HYSTERESIS
* Indicates that, when INV_GRID is selected, hysteresis should
* be used to prevent the control signal from switching rapidly across
* elements of the grid.
*
* @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
* inv_open_low_power_pedometer().
*
* @param function Indicates what functions will be used.
* Can be a bitwise OR of several values.
*
* @return Zero if the command is successful; an ML error code otherwise.
*/
inv_error_t inv_set_control_func(unsigned short function)
{
INVENSENSE_FUNC_START;
unsigned char regs[8] = { DINA06, DINA26,
DINA46, DINA66,
DINA0E, DINA2E,
DINA4E, DINA6E
};
unsigned char i;
inv_error_t result;
if (inv_get_state() < INV_STATE_DMP_OPENED)
return INV_ERROR_SM_IMPROPER_STATE;
if ((function & INV_SMOOTH) == 0) {
for (i = 0; i < 8; i++) {
regs[i] = DINA80 + 3;
}
}
result = inv_set_mpu_memory(KEY_CFG_4, 8, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
cntrl_params.functions = function;
result = inv_set_dead_zone();
return result;
}
inv_error_t inv_set_max_gyro_bias(long bias)
{
inv_error_t result = INV_SUCCESS;
if (bias < 0) {
bias = bias * -1;
}
sgb.maxGyroBias = bias;
//DMP Code to push down into Mantis
{
unsigned char regs[4];
long dmp_thresh;
dmp_thresh = (long)(bias*46850.8254f);
inv_int32_to_big8(dmp_thresh, ®s[0]);
result = inv_set_mpu_memory(KEY_D_2_236, 4, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
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 *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;
}
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;
}
