| /* |
| $License: |
| Copyright 2011 InvenSense, Inc. |
| |
| Licensed under the Apache License, Version 2.0 (the "License"); |
| you may not use this file except in compliance with the License. |
| You may obtain a copy of the License at |
| |
| http://www.apache.org/licenses/LICENSE-2.0 |
| |
| Unless required by applicable law or agreed to in writing, software |
| distributed under the License is distributed on an "AS IS" BASIS, |
| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| See the License for the specific language governing permissions and |
| limitations under the License. |
| $ |
| */ |
| |
| /****************************************************************************** |
| * |
| * $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-BiasNoMot" |
| |
| |
| #define _mlDebug(x) //{x} |
| |
| /** |
| * @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 <b>NOT</b> have been called. |
| * |
| * @param func A user defined callback function accepting a |
| * 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]; |
| |
| 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); |
| |
| 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.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->accel_lpf_gain * rate; |
| timeChange = inv_get_fifo_rate(); |
| |
| accelMag = 0; |
| for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { |
| inv_obj->accel_lpf[kk] = |
| inv_q30_mult(((1L << 30) - gain), inv_obj->accel_lpf[kk]); |
| inv_obj->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]); |
| temp = accel[0] - inv_obj->accel_lpf[0]; |
| accelMag += (long long)temp *temp; |
| } |
| |
| if (accelMag > inv_obj->no_motion_accel_threshold) { |
| inv_obj->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->no_motion_accel_time) > |
| 5 * inv_obj->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->interrupt_sources & INV_INT_MOTION) == 0) && |
| ((currentTime - inv_obj->poll_no_motion) <= 1000)) |
| return INV_SUCCESS; |
| |
| inv_obj->poll_no_motion = 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]; |
| |
| _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); |
| ) |
| if (motionFlag == inv_obj->motion_duration) { |
| if (inv_obj->motion_state == INV_MOTION) { |
| inv_update_bias(); |
| 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->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->motion_duration >> 8) & 0xff); |
| regs[1] = (unsigned char)(inv_obj->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->motion_state == INV_NO_MOTION) { |
| if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) { |
| inv_update_bias(); |
| 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]; |
| |
| _mlDebug(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->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->gyro_orient[i]) + |
| inv_q30_mult(biasTmp2[1], |
| inv_obj->gyro_orient[i + 3]) + |
| inv_q30_mult(biasTmp2[2], inv_obj->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); |
| } |
| inv_set_motion_state(INV_NO_MOTION); |
| } else { |
| // We are in a motion state |
| inv_set_motion_state(INV_MOTION); |
| } |
| inv_obj->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; |
| } |
