1 /* 2 $License: 3 Copyright 2011 InvenSense, Inc. 4 5 Licensed under the Apache License, Version 2.0 (the "License"); 6 you may not use this file except in compliance with the License. 7 You may obtain a copy of the License at 8 9 http://www.apache.org/licenses/LICENSE-2.0 10 11 Unless required by applicable law or agreed to in writing, software 12 distributed under the License is distributed on an "AS IS" BASIS, 13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 See the License for the specific language governing permissions and 15 limitations under the License. 16 $ 17 */ 18 19 /****************************************************************************** 20 * 21 * $Id:$ 22 * 23 *****************************************************************************/ 24 25 #include "mlBiasNoMotion.h" 26 #include "ml.h" 27 #include "mlinclude.h" 28 #include "mlos.h" 29 #include "mlFIFO.h" 30 #include "dmpKey.h" 31 #include "accel.h" 32 #include "mlMathFunc.h" 33 #include "mldl.h" 34 #include "mlstates.h" 35 #include "mlSetGyroBias.h" 36 37 #include "log.h" 38 #undef MPL_LOG_TAG 39 #define MPL_LOG_TAG "MPL-BiasNoMot" 40 41 42 #define _mlDebug(x) //{x} 43 44 /** 45 * @brief inv_set_motion_callback is used to register a callback function that 46 * will trigger when a change of motion state is detected. 47 * 48 * @pre inv_dmp_open() 49 * @ifnot MPL_MF 50 * or inv_open_low_power_pedometer() 51 * or inv_eis_open_dmp() 52 * @endif 53 * and inv_dmp_start() 54 * must <b>NOT</b> have been called. 55 * 56 * @param func A user defined callback function accepting a 57 * motion_state parameter, the new motion state. 58 * May be one of INV_MOTION or INV_NO_MOTION. 59 * @return INV_SUCCESS if successful or Non-zero error code otherwise. 60 */ 61 inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state)) 62 { 63 INVENSENSE_FUNC_START; 64 65 if ((inv_get_state() != INV_STATE_DMP_OPENED) && 66 (inv_get_state() != INV_STATE_DMP_STARTED)) 67 return INV_ERROR_SM_IMPROPER_STATE; 68 69 inv_params_obj.motion_cb_func = func; 70 71 return INV_SUCCESS; 72 } 73 74 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 75 defined CONFIG_MPU_SENSORS_MPU6050B1 76 /** Turns on the feature to compute gyro bias from No Motion */ 77 inv_error_t inv_turn_on_bias_from_no_motion() 78 { 79 inv_error_t result; 80 unsigned char regs[3] = { 0x0d, DINA35, 0x5d }; 81 inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; 82 result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); 83 return result; 84 } 85 86 /** Turns off the feature to compute gyro bias from No Motion 87 */ 88 inv_error_t inv_turn_off_bias_from_no_motion() 89 { 90 inv_error_t result; 91 unsigned char regs[3] = { DINA90 + 8, DINA90 + 8, DINA90 + 8 }; 92 inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; 93 result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); 94 return result; 95 } 96 #endif 97 98 inv_error_t inv_update_bias(void) 99 { 100 INVENSENSE_FUNC_START; 101 inv_error_t result; 102 unsigned char regs[12]; 103 short bias[GYRO_NUM_AXES]; 104 105 if ((inv_params_obj.bias_mode & INV_BIAS_FROM_NO_MOTION) 106 && inv_get_gyro_present()) { 107 108 regs[0] = DINAA0 + 3; 109 result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); 110 if (result) { 111 LOG_RESULT_LOCATION(result); 112 return result; 113 } 114 115 result = inv_get_mpu_memory(KEY_D_1_244, 12, regs); 116 if (result) { 117 LOG_RESULT_LOCATION(result); 118 return result; 119 } 120 121 inv_convert_bias(regs, bias); 122 123 regs[0] = DINAA0 + 15; 124 result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); 125 if (result) { 126 LOG_RESULT_LOCATION(result); 127 return result; 128 } 129 130 result = inv_set_gyro_bias_in_hw_unit(bias, INV_SGB_NO_MOTION); 131 if (result) { 132 LOG_RESULT_LOCATION(result); 133 return result; 134 } 135 136 result = 137 inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), 138 MPUREG_TEMP_OUT_H, 2, regs); 139 if (result) { 140 LOG_RESULT_LOCATION(result); 141 return result; 142 } 143 result = inv_set_mpu_memory(KEY_DMP_PREVPTAT, 2, regs); 144 if (result) { 145 LOG_RESULT_LOCATION(result); 146 return result; 147 } 148 149 inv_obj.got_no_motion_bias = TRUE; 150 } 151 return INV_SUCCESS; 152 } 153 154 inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) 155 { 156 long gain; 157 unsigned long timeChange; 158 long rate; 159 inv_error_t result; 160 long accel[3], temp; 161 long long accelMag; 162 unsigned long currentTime; 163 int kk; 164 165 if (!inv_accel_present()) { 166 return INV_SUCCESS; 167 } 168 169 currentTime = inv_get_tick_count(); 170 171 // We always run the accel low pass filter at the highest sample rate possible 172 result = inv_get_accel(accel); 173 if (result != INV_ERROR_FEATURE_NOT_ENABLED) { 174 if (result) { 175 LOG_RESULT_LOCATION(result); 176 return result; 177 } 178 rate = inv_get_fifo_rate() * 5 + 5; 179 if (rate > 200) 180 rate = 200; 181 182 gain = inv_obj->accel_lpf_gain * rate; 183 timeChange = inv_get_fifo_rate(); 184 185 accelMag = 0; 186 for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { 187 inv_obj->accel_lpf[kk] = 188 inv_q30_mult(((1L << 30) - gain), inv_obj->accel_lpf[kk]); 189 inv_obj->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]); 190 temp = accel[0] - inv_obj->accel_lpf[0]; 191 accelMag += (long long)temp *temp; 192 } 193 194 if (accelMag > inv_obj->no_motion_accel_threshold) { 195 inv_obj->no_motion_accel_time = currentTime; 196 197 // Check for change of state 198 if (!inv_get_gyro_present()) 199 inv_set_motion_state(INV_MOTION); 200 201 } else if ((currentTime - inv_obj->no_motion_accel_time) > 202 5 * inv_obj->motion_duration) { 203 // We have no motion according to accel 204 // Check fsor change of state 205 if (!inv_get_gyro_present()) 206 inv_set_motion_state(INV_NO_MOTION); 207 } 208 } 209 return INV_SUCCESS; 210 } 211 212 /** 213 * @internal 214 * @brief Manually update the motion/no motion status. This is a 215 * convienence function for implementations that do not wish to use 216 * inv_update_data. 217 * This function can be called periodically to check for the 218 * 'no motion' state and update the internal motion status and bias 219 * calculations. 220 */ 221 inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj) 222 { 223 INVENSENSE_FUNC_START; 224 unsigned char regs[3] = { 0 }; 225 unsigned short motionFlag = 0; 226 unsigned long currentTime; 227 inv_error_t result; 228 229 result = MLAccelMotionDetection(inv_obj); 230 231 currentTime = inv_get_tick_count(); 232 233 // If it is not time to poll for a no motion event, return 234 if (((inv_obj->interrupt_sources & INV_INT_MOTION) == 0) && 235 ((currentTime - inv_obj->poll_no_motion) <= 1000)) 236 return INV_SUCCESS; 237 238 inv_obj->poll_no_motion = currentTime; 239 240 #if defined CONFIG_MPU_SENSORS_MPU3050 241 if (inv_get_gyro_present() 242 && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { 243 static long repeatBiasUpdateTime = 0; 244 245 result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); 246 if (result) { 247 LOG_RESULT_LOCATION(result); 248 return result; 249 } 250 251 motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; 252 253 _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); 254 ) 255 if (motionFlag == inv_obj->motion_duration) { 256 if (inv_obj->motion_state == INV_MOTION) { 257 inv_update_bias(); 258 repeatBiasUpdateTime = inv_get_tick_count(); 259 260 regs[0] = DINAD8 + 1; 261 regs[1] = DINA0C; 262 regs[2] = DINAD8 + 2; 263 result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); 264 if (result) { 265 LOG_RESULT_LOCATION(result); 266 return result; 267 } 268 269 regs[0] = 0; 270 regs[1] = 5; 271 result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); 272 if (result) { 273 LOG_RESULT_LOCATION(result); 274 return result; 275 } 276 277 //Trigger no motion callback 278 inv_set_motion_state(INV_NO_MOTION); 279 } 280 } 281 if (motionFlag == 5) { 282 if (inv_obj->motion_state == INV_NO_MOTION) { 283 regs[0] = DINAD8 + 2; 284 regs[1] = DINA0C; 285 regs[2] = DINAD8 + 1; 286 result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); 287 if (result) { 288 LOG_RESULT_LOCATION(result); 289 return result; 290 } 291 292 regs[0] = 293 (unsigned char)((inv_obj->motion_duration >> 8) & 0xff); 294 regs[1] = (unsigned char)(inv_obj->motion_duration & 0xff); 295 result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); 296 if (result) { 297 LOG_RESULT_LOCATION(result); 298 return result; 299 } 300 301 //Trigger no motion callback 302 inv_set_motion_state(INV_MOTION); 303 } 304 } 305 if (inv_obj->motion_state == INV_NO_MOTION) { 306 if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) { 307 inv_update_bias(); 308 repeatBiasUpdateTime = inv_get_tick_count(); 309 } 310 } 311 } 312 #else // CONFIG_MPU_SENSORS_MPU3050 313 if (inv_get_gyro_present() 314 && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { 315 result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); 316 if (result) { 317 LOG_RESULT_LOCATION(result); 318 return result; 319 } 320 321 motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; 322 323 _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); 324 ) 325 if (motionFlag > 0) { 326 unsigned char biasReg[12]; 327 long biasTmp2[3], biasTmp[3]; 328 int i; 329 330 if (inv_obj->last_motion != motionFlag) { 331 result = inv_get_mpu_memory(KEY_D_2_96, 12, biasReg); 332 333 for (i = 0; i < 3; i++) { 334 biasTmp2[i] = inv_big8_to_int32(&biasReg[i * 4]); 335 } 336 // Rotate bias vector by the transpose of the orientation matrix 337 for (i = 0; i < 3; ++i) { 338 biasTmp[i] = 339 inv_q30_mult(biasTmp2[0], 340 inv_obj->gyro_orient[i]) + 341 inv_q30_mult(biasTmp2[1], 342 inv_obj->gyro_orient[i + 3]) + 343 inv_q30_mult(biasTmp2[2], inv_obj->gyro_orient[i + 6]); 344 } 345 inv_obj->gyro_bias[0] = inv_q30_mult(biasTmp[0], 1501974482L); 346 inv_obj->gyro_bias[1] = inv_q30_mult(biasTmp[1], 1501974482L); 347 inv_obj->gyro_bias[2] = inv_q30_mult(biasTmp[2], 1501974482L); 348 } 349 inv_set_motion_state(INV_NO_MOTION); 350 } else { 351 // We are in a motion state 352 inv_set_motion_state(INV_MOTION); 353 } 354 inv_obj->last_motion = motionFlag; 355 356 } 357 #endif // CONFIG_MPU_SENSORS_MPU3050 358 return INV_SUCCESS; 359 } 360 361 inv_error_t inv_enable_bias_no_motion(void) 362 { 363 inv_error_t result; 364 inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; 365 result = 366 inv_register_fifo_rate_process(MLPollMotionStatus, 367 INV_PRIORITY_BIAS_NO_MOTION); 368 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 369 defined CONFIG_MPU_SENSORS_MPU6050B1 370 if (result) { 371 LOG_RESULT_LOCATION(result); 372 return result; 373 } 374 result = inv_turn_on_bias_from_no_motion(); 375 #endif 376 return result; 377 } 378 379 inv_error_t inv_disable_bias_no_motion(void) 380 { 381 inv_error_t result; 382 inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; 383 result = inv_unregister_fifo_rate_process(MLPollMotionStatus); 384 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 385 defined CONFIG_MPU_SENSORS_MPU6050B1 386 if (result) { 387 LOG_RESULT_LOCATION(result); 388 return result; 389 } 390 result = inv_turn_off_bias_from_no_motion(); 391 #endif 392 return result; 393 } 394
