diff options
Diffstat (limited to 'invensense/mlsdk/mllite/mlBiasNoMotion.c')
-rw-r--r-- | invensense/mlsdk/mllite/mlBiasNoMotion.c | 393 |
1 files changed, 393 insertions, 0 deletions
diff --git a/invensense/mlsdk/mllite/mlBiasNoMotion.c b/invensense/mlsdk/mllite/mlBiasNoMotion.c new file mode 100644 index 0000000..0afe627 --- /dev/null +++ b/invensense/mlsdk/mllite/mlBiasNoMotion.c @@ -0,0 +1,393 @@ +/* + $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; +} |