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: mlcontrol.c 5641 2011-06-14 02:10:02Z mcaramello $
22 *
23 *******************************************************************************/
24
25 /**
26 * @defgroup CONTROL
27 * @brief Motion Library - Control Engine.
28 * The Control Library processes gyroscopes, accelerometers, and
29 * compasses to provide control signals that can be used in user
30 * interfaces.
31 * These signals can be used to manipulate objects such as documents,
32 * images, cursors, menus, etc.
33 *
34 * @{
35 * @file mlcontrol.c
36 * @brief The Control Library.
37 *
38 */
39
40 /* ------------------ */
41 /* - Include Files. - */
42 /* ------------------ */
43
44 #include "mltypes.h"
45 #include "mlinclude.h"
46 #include "mltypes.h"
47 #include "ml.h"
48 #include "mlos.h"
49 #include "mlsl.h"
50 #include "mldl.h"
51 #include "mlcontrol.h"
52 #include "dmpKey.h"
53 #include "mlstates.h"
54 #include "mlFIFO.h"
55 #include "string.h"
56
57 /* - Global Vars. - */
58 struct control_params cntrl_params = {
59 {
60 MLCTRL_SENSITIVITY_0_DEFAULT,
61 MLCTRL_SENSITIVITY_1_DEFAULT,
62 MLCTRL_SENSITIVITY_2_DEFAULT,
63 MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity
64 MLCTRL_FUNCTIONS_DEFAULT, // functions
65 {
66 MLCTRL_PARAMETER_ARRAY_0_DEFAULT,
67 MLCTRL_PARAMETER_ARRAY_1_DEFAULT,
68 MLCTRL_PARAMETER_ARRAY_2_DEFAULT,
69 MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray
70 {
71 MLCTRL_PARAMETER_AXIS_0_DEFAULT,
72 MLCTRL_PARAMETER_AXIS_1_DEFAULT,
73 MLCTRL_PARAMETER_AXIS_2_DEFAULT,
74 MLCTRL_PARAMETER_AXIS_3_DEFAULT}, // parameterAxis
75 {
76 MLCTRL_GRID_THRESHOLD_0_DEFAULT,
77 MLCTRL_GRID_THRESHOLD_1_DEFAULT,
78 MLCTRL_GRID_THRESHOLD_2_DEFAULT,
79 MLCTRL_GRID_THRESHOLD_3_DEFAULT}, // gridThreshold
80 {
81 MLCTRL_GRID_MAXIMUM_0_DEFAULT,
82 MLCTRL_GRID_MAXIMUM_1_DEFAULT,
83 MLCTRL_GRID_MAXIMUM_2_DEFAULT,
84 MLCTRL_GRID_MAXIMUM_3_DEFAULT}, // gridMaximum
85 MLCTRL_GRID_CALLBACK_DEFAULT // gridCallback
86 };
87
88 /* - Extern Vars. - */
89 struct control_obj cntrl_obj;
90 extern const unsigned char *dmpConfig1;
91
92 /* -------------- */
93 /* - Functions. - */
94 /* -------------- */
95
96 /**
97 * @brief inv_set_control_sensitivity is used to set the sensitivity for a control
98 * signal.
99 *
100 * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
101 * inv_open_low_power_pedometer().
102 *
103 * @param controlSignal Indicates which control signal is being modified.
104 * Must be one of:
105 * - INV_CONTROL_1,
106 * - INV_CONTROL_2,
107 * - INV_CONTROL_3 or
108 * - INV_CONTROL_4.
109 *
110 * @param sensitivity The sensitivity of the control signal.
111 *
112 * @return error code
113 */
inv_set_control_sensitivity(unsigned short controlSignal,long sensitivity)114 inv_error_t inv_set_control_sensitivity(unsigned short controlSignal,
115 long sensitivity)
116 {
117 INVENSENSE_FUNC_START;
118 unsigned char regs[2];
119 long finalSens = 0;
120 inv_error_t result;
121
122 if (inv_get_state() < INV_STATE_DMP_OPENED)
123 return INV_ERROR_SM_IMPROPER_STATE;
124
125 finalSens = sensitivity * 100;
126 if (finalSens > 16384) {
127 finalSens = 16384;
128 }
129 regs[0] = (unsigned char)(finalSens / 256);
130 regs[1] = (unsigned char)(finalSens % 256);
131 switch (controlSignal) {
132 case INV_CONTROL_1:
133 result = inv_set_mpu_memory(KEY_D_0_224, 2, regs);
134 if (result) {
135 LOG_RESULT_LOCATION(result);
136 return result;
137 }
138 cntrl_params.sensitivity[0] = (unsigned short)sensitivity;
139 break;
140 case INV_CONTROL_2:
141 result = inv_set_mpu_memory(KEY_D_0_228, 2, regs);
142 if (result) {
143 LOG_RESULT_LOCATION(result);
144 return result;
145 }
146 cntrl_params.sensitivity[1] = (unsigned short)sensitivity;
147 break;
148 case INV_CONTROL_3:
149 result = inv_set_mpu_memory(KEY_D_0_232, 2, regs);
150 if (result) {
151 LOG_RESULT_LOCATION(result);
152 return result;
153 }
154 cntrl_params.sensitivity[2] = (unsigned short)sensitivity;
155 break;
156 case INV_CONTROL_4:
157 result = inv_set_mpu_memory(KEY_D_0_236, 2, regs);
158 if (result) {
159 LOG_RESULT_LOCATION(result);
160 return result;
161 }
162 cntrl_params.sensitivity[3] = (unsigned short)sensitivity;
163 break;
164 default:
165 break;
166 }
167 if (finalSens != sensitivity * 100) {
168 return INV_ERROR_INVALID_PARAMETER;
169 } else {
170 return INV_SUCCESS;
171 }
172 }
173
174 /**
175 * @brief inv_set_control_func allows the user to choose how the sensor data will
176 * be processed in order to provide a control parameter.
177 * inv_set_control_func allows the user to choose which control functions
178 * will be incorporated in the sensor data processing.
179 * The control functions are:
180 * - INV_GRID
181 * Indicates that the user will be controlling a system that
182 * has discrete steps, such as icons, menu entries, pixels, etc.
183 * - INV_SMOOTH
184 * Indicates that noise from unintentional motion should be filtered out.
185 * - INV_DEAD_ZONE
186 * Indicates that a dead zone should be used, below which sensor
187 * data is set to zero.
188 * - INV_HYSTERESIS
189 * Indicates that, when INV_GRID is selected, hysteresis should
190 * be used to prevent the control signal from switching rapidly across
191 * elements of the grid.
192 *
193 * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
194 * inv_open_low_power_pedometer().
195 *
196 * @param function Indicates what functions will be used.
197 * Can be a bitwise OR of several values.
198 *
199 * @return Zero if the command is successful; an ML error code otherwise.
200 */
inv_set_control_func(unsigned short function)201 inv_error_t inv_set_control_func(unsigned short function)
202 {
203 INVENSENSE_FUNC_START;
204 unsigned char regs[8] = { DINA06, DINA26,
205 DINA46, DINA66,
206 DINA0E, DINA2E,
207 DINA4E, DINA6E
208 };
209 unsigned char i;
210 inv_error_t result;
211
212 if (inv_get_state() < INV_STATE_DMP_OPENED)
213 return INV_ERROR_SM_IMPROPER_STATE;
214
215 if ((function & INV_SMOOTH) == 0) {
216 for (i = 0; i < 8; i++) {
217 regs[i] = DINA80 + 3;
218 }
219 }
220 result = inv_set_mpu_memory(KEY_CFG_4, 8, regs);
221 if (result) {
222 LOG_RESULT_LOCATION(result);
223 return result;
224 }
225 cntrl_params.functions = function;
226 result = inv_set_dead_zone();
227
228 return result;
229 }
230
231 /**
232 * @brief inv_get_control_signal is used to get the current control signal with
233 * high precision.
234 * inv_get_control_signal is used to acquire the current data of a control signal.
235 * If INV_GRID is being used, inv_get_grid_number will probably be preferrable.
236 *
237 * @param controlSignal Indicates which control signal is being queried.
238 * Must be one of:
239 * - INV_CONTROL_1,
240 * - INV_CONTROL_2,
241 * - INV_CONTROL_3 or
242 * - INV_CONTROL_4.
243 *
244 * @param reset Indicates whether the control signal should be reset to zero.
245 * Options are INV_RESET or INV_NO_RESET
246 * @param data A pointer to the current control signal data.
247 *
248 * @return Zero if the command is successful; an ML error code otherwise.
249 */
inv_get_control_signal(unsigned short controlSignal,unsigned short reset,long * data)250 inv_error_t inv_get_control_signal(unsigned short controlSignal,
251 unsigned short reset, long *data)
252 {
253 INVENSENSE_FUNC_START;
254
255 if (inv_get_state() != INV_STATE_DMP_STARTED)
256 return INV_ERROR_SM_IMPROPER_STATE;
257
258 switch (controlSignal) {
259 case INV_CONTROL_1:
260 *data = cntrl_obj.controlInt[0];
261 if (reset == INV_RESET) {
262 cntrl_obj.controlInt[0] = 0;
263 }
264 break;
265 case INV_CONTROL_2:
266 *data = cntrl_obj.controlInt[1];
267 if (reset == INV_RESET) {
268 cntrl_obj.controlInt[1] = 0;
269 }
270 break;
271 case INV_CONTROL_3:
272 *data = cntrl_obj.controlInt[2];
273 if (reset == INV_RESET) {
274 cntrl_obj.controlInt[2] = 0;
275 }
276 break;
277 case INV_CONTROL_4:
278 *data = cntrl_obj.controlInt[3];
279 if (reset == INV_RESET) {
280 cntrl_obj.controlInt[3] = 0;
281 }
282 break;
283 default:
284 break;
285 }
286 return INV_SUCCESS;
287 }
288
289 /**
290 * @brief inv_get_grid_num is used to get the current grid location for a certain
291 * control signal.
292 * inv_get_grid_num is used to acquire the current grid location.
293 *
294 * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
295 * inv_open_low_power_pedometer().
296 *
297 * @param controlSignal Indicates which control signal is being queried.
298 * Must be one of:
299 * - INV_CONTROL_1,
300 * - INV_CONTROL_2,
301 * - INV_CONTROL_3 or
302 * - INV_CONTROL_4.
303 *
304 * @param reset Indicates whether the control signal should be reset to zero.
305 * Options are INV_RESET or INV_NO_RESET
306 * @param data A pointer to the current grid number.
307 *
308 * @return Zero if the command is successful; an ML error code otherwise.
309 */
310
inv_get_grid_num(unsigned short controlSignal,unsigned short reset,long * data)311 inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset,
312 long *data)
313 {
314 INVENSENSE_FUNC_START;
315
316 if (inv_get_state() != INV_STATE_DMP_STARTED)
317 return INV_ERROR_SM_IMPROPER_STATE;
318
319 switch (controlSignal) {
320 case INV_CONTROL_1:
321 *data = cntrl_obj.gridNum[0];
322 if (reset == INV_RESET) {
323 cntrl_obj.gridNum[0] = 0;
324 }
325 break;
326 case INV_CONTROL_2:
327 *data = cntrl_obj.gridNum[1];
328 if (reset == INV_RESET) {
329 cntrl_obj.gridNum[1] = 0;
330 }
331 break;
332 case INV_CONTROL_3:
333 *data = cntrl_obj.gridNum[2];
334 if (reset == INV_RESET) {
335 cntrl_obj.gridNum[2] = 0;
336 }
337 break;
338 case INV_CONTROL_4:
339 *data = cntrl_obj.gridNum[3];
340 if (reset == INV_RESET) {
341 cntrl_obj.gridNum[3] = 0;
342 }
343 break;
344 default:
345 break;
346 }
347
348 return INV_SUCCESS;
349 }
350
351 /**
352 * @brief inv_set_grid_thresh is used to set the grid size for a control signal.
353 * inv_set_grid_thresh is used to adjust the size of the grid being controlled.
354 * @param controlSignal Indicates which control signal is being modified.
355 * Must be one of:
356 * - INV_CONTROL_1,
357 * - INV_CONTROL_2,
358 * - INV_CONTROL_3 and
359 * - INV_CONTROL_4.
360 * @param threshold The threshold of the control signal at which the grid
361 * number will be incremented or decremented.
362 * @return Zero if the command is successful; an ML error code otherwise.
363 */
364
inv_set_grid_thresh(unsigned short controlSignal,long threshold)365 inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold)
366 {
367 INVENSENSE_FUNC_START;
368
369 if (inv_get_state() < INV_STATE_DMP_OPENED)
370 return INV_ERROR_SM_IMPROPER_STATE;
371
372 switch (controlSignal) {
373 case INV_CONTROL_1:
374 cntrl_params.gridThreshold[0] = threshold;
375 break;
376 case INV_CONTROL_2:
377 cntrl_params.gridThreshold[1] = threshold;
378 break;
379 case INV_CONTROL_3:
380 cntrl_params.gridThreshold[2] = threshold;
381 break;
382 case INV_CONTROL_4:
383 cntrl_params.gridThreshold[3] = threshold;
384 break;
385 default:
386 return INV_ERROR_INVALID_PARAMETER;
387 break;
388 }
389
390 return INV_SUCCESS;
391 }
392
393 /**
394 * @brief inv_set_grid_max is used to set the maximum grid number for a control signal.
395 * inv_set_grid_max is used to adjust the maximum allowed grid number, above
396 * which the grid number will not be incremented.
397 * The minimum grid number is always zero.
398 *
399 * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
400 * inv_open_low_power_pedometer().
401 *
402 * @param controlSignal Indicates which control signal is being modified.
403 * Must be one of:
404 * - INV_CONTROL_1,
405 * - INV_CONTROL_2,
406 * - INV_CONTROL_3 and
407 * - INV_CONTROL_4.
408 *
409 * @param maximum The maximum grid number for a control signal.
410 * @return Zero if the command is successful; an ML error code otherwise.
411 */
412
inv_set_grid_max(unsigned short controlSignal,long maximum)413 inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum)
414 {
415 INVENSENSE_FUNC_START;
416
417 if (inv_get_state() != INV_STATE_DMP_OPENED)
418 return INV_ERROR_SM_IMPROPER_STATE;
419
420 switch (controlSignal) {
421 case INV_CONTROL_1:
422 cntrl_params.gridMaximum[0] = maximum;
423 break;
424 case INV_CONTROL_2:
425 cntrl_params.gridMaximum[1] = maximum;
426 break;
427 case INV_CONTROL_3:
428 cntrl_params.gridMaximum[2] = maximum;
429 break;
430 case INV_CONTROL_4:
431 cntrl_params.gridMaximum[3] = maximum;
432 break;
433 default:
434 return INV_ERROR_INVALID_PARAMETER;
435 break;
436 }
437
438 return INV_SUCCESS;
439 }
440
441 /**
442 * @brief GridCallback function pointer type, to be passed as argument of
443 * inv_set_grid_callback.
444 *
445 * @param controlSignal Indicates which control signal crossed a grid threshold.
446 * Must be one of:
447 * - INV_CONTROL_1,
448 * - INV_CONTROL_2,
449 * - INV_CONTROL_3 and
450 * - INV_CONTROL_4.
451 *
452 * @param gridNumber An array of four numbers representing the grid number for each
453 * control signal.
454 * @param gridChange An array of four numbers representing the change in grid number
455 * for each control signal.
456 **/
457 typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum,
458 long *gridChange);
459
460 /**
461 * @brief inv_set_grid_callback is used to register a callback function that
462 * will trigger when the grid location changes.
463 * inv_set_grid_callback allows a user to define a callback function that will
464 * run when a control signal crosses a grid threshold.
465
466 * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
467 * inv_open_low_power_pedometer(). inv_dmp_start must <b>NOT</b> have
468 * been called.
469 *
470 * @param func A user defined callback function
471 * @return Zero if the command is successful; an ML error code otherwise.
472 **/
inv_set_grid_callback(fpGridCb func)473 inv_error_t inv_set_grid_callback(fpGridCb func)
474 {
475 INVENSENSE_FUNC_START;
476
477 if (inv_get_state() != INV_STATE_DMP_OPENED)
478 return INV_ERROR_SM_IMPROPER_STATE;
479
480 cntrl_params.gridCallback = func;
481 return INV_SUCCESS;
482 }
483
484 /**
485 * @brief inv_set_control_data is used to assign physical parameters to control signals.
486 * inv_set_control_data allows flexibility in assigning physical parameters to
487 * control signals. For example, the user is allowed to use raw gyroscope data
488 * as an input to the control algorithm.
489 * Alternatively, angular velocity can be used, which combines gyroscopes and
490 * accelerometers to provide a more robust physical parameter. Finally, angular
491 * velocity in world coordinates can be used, providing a control signal in
492 * which pitch and yaw are provided relative to gravity.
493 *
494 * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
495 * inv_open_low_power_pedometer().
496 *
497 * @param controlSignal Indicates which control signal is being modified.
498 * Must be one of:
499 * - INV_CONTROL_1,
500 * - INV_CONTROL_2,
501 * - INV_CONTROL_3 or
502 * - INV_CONTROL_4.
503 *
504 * @param parameterArray Indicates which parameter array is being assigned to a
505 * control signal. Must be one of:
506 * - INV_GYROS,
507 * - INV_ANGULAR_VELOCITY, or
508 *
509 * @param parameterAxis Indicates which axis of the parameter array will be used.
510 * Must be:
511 * - INV_ROLL,
512 * - INV_PITCH, or
513 * - INV_YAW.
514 */
515
inv_set_control_data(unsigned short controlSignal,unsigned short parameterArray,unsigned short parameterAxis)516 inv_error_t inv_set_control_data(unsigned short controlSignal,
517 unsigned short parameterArray,
518 unsigned short parameterAxis)
519 {
520 INVENSENSE_FUNC_START;
521 unsigned char regs[2] = { DINA80 + 10, DINA20 };
522 inv_error_t result;
523
524 if (inv_get_state() != INV_STATE_DMP_OPENED)
525 return INV_ERROR_SM_IMPROPER_STATE;
526
527 if (parameterArray == INV_ANGULAR_VELOCITY) {
528 regs[0] = DINA80 + 5;
529 regs[1] = DINA00;
530 }
531 switch (controlSignal) {
532 case INV_CONTROL_1:
533 cntrl_params.parameterArray[0] = parameterArray;
534 switch (parameterAxis) {
535 case INV_PITCH:
536 regs[1] += 0x02;
537 cntrl_params.parameterAxis[0] = 0;
538 break;
539 case INV_ROLL:
540 regs[1] = DINA22;
541 cntrl_params.parameterAxis[0] = 1;
542 break;
543 case INV_YAW:
544 regs[1] = DINA42;
545 cntrl_params.parameterAxis[0] = 2;
546 break;
547 default:
548 return INV_ERROR_INVALID_PARAMETER;
549 }
550 result = inv_set_mpu_memory(KEY_CFG_3, 2, regs);
551 if (result) {
552 LOG_RESULT_LOCATION(result);
553 return result;
554 }
555 break;
556 case INV_CONTROL_2:
557 cntrl_params.parameterArray[1] = parameterArray;
558 switch (parameterAxis) {
559 case INV_PITCH:
560 regs[1] += DINA0E;
561 cntrl_params.parameterAxis[1] = 0;
562 break;
563 case INV_ROLL:
564 regs[1] += DINA2E;
565 cntrl_params.parameterAxis[1] = 1;
566 break;
567 case INV_YAW:
568 regs[1] += DINA4E;
569 cntrl_params.parameterAxis[1] = 2;
570 break;
571 default:
572 return INV_ERROR_INVALID_PARAMETER;
573 }
574 result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs);
575 if (result) {
576 LOG_RESULT_LOCATION(result);
577 return result;
578 }
579 break;
580 case INV_CONTROL_3:
581 cntrl_params.parameterArray[2] = parameterArray;
582 switch (parameterAxis) {
583 case INV_PITCH:
584 regs[1] += DINA0E;
585 cntrl_params.parameterAxis[2] = 0;
586 break;
587 case INV_ROLL:
588 regs[1] += DINA2E;
589 cntrl_params.parameterAxis[2] = 1;
590 break;
591 case INV_YAW:
592 regs[1] += DINA4E;
593 cntrl_params.parameterAxis[2] = 2;
594 break;
595 default:
596 return INV_ERROR_INVALID_PARAMETER;
597 }
598 result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs);
599 if (result) {
600 LOG_RESULT_LOCATION(result);
601 return result;
602 }
603 break;
604 case INV_CONTROL_4:
605 cntrl_params.parameterArray[3] = parameterArray;
606 switch (parameterAxis) {
607 case INV_PITCH:
608 regs[1] += DINA0E;
609 cntrl_params.parameterAxis[3] = 0;
610 break;
611 case INV_ROLL:
612 regs[1] += DINA2E;
613 cntrl_params.parameterAxis[3] = 1;
614 break;
615 case INV_YAW:
616 regs[1] += DINA4E;
617 cntrl_params.parameterAxis[3] = 2;
618 break;
619 default:
620 return INV_ERROR_INVALID_PARAMETER;
621 }
622 result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs);
623 if (result) {
624 LOG_RESULT_LOCATION(result);
625 return result;
626 }
627 break;
628 default:
629 result = INV_ERROR_INVALID_PARAMETER;
630 break;
631 }
632 return result;
633 }
634
635 /**
636 * @brief inv_get_control_data is used to get the current control data.
637 *
638 * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
639 * inv_open_low_power_pedometer().
640 *
641 * @param controlSignal Indicates which control signal is being queried.
642 * Must be one of:
643 * - INV_CONTROL_1,
644 * - INV_CONTROL_2,
645 * - INV_CONTROL_3 or
646 * - INV_CONTROL_4.
647 *
648 * @param gridNum A pointer to pass gridNum info back to the user.
649 * @param gridChange A pointer to pass gridChange info back to the user.
650 *
651 * @return Zero if the command is successful; an ML error code otherwise.
652 */
653
inv_get_control_data(long * controlSignal,long * gridNum,long * gridChange)654 inv_error_t inv_get_control_data(long *controlSignal, long *gridNum,
655 long *gridChange)
656 {
657 INVENSENSE_FUNC_START;
658 int_fast8_t i = 0;
659
660 if (inv_get_state() != INV_STATE_DMP_STARTED)
661 return INV_ERROR_SM_IMPROPER_STATE;
662
663 for (i = 0; i < 4; i++) {
664 controlSignal[i] = cntrl_obj.controlInt[i];
665 gridNum[i] = cntrl_obj.gridNum[i];
666 gridChange[i] = cntrl_obj.gridChange[i];
667 }
668 return INV_SUCCESS;
669 }
670
671 /**
672 * @internal
673 * @brief Update the ML Control engine. This function should be called
674 * every time new data from the MPU becomes available.
675 * Control engine outputs are written to the cntrl_obj data
676 * structure.
677 * @return INV_SUCCESS or an error code.
678 **/
inv_update_control(struct inv_obj_t * inv_obj)679 inv_error_t inv_update_control(struct inv_obj_t * inv_obj)
680 {
681 INVENSENSE_FUNC_START;
682 unsigned char i;
683 long gridTmp;
684 long tmp;
685
686 inv_get_cntrl_data(cntrl_obj.mlGridNumDMP);
687
688 for (i = 0; i < 4; i++) {
689 if (cntrl_params.functions & INV_GRID) {
690 if (cntrl_params.functions & INV_HYSTERESIS) {
691 cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i];
692 }
693 cntrl_obj.mlGridNumDMP[i] =
694 cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L;
695 cntrl_obj.controlInt[i] =
696 (cntrl_obj.mlGridNumDMP[i] %
697 (128 * cntrl_params.gridThreshold[i])) / 128;
698 gridTmp =
699 cntrl_obj.mlGridNumDMP[i] / (128 *
700 cntrl_params.gridThreshold[i]);
701 tmp = 1 + 16777216L / cntrl_params.gridThreshold[i];
702 cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i];
703 if (cntrl_obj.gridChange[i] > tmp / 2) {
704 cntrl_obj.gridChange[i] =
705 gridTmp - tmp - cntrl_obj.lastGridNum[i];
706 } else if (cntrl_obj.gridChange[i] < -tmp / 2) {
707 cntrl_obj.gridChange[i] =
708 gridTmp + tmp - cntrl_obj.lastGridNum[i];
709 }
710 if ((cntrl_params.functions & INV_HYSTERESIS)
711 && (cntrl_obj.gridChange[i] != 0)) {
712 if (cntrl_obj.gridChange[i] > 0) {
713 cntrl_obj.gridNumOffset[i] +=
714 128 * cntrl_params.gridThreshold[i];
715 cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
716 }
717 if (cntrl_obj.gridChange[i] < 0) {
718 cntrl_obj.gridNumOffset[i] -=
719 128 * cntrl_params.gridThreshold[i];
720 cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
721 }
722 }
723 cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i];
724 if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) {
725 cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i];
726 if (cntrl_obj.controlInt[i] >=
727 cntrl_params.gridThreshold[i] / 2) {
728 cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
729 }
730 } else if (cntrl_obj.gridNum[i] <= 0) {
731 cntrl_obj.gridNum[i] = 0;
732 if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) {
733 cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2;
734 }
735 }
736 cntrl_obj.lastGridNum[i] = gridTmp;
737 if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) {
738 cntrl_params.gridCallback((INV_CONTROL_1 << i),
739 cntrl_obj.gridNum,
740 cntrl_obj.gridChange);
741 }
742
743 } else {
744 cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i];
745 }
746
747 }
748
749 return INV_SUCCESS;
750 }
751
752 /**
753 * @brief Enables the INV_CONTROL engine.
754 *
755 * @note This function replaces MLEnable(INV_CONTROL)
756 *
757 * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must
758 * have been called.
759 *
760 * @return INV_SUCCESS or non-zero error code
761 */
inv_enable_control(void)762 inv_error_t inv_enable_control(void)
763 {
764 INVENSENSE_FUNC_START;
765
766 if (inv_get_state() != INV_STATE_DMP_OPENED)
767 return INV_ERROR_SM_IMPROPER_STATE;
768
769 memset(&cntrl_obj, 0, sizeof(cntrl_obj));
770
771 inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL); // fixme, someone needs to send control data to the fifo
772 return INV_SUCCESS;
773 }
774
775 /**
776 * @brief Disables the INV_CONTROL engine.
777 *
778 * @note This function replaces MLDisable(INV_CONTROL)
779 *
780 * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must
781 * have been called.
782 *
783 * @return INV_SUCCESS or non-zero error code
784 */
inv_disable_control(void)785 inv_error_t inv_disable_control(void)
786 {
787 INVENSENSE_FUNC_START;
788
789 if (inv_get_state() < INV_STATE_DMP_STARTED)
790 return INV_ERROR_SM_IMPROPER_STATE;
791
792 return INV_SUCCESS;
793 }
794
795 /**
796 * @}
797 */
798