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 "mlSetGyroBias.h"
26 #include "mlFIFO.h"
27 #include "ml.h"
28 #include <string.h>
29 #include "mldl.h"
30 #include "mlMathFunc.h"
31
32 typedef struct {
33 int needToSetBias;
34 short currentBias[3];
35 int mode;
36 int motion;
37 } tSGB;
38
39 tSGB sgb;
40
41 /** Records a motion event that may cause a callback when the priority for this
42 * feature is met.
43 */
inv_set_motion_state(int motion)44 void inv_set_motion_state(int motion)
45 {
46 sgb.motion = motion;
47 }
48
49 /** Converts from internal DMP gyro bias registers to external hardware gyro bias by
50 * applying scaling and transformation.
51 */
inv_convert_bias(const unsigned char * regs,short * bias)52 void inv_convert_bias(const unsigned char *regs, short *bias)
53 {
54 long biasTmp2[3], biasTmp[3], biasPrev[3];
55 int i;
56 int sf;
57 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
58
59 if (mldl_cfg->gyro_sens_trim != 0) {
60 sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
61 } else {
62 sf = 2000;
63 }
64 for (i = 0; i < 3; i++) {
65 biasTmp2[i] = inv_big8_to_int32(®s[i * 4]);
66 }
67 // Rotate bias vector by the transpose of the orientation matrix
68 for (i = 0; i < 3; ++i) {
69 biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.gyro_orient[i]) +
70 inv_q30_mult(biasTmp2[1], inv_obj.gyro_orient[i + 3]) +
71 inv_q30_mult(biasTmp2[2], inv_obj.gyro_orient[i + 6]);
72 }
73
74 for (i = 0; i < GYRO_NUM_AXES; i++) {
75 biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf);
76 biasPrev[i] = (long)mldl_cfg->offset[i];
77 if (biasPrev[i] > 32767)
78 biasPrev[i] -= 65536L;
79 }
80
81 for (i = 0; i < GYRO_NUM_AXES; i++) {
82 bias[i] = (short)(biasPrev[i] - biasTmp[i]);
83 }
84 }
85
86 /** Records hardware biases in format as used by hardware gyro registers.
87 * Note, the hardware will add this value to the measured gyro data.
88 */
inv_set_gyro_bias_in_hw_unit(const short * bias,int mode)89 inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode)
90 {
91 if (sgb.currentBias[0] != bias[0])
92 sgb.needToSetBias = 1;
93 if (sgb.currentBias[1] != bias[1])
94 sgb.needToSetBias = 1;
95 if (sgb.currentBias[2] != bias[2])
96 sgb.needToSetBias = 1;
97 if (sgb.needToSetBias) {
98 memcpy(sgb.currentBias, bias, sizeof(sgb.currentBias));
99 sgb.mode = mode;
100 }
101 return INV_SUCCESS;
102 }
103
104 /** Records gyro biases
105 * @param[in] bias Bias where 1dps is 2^16. In chip frame.
106 */
inv_set_gyro_bias_in_dps(const long * bias,int mode)107 inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode)
108 {
109 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
110 int sf, i;
111 long biasTmp;
112 short offset[3];
113 inv_error_t result;
114
115 if (mldl_cfg->gyro_sens_trim != 0) {
116 sf = 2000 * 131 / mldl_cfg->gyro_sens_trim;
117 } else {
118 sf = 2000;
119 }
120
121 for (i = 0; i < GYRO_NUM_AXES; i++) {
122 biasTmp = -bias[i] / sf;
123 if (biasTmp < 0)
124 biasTmp += 65536L;
125 offset[i] = (short)biasTmp;
126 }
127 result = inv_set_gyro_bias_in_hw_unit(offset, mode);
128 return result;
129 }
130
inv_set_gyro_bias_in_dps_float(const float * bias,int mode)131 inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode)
132 {
133 long biasL[3];
134 inv_error_t result;
135
136 biasL[0] = (long)(bias[0] * (1L << 16));
137 biasL[1] = (long)(bias[1] * (1L << 16));
138 biasL[2] = (long)(bias[2] * (1L << 16));
139 result = inv_set_gyro_bias_in_dps(biasL, mode);
140 return result;
141 }
142
MLSetGyroBiasCB(struct inv_obj_t * inv_obj)143 inv_error_t MLSetGyroBiasCB(struct inv_obj_t * inv_obj)
144 {
145 inv_error_t result = INV_SUCCESS;
146 if (sgb.needToSetBias) {
147 result = inv_set_offset(sgb.currentBias);
148 sgb.needToSetBias = 0;
149 }
150
151 // Check if motion state has changed
152 if (sgb.motion == INV_MOTION) {
153 // We are moving
154 if (inv_obj->motion_state == INV_NO_MOTION) {
155 //Trigger motion callback
156 inv_obj->motion_state = INV_MOTION;
157 inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_MOTION;
158 if (inv_params_obj.motion_cb_func) {
159 inv_params_obj.motion_cb_func(INV_MOTION);
160 }
161 }
162 } else if (sgb.motion == INV_NO_MOTION){
163 // We are not moving
164 if (inv_obj->motion_state == INV_MOTION) {
165 //Trigger no motion callback
166 inv_obj->motion_state = INV_NO_MOTION;
167 inv_obj->got_no_motion_bias = TRUE;
168 inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_NO_MOTION;
169 if (inv_params_obj.motion_cb_func) {
170 inv_params_obj.motion_cb_func(INV_NO_MOTION);
171 }
172 }
173 }
174
175 return result;
176 }
177
inv_enable_set_bias(void)178 inv_error_t inv_enable_set_bias(void)
179 {
180 inv_error_t result;
181 memset(&sgb, 0, sizeof(sgb));
182
183 sgb.motion = inv_obj.motion_state;
184
185 result =
186 inv_register_fifo_rate_process(MLSetGyroBiasCB,
187 INV_PRIORITY_SET_GYRO_BIASES);
188 if (result == INV_ERROR_INVALID_PARAMETER)
189 result = INV_SUCCESS; /* We already registered this */
190 return result;
191 }
192
inv_disable_set_bias(void)193 inv_error_t inv_disable_set_bias(void)
194 {
195 inv_error_t result;
196 result = inv_unregister_fifo_rate_process(MLSetGyroBiasCB);
197 return INV_SUCCESS; // FIXME need to disable
198 }
199