/* $License: Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. $ */ /****************************************************************************** * * $Id:$ * *****************************************************************************/ #include "mlBiasNoMotion.h" #include "ml.h" #include "mlinclude.h" #include "mlos.h" #include "mlFIFO.h" #include "dmpKey.h" #include "accel.h" #include "mlMathFunc.h" #include "mldl.h" #include "mlstates.h" #include "mlSetGyroBias.h" #include "log.h" #undef MPL_LOG_TAG #define MPL_LOG_TAG "MPL-bias_no_mot" #undef MPL_LOG_NDEBUG #define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ #if (MPL_LOG_NDEBUG == 0) #undef MPL_LOGV #define MPL_LOGV(...) do { } while (0) #endif /** * @brief inv_set_motion_callback is used to register a callback function that * will trigger when a change of motion state is detected. * * @pre inv_dmp_open() * @ifnot MPL_MF * or inv_open_low_power_pedometer() * or inv_eis_open_dmp() * @endif * and inv_dmp_start() * must NOT have been called. * * @param func A user defined callback function accepting a * lite_fusion->motion_state parameter, the new motion state. * May be one of INV_MOTION or INV_NO_MOTION. * @return INV_SUCCESS if successful or Non-zero error code otherwise. */ inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state)) { INVENSENSE_FUNC_START; if ((inv_get_state() != INV_STATE_DMP_OPENED) && (inv_get_state() != INV_STATE_DMP_STARTED)) return INV_ERROR_SM_IMPROPER_STATE; inv_params_obj.motion_cb_func = func; return INV_SUCCESS; } #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 /** Turns on the feature to compute gyro bias from No Motion */ inv_error_t inv_turn_on_bias_from_no_motion() { inv_error_t result; unsigned char regs[3] = { 0x0d, DINA35, 0x5d }; inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); return result; } /** Turns off the feature to compute gyro bias from No Motion */ inv_error_t inv_turn_off_bias_from_no_motion() { inv_error_t result; unsigned char regs[3] = { DINA90 + 8, DINA90 + 8, DINA90 + 8 }; inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); return result; } #endif inv_error_t inv_update_bias(void) { INVENSENSE_FUNC_START; inv_error_t result; unsigned char regs[12]; short bias[GYRO_NUM_AXES]; unsigned short motion_flag; if ((inv_params_obj.bias_mode & INV_BIAS_FROM_NO_MOTION) && inv_get_gyro_present()) { regs[0] = DINAA0 + 3; result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_get_mpu_memory(KEY_D_1_244, 12, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } inv_convert_bias(regs, bias); result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } motion_flag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; if (motion_flag != inv_obj.lite_fusion->motion_duration) return INV_WARNING_MOTION_RACE; regs[0] = DINAA0 + 15; result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_set_gyro_bias_in_hw_unit(bias, INV_SGB_NO_MOTION); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_set_mpu_memory(KEY_DMP_PREVPTAT, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } inv_obj.lite_fusion->got_no_motion_bias = true; } return INV_SUCCESS; } inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) { long gain; unsigned long timeChange; long rate; inv_error_t result; long accel[3], temp; long long accelMag; unsigned long currentTime; int kk; if (!inv_accel_present()) { return INV_SUCCESS; } currentTime = inv_get_tick_count(); // We always run the accel low pass filter at the highest sample rate possible result = inv_get_accel(accel); if (result != INV_ERROR_FEATURE_NOT_ENABLED) { if (result) { LOG_RESULT_LOCATION(result); return result; } rate = inv_get_fifo_rate() * 5 + 5; if (rate > 200) rate = 200; gain = inv_obj->lite_fusion->accel_lpf_gain * rate; timeChange = inv_get_fifo_rate(); accelMag = 0; for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { inv_obj->lite_fusion->accel_lpf[kk] = inv_q30_mult(((1L << 30) - gain), inv_obj->lite_fusion->accel_lpf[kk]); inv_obj->lite_fusion->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]); temp = accel[0] - inv_obj->lite_fusion->accel_lpf[0]; accelMag += (long long)temp *temp; } if (accelMag > inv_obj->lite_fusion->no_motion_accel_threshold) { inv_obj->lite_fusion->no_motion_accel_time = currentTime; // Check for change of state if (!inv_get_gyro_present()) inv_set_motion_state(INV_MOTION); } else if ((currentTime - inv_obj->lite_fusion->no_motion_accel_time) > 5 * inv_obj->lite_fusion->motion_duration) { // We have no motion according to accel // Check fsor change of state if (!inv_get_gyro_present()) inv_set_motion_state(INV_NO_MOTION); } } return INV_SUCCESS; } /** * @internal * @brief Manually update the motion/no motion status. This is a * convienence function for implementations that do not wish to use * inv_update_data. * This function can be called periodically to check for the * 'no motion' state and update the internal motion status and bias * calculations. */ inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj) { INVENSENSE_FUNC_START; unsigned char regs[3] = { 0 }; unsigned short motionFlag = 0; unsigned long currentTime; inv_error_t result; result = MLAccelMotionDetection(inv_obj); currentTime = inv_get_tick_count(); // If it is not time to poll for a no motion event, return if (((inv_obj->sys->interrupt_sources & INV_INT_MOTION) == 0) && ((currentTime - inv_obj->lite_fusion->poll_no_motion_time) <= 1000)) return INV_SUCCESS; inv_obj->lite_fusion->poll_no_motion_time = currentTime; #if defined CONFIG_MPU_SENSORS_MPU3050 if (inv_get_gyro_present() && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { static long repeatBiasUpdateTime = 0; result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); if (motionFlag == inv_obj->lite_fusion->motion_duration) { if (inv_obj->lite_fusion->motion_state == INV_MOTION) { result = inv_update_bias(); /* Make sure we were able to read the gyro bias before changing to a motion state. */ if ( result == INV_WARNING_MOTION_RACE ) return INV_SUCCESS; repeatBiasUpdateTime = inv_get_tick_count(); regs[0] = DINAD8 + 1; regs[1] = DINA0C; regs[2] = DINAD8 + 2; result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } regs[0] = 0; regs[1] = 5; result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } //Trigger no motion callback inv_set_motion_state(INV_NO_MOTION); } } if (motionFlag == 5) { if (inv_obj->lite_fusion->motion_state == INV_NO_MOTION) { regs[0] = DINAD8 + 2; regs[1] = DINA0C; regs[2] = DINAD8 + 1; result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } regs[0] = (unsigned char)((inv_obj->lite_fusion->motion_duration >> 8) & 0xff); regs[1] = (unsigned char)(inv_obj->lite_fusion->motion_duration & 0xff); result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } //Trigger no motion callback inv_set_motion_state(INV_MOTION); } } if (inv_obj->lite_fusion->motion_state == INV_NO_MOTION) { if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) { result = inv_update_bias(); /* Make sure we were able to read the gyro bias before changing to a motion state. */ if ( result == INV_WARNING_MOTION_RACE ) return INV_SUCCESS; repeatBiasUpdateTime = inv_get_tick_count(); } } } #else // CONFIG_MPU_SENSORS_MPU3050 if (inv_get_gyro_present() && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); if (motionFlag > 0) { unsigned char biasReg[12]; long biasTmp2[3], biasTmp[3]; int i; if (inv_obj->lite_fusion->last_motion != motionFlag) { result = inv_get_mpu_memory(KEY_D_2_96, 12, biasReg); for (i = 0; i < 3; i++) { biasTmp2[i] = inv_big8_to_int32(&biasReg[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]); } inv_obj->gyro->bias[0] = inv_q30_mult(biasTmp[0], 1501974482L); inv_obj->gyro->bias[1] = inv_q30_mult(biasTmp[1], 1501974482L); inv_obj->gyro->bias[2] = inv_q30_mult(biasTmp[2], 1501974482L); MPL_LOGV("Bias %f %f %f\n", (float)inv_obj->gyro->bias[0] / (1L << 16), (float)inv_obj->gyro->bias[1] / (1L << 16), (float)inv_obj->gyro->bias[2] / (1L << 16)); } inv_set_motion_state(INV_NO_MOTION); } else { // We are in a motion state inv_set_motion_state(INV_MOTION); } inv_obj->lite_fusion->last_motion = motionFlag; } #endif // CONFIG_MPU_SENSORS_MPU3050 return INV_SUCCESS; } inv_error_t inv_enable_bias_no_motion(void) { inv_error_t result; inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; result = inv_register_fifo_rate_process(MLPollMotionStatus, INV_PRIORITY_BIAS_NO_MOTION); #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_turn_on_bias_from_no_motion(); #endif return result; } inv_error_t inv_disable_bias_no_motion(void) { inv_error_t result; inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; result = inv_unregister_fifo_rate_process(MLPollMotionStatus); #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ defined CONFIG_MPU_SENSORS_MPU6050B1 if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_turn_off_bias_from_no_motion(); #endif return result; }