1895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $License: 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Copyright 2011 InvenSense, Inc. 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Licensed under the Apache License, Version 2.0 (the "License"); 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall you may not use this file except in compliance with the License. 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall You may obtain a copy of the License at 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall http://www.apache.org/licenses/LICENSE-2.0 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Unless required by applicable law or agreed to in writing, software 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall distributed under the License is distributed on an "AS IS" BASIS, 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall See the License for the specific language governing permissions and 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall limitations under the License. 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $ 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/****************************************************************************** 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * $Id:$ 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *****************************************************************************/ 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlSetGyroBias.h" 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <string.h> 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlMathFunc.h" 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgralltypedef struct { 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int needToSetBias; 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short currentBias[3]; 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int mode; 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int motion; 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} tSGB; 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 39895401859313187f15a800e62d43e6bcbf48fadaJP AbgralltSGB sgb; 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Records a motion event that may cause a callback when the priority for this 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * feature is met. 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_set_motion_state(int motion) 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sgb.motion = motion; 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Converts from internal DMP gyro bias registers to external hardware gyro bias by 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* applying scaling and transformation. 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_convert_bias(const unsigned char *regs, short *bias) 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long biasTmp2[3], biasTmp[3], biasPrev[3]; 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int sf; 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->gyro_sens_trim != 0) { 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = 2000; 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasTmp2[i] = inv_big8_to_int32(®s[i * 4]); 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Rotate bias vector by the transpose of the orientation matrix 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; ++i) { 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.gyro_orient[i]) + 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult(biasTmp2[1], inv_obj.gyro_orient[i + 3]) + 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_q30_mult(biasTmp2[2], inv_obj.gyro_orient[i + 6]); 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < GYRO_NUM_AXES; i++) { 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf); 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasPrev[i] = (long)mldl_cfg->offset[i]; 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (biasPrev[i] > 32767) 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasPrev[i] -= 65536L; 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < GYRO_NUM_AXES; i++) { 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall bias[i] = (short)(biasPrev[i] - biasTmp[i]); 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Records hardware biases in format as used by hardware gyro registers. 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* Note, the hardware will add this value to the measured gyro data. 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode) 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sgb.currentBias[0] != bias[0]) 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sgb.needToSetBias = 1; 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sgb.currentBias[1] != bias[1]) 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sgb.needToSetBias = 1; 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sgb.currentBias[2] != bias[2]) 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sgb.needToSetBias = 1; 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sgb.needToSetBias) { 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memcpy(sgb.currentBias, bias, sizeof(sgb.currentBias)); 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sgb.mode = mode; 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** Records gyro biases 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall* @param[in] bias Bias where 1dps is 2^16. In chip frame. 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall*/ 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode) 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int sf, i; 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long biasTmp; 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall short offset[3]; 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->gyro_sens_trim != 0) { 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sf = 2000; 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < GYRO_NUM_AXES; i++) { 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasTmp = -bias[i] / sf; 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (biasTmp < 0) 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasTmp += 65536L; 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall offset[i] = (short)biasTmp; 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_gyro_bias_in_hw_unit(offset, mode); 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode) 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long biasL[3]; 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasL[0] = (long)(bias[0] * (1L << 16)); 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasL[1] = (long)(bias[1] * (1L << 16)); 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall biasL[2] = (long)(bias[2] * (1L << 16)); 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_gyro_bias_in_dps(biasL, mode); 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t MLSetGyroBiasCB(struct inv_obj_t * inv_obj) 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result = INV_SUCCESS; 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sgb.needToSetBias) { 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_offset(sgb.currentBias); 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sgb.needToSetBias = 0; 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Check if motion state has changed 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (sgb.motion == INV_MOTION) { 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // We are moving 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj->motion_state == INV_NO_MOTION) { 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Trigger motion callback 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj->motion_state = INV_MOTION; 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_params_obj.motion_cb_func) { 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_params_obj.motion_cb_func(INV_MOTION); 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (sgb.motion == INV_NO_MOTION){ 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // We are not moving 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj->motion_state == INV_MOTION) { 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Trigger no motion callback 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj->motion_state = INV_NO_MOTION; 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj->got_no_motion_bias = TRUE; 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_NO_MOTION; 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_params_obj.motion_cb_func) { 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_params_obj.motion_cb_func(INV_NO_MOTION); 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_enable_set_bias(void) 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall memset(&sgb, 0, sizeof(sgb)); 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall sgb.motion = inv_obj.motion_state; 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_register_fifo_rate_process(MLSetGyroBiasCB, 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INV_PRIORITY_SET_GYRO_BIASES); 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result == INV_ERROR_INVALID_PARAMETER) 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = INV_SUCCESS; /* We already registered this */ 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_disable_set_bias(void) 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_unregister_fifo_rate_process(MLSetGyroBiasCB); 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; // FIXME need to disable 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 199