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 */ 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 */ 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 */ 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 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 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 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 **/ 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 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 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 **/ 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 */ 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 */ 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
