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 */
inv_set_motion_callback(void (* func)(unsigned short motion_state))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 */
inv_turn_on_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 */
inv_turn_off_bias_from_no_motion()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
inv_update_bias(void)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
MLAccelMotionDetection(struct inv_obj_t * inv_obj)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 */
MLPollMotionStatus(struct inv_obj_t * inv_obj)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
inv_enable_bias_no_motion(void)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
inv_disable_bias_no_motion(void)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