| /* |
| $License: |
| Copyright 2011 InvenSense, Inc. |
| |
| Licensed under the Apache License, Version 2.0 (the "License"); |
| you may not use this file except in compliance with the License. |
| You may obtain a copy of the License at |
| |
| http://www.apache.org/licenses/LICENSE-2.0 |
| |
| Unless required by applicable law or agreed to in writing, software |
| distributed under the License is distributed on an "AS IS" BASIS, |
| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| See the License for the specific language governing permissions and |
| limitations under the License. |
| $ |
| */ |
| |
| /******************************************************************************* |
| * |
| * $Id: mlcontrol.c 5641 2011-06-14 02:10:02Z mcaramello $ |
| * |
| *******************************************************************************/ |
| |
| /** |
| * @defgroup CONTROL |
| * @brief Motion Library - Control Engine. |
| * The Control Library processes gyroscopes, accelerometers, and |
| * compasses to provide control signals that can be used in user |
| * interfaces. |
| * These signals can be used to manipulate objects such as documents, |
| * images, cursors, menus, etc. |
| * |
| * @{ |
| * @file mlcontrol.c |
| * @brief The Control Library. |
| * |
| */ |
| |
| /* ------------------ */ |
| /* - Include Files. - */ |
| /* ------------------ */ |
| |
| #include "mltypes.h" |
| #include "mlinclude.h" |
| #include "mltypes.h" |
| #include "ml.h" |
| #include "mlos.h" |
| #include "mlsl.h" |
| #include "mldl.h" |
| #include "mlcontrol.h" |
| #include "dmpKey.h" |
| #include "mlstates.h" |
| #include "mlFIFO.h" |
| #include "string.h" |
| |
| /* - Global Vars. - */ |
| struct control_params cntrl_params = { |
| { |
| MLCTRL_SENSITIVITY_0_DEFAULT, |
| MLCTRL_SENSITIVITY_1_DEFAULT, |
| MLCTRL_SENSITIVITY_2_DEFAULT, |
| MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity |
| MLCTRL_FUNCTIONS_DEFAULT, // functions |
| { |
| MLCTRL_PARAMETER_ARRAY_0_DEFAULT, |
| MLCTRL_PARAMETER_ARRAY_1_DEFAULT, |
| MLCTRL_PARAMETER_ARRAY_2_DEFAULT, |
| MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray |
| { |
| MLCTRL_PARAMETER_AXIS_0_DEFAULT, |
| MLCTRL_PARAMETER_AXIS_1_DEFAULT, |
| MLCTRL_PARAMETER_AXIS_2_DEFAULT, |
| MLCTRL_PARAMETER_AXIS_3_DEFAULT}, // parameterAxis |
| { |
| MLCTRL_GRID_THRESHOLD_0_DEFAULT, |
| MLCTRL_GRID_THRESHOLD_1_DEFAULT, |
| MLCTRL_GRID_THRESHOLD_2_DEFAULT, |
| MLCTRL_GRID_THRESHOLD_3_DEFAULT}, // gridThreshold |
| { |
| MLCTRL_GRID_MAXIMUM_0_DEFAULT, |
| MLCTRL_GRID_MAXIMUM_1_DEFAULT, |
| MLCTRL_GRID_MAXIMUM_2_DEFAULT, |
| MLCTRL_GRID_MAXIMUM_3_DEFAULT}, // gridMaximum |
| MLCTRL_GRID_CALLBACK_DEFAULT // gridCallback |
| }; |
| |
| /* - Extern Vars. - */ |
| struct control_obj cntrl_obj; |
| extern const unsigned char *dmpConfig1; |
| |
| /* -------------- */ |
| /* - Functions. - */ |
| /* -------------- */ |
| |
| /** |
| * @brief inv_set_control_sensitivity is used to set the sensitivity for a control |
| * signal. |
| * |
| * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or |
| * inv_open_low_power_pedometer(). |
| * |
| * @param controlSignal Indicates which control signal is being modified. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 or |
| * - INV_CONTROL_4. |
| * |
| * @param sensitivity The sensitivity of the control signal. |
| * |
| * @return error code |
| */ |
| inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, |
| long sensitivity) |
| { |
| INVENSENSE_FUNC_START; |
| unsigned char regs[2]; |
| long finalSens = 0; |
| inv_error_t result; |
| |
| if (inv_get_state() < INV_STATE_DMP_OPENED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| finalSens = sensitivity * 100; |
| if (finalSens > 16384) { |
| finalSens = 16384; |
| } |
| regs[0] = (unsigned char)(finalSens / 256); |
| regs[1] = (unsigned char)(finalSens % 256); |
| switch (controlSignal) { |
| case INV_CONTROL_1: |
| result = inv_set_mpu_memory(KEY_D_0_224, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| cntrl_params.sensitivity[0] = (unsigned short)sensitivity; |
| break; |
| case INV_CONTROL_2: |
| result = inv_set_mpu_memory(KEY_D_0_228, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| cntrl_params.sensitivity[1] = (unsigned short)sensitivity; |
| break; |
| case INV_CONTROL_3: |
| result = inv_set_mpu_memory(KEY_D_0_232, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| cntrl_params.sensitivity[2] = (unsigned short)sensitivity; |
| break; |
| case INV_CONTROL_4: |
| result = inv_set_mpu_memory(KEY_D_0_236, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| cntrl_params.sensitivity[3] = (unsigned short)sensitivity; |
| break; |
| default: |
| break; |
| } |
| if (finalSens != sensitivity * 100) { |
| return INV_ERROR_INVALID_PARAMETER; |
| } else { |
| return INV_SUCCESS; |
| } |
| } |
| |
| /** |
| * @brief inv_set_control_func allows the user to choose how the sensor data will |
| * be processed in order to provide a control parameter. |
| * inv_set_control_func allows the user to choose which control functions |
| * will be incorporated in the sensor data processing. |
| * The control functions are: |
| * - INV_GRID |
| * Indicates that the user will be controlling a system that |
| * has discrete steps, such as icons, menu entries, pixels, etc. |
| * - INV_SMOOTH |
| * Indicates that noise from unintentional motion should be filtered out. |
| * - INV_DEAD_ZONE |
| * Indicates that a dead zone should be used, below which sensor |
| * data is set to zero. |
| * - INV_HYSTERESIS |
| * Indicates that, when INV_GRID is selected, hysteresis should |
| * be used to prevent the control signal from switching rapidly across |
| * elements of the grid. |
| * |
| * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or |
| * inv_open_low_power_pedometer(). |
| * |
| * @param function Indicates what functions will be used. |
| * Can be a bitwise OR of several values. |
| * |
| * @return Zero if the command is successful; an ML error code otherwise. |
| */ |
| inv_error_t inv_set_control_func(unsigned short function) |
| { |
| INVENSENSE_FUNC_START; |
| unsigned char regs[8] = { DINA06, DINA26, |
| DINA46, DINA66, |
| DINA0E, DINA2E, |
| DINA4E, DINA6E |
| }; |
| unsigned char i; |
| inv_error_t result; |
| |
| if (inv_get_state() < INV_STATE_DMP_OPENED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| if ((function & INV_SMOOTH) == 0) { |
| for (i = 0; i < 8; i++) { |
| regs[i] = DINA80 + 3; |
| } |
| } |
| result = inv_set_mpu_memory(KEY_CFG_4, 8, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| cntrl_params.functions = function; |
| result = inv_set_dead_zone(); |
| |
| return result; |
| } |
| |
| /** |
| * @brief inv_get_control_signal is used to get the current control signal with |
| * high precision. |
| * inv_get_control_signal is used to acquire the current data of a control signal. |
| * If INV_GRID is being used, inv_get_grid_number will probably be preferrable. |
| * |
| * @param controlSignal Indicates which control signal is being queried. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 or |
| * - INV_CONTROL_4. |
| * |
| * @param reset Indicates whether the control signal should be reset to zero. |
| * Options are INV_RESET or INV_NO_RESET |
| * @param data A pointer to the current control signal data. |
| * |
| * @return Zero if the command is successful; an ML error code otherwise. |
| */ |
| inv_error_t inv_get_control_signal(unsigned short controlSignal, |
| unsigned short reset, long *data) |
| { |
| INVENSENSE_FUNC_START; |
| |
| if (inv_get_state() != INV_STATE_DMP_STARTED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| switch (controlSignal) { |
| case INV_CONTROL_1: |
| *data = cntrl_obj.controlInt[0]; |
| if (reset == INV_RESET) { |
| cntrl_obj.controlInt[0] = 0; |
| } |
| break; |
| case INV_CONTROL_2: |
| *data = cntrl_obj.controlInt[1]; |
| if (reset == INV_RESET) { |
| cntrl_obj.controlInt[1] = 0; |
| } |
| break; |
| case INV_CONTROL_3: |
| *data = cntrl_obj.controlInt[2]; |
| if (reset == INV_RESET) { |
| cntrl_obj.controlInt[2] = 0; |
| } |
| break; |
| case INV_CONTROL_4: |
| *data = cntrl_obj.controlInt[3]; |
| if (reset == INV_RESET) { |
| cntrl_obj.controlInt[3] = 0; |
| } |
| break; |
| default: |
| break; |
| } |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief inv_get_grid_num is used to get the current grid location for a certain |
| * control signal. |
| * inv_get_grid_num is used to acquire the current grid location. |
| * |
| * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or |
| * inv_open_low_power_pedometer(). |
| * |
| * @param controlSignal Indicates which control signal is being queried. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 or |
| * - INV_CONTROL_4. |
| * |
| * @param reset Indicates whether the control signal should be reset to zero. |
| * Options are INV_RESET or INV_NO_RESET |
| * @param data A pointer to the current grid number. |
| * |
| * @return Zero if the command is successful; an ML error code otherwise. |
| */ |
| |
| inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset, |
| long *data) |
| { |
| INVENSENSE_FUNC_START; |
| |
| if (inv_get_state() != INV_STATE_DMP_STARTED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| switch (controlSignal) { |
| case INV_CONTROL_1: |
| *data = cntrl_obj.gridNum[0]; |
| if (reset == INV_RESET) { |
| cntrl_obj.gridNum[0] = 0; |
| } |
| break; |
| case INV_CONTROL_2: |
| *data = cntrl_obj.gridNum[1]; |
| if (reset == INV_RESET) { |
| cntrl_obj.gridNum[1] = 0; |
| } |
| break; |
| case INV_CONTROL_3: |
| *data = cntrl_obj.gridNum[2]; |
| if (reset == INV_RESET) { |
| cntrl_obj.gridNum[2] = 0; |
| } |
| break; |
| case INV_CONTROL_4: |
| *data = cntrl_obj.gridNum[3]; |
| if (reset == INV_RESET) { |
| cntrl_obj.gridNum[3] = 0; |
| } |
| break; |
| default: |
| break; |
| } |
| |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief inv_set_grid_thresh is used to set the grid size for a control signal. |
| * inv_set_grid_thresh is used to adjust the size of the grid being controlled. |
| * @param controlSignal Indicates which control signal is being modified. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 and |
| * - INV_CONTROL_4. |
| * @param threshold The threshold of the control signal at which the grid |
| * number will be incremented or decremented. |
| * @return Zero if the command is successful; an ML error code otherwise. |
| */ |
| |
| inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold) |
| { |
| INVENSENSE_FUNC_START; |
| |
| if (inv_get_state() < INV_STATE_DMP_OPENED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| switch (controlSignal) { |
| case INV_CONTROL_1: |
| cntrl_params.gridThreshold[0] = threshold; |
| break; |
| case INV_CONTROL_2: |
| cntrl_params.gridThreshold[1] = threshold; |
| break; |
| case INV_CONTROL_3: |
| cntrl_params.gridThreshold[2] = threshold; |
| break; |
| case INV_CONTROL_4: |
| cntrl_params.gridThreshold[3] = threshold; |
| break; |
| default: |
| return INV_ERROR_INVALID_PARAMETER; |
| break; |
| } |
| |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief inv_set_grid_max is used to set the maximum grid number for a control signal. |
| * inv_set_grid_max is used to adjust the maximum allowed grid number, above |
| * which the grid number will not be incremented. |
| * The minimum grid number is always zero. |
| * |
| * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or |
| * inv_open_low_power_pedometer(). |
| * |
| * @param controlSignal Indicates which control signal is being modified. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 and |
| * - INV_CONTROL_4. |
| * |
| * @param maximum The maximum grid number for a control signal. |
| * @return Zero if the command is successful; an ML error code otherwise. |
| */ |
| |
| inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum) |
| { |
| INVENSENSE_FUNC_START; |
| |
| if (inv_get_state() != INV_STATE_DMP_OPENED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| switch (controlSignal) { |
| case INV_CONTROL_1: |
| cntrl_params.gridMaximum[0] = maximum; |
| break; |
| case INV_CONTROL_2: |
| cntrl_params.gridMaximum[1] = maximum; |
| break; |
| case INV_CONTROL_3: |
| cntrl_params.gridMaximum[2] = maximum; |
| break; |
| case INV_CONTROL_4: |
| cntrl_params.gridMaximum[3] = maximum; |
| break; |
| default: |
| return INV_ERROR_INVALID_PARAMETER; |
| break; |
| } |
| |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief GridCallback function pointer type, to be passed as argument of |
| * inv_set_grid_callback. |
| * |
| * @param controlSignal Indicates which control signal crossed a grid threshold. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 and |
| * - INV_CONTROL_4. |
| * |
| * @param gridNumber An array of four numbers representing the grid number for each |
| * control signal. |
| * @param gridChange An array of four numbers representing the change in grid number |
| * for each control signal. |
| **/ |
| typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum, |
| long *gridChange); |
| |
| /** |
| * @brief inv_set_grid_callback is used to register a callback function that |
| * will trigger when the grid location changes. |
| * inv_set_grid_callback allows a user to define a callback function that will |
| * run when a control signal crosses a grid threshold. |
| |
| * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or |
| * inv_open_low_power_pedometer(). inv_dmp_start must <b>NOT</b> have |
| * been called. |
| * |
| * @param func A user defined callback function |
| * @return Zero if the command is successful; an ML error code otherwise. |
| **/ |
| inv_error_t inv_set_grid_callback(fpGridCb func) |
| { |
| INVENSENSE_FUNC_START; |
| |
| if (inv_get_state() != INV_STATE_DMP_OPENED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| cntrl_params.gridCallback = func; |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief inv_set_control_data is used to assign physical parameters to control signals. |
| * inv_set_control_data allows flexibility in assigning physical parameters to |
| * control signals. For example, the user is allowed to use raw gyroscope data |
| * as an input to the control algorithm. |
| * Alternatively, angular velocity can be used, which combines gyroscopes and |
| * accelerometers to provide a more robust physical parameter. Finally, angular |
| * velocity in world coordinates can be used, providing a control signal in |
| * which pitch and yaw are provided relative to gravity. |
| * |
| * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or |
| * inv_open_low_power_pedometer(). |
| * |
| * @param controlSignal Indicates which control signal is being modified. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 or |
| * - INV_CONTROL_4. |
| * |
| * @param parameterArray Indicates which parameter array is being assigned to a |
| * control signal. Must be one of: |
| * - INV_GYROS, |
| * - INV_ANGULAR_VELOCITY, or |
| * |
| * @param parameterAxis Indicates which axis of the parameter array will be used. |
| * Must be: |
| * - INV_ROLL, |
| * - INV_PITCH, or |
| * - INV_YAW. |
| */ |
| |
| inv_error_t inv_set_control_data(unsigned short controlSignal, |
| unsigned short parameterArray, |
| unsigned short parameterAxis) |
| { |
| INVENSENSE_FUNC_START; |
| unsigned char regs[2] = { DINA80 + 10, DINA20 }; |
| inv_error_t result; |
| |
| if (inv_get_state() != INV_STATE_DMP_OPENED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| if (parameterArray == INV_ANGULAR_VELOCITY) { |
| regs[0] = DINA80 + 5; |
| regs[1] = DINA00; |
| } |
| switch (controlSignal) { |
| case INV_CONTROL_1: |
| cntrl_params.parameterArray[0] = parameterArray; |
| switch (parameterAxis) { |
| case INV_PITCH: |
| regs[1] += 0x02; |
| cntrl_params.parameterAxis[0] = 0; |
| break; |
| case INV_ROLL: |
| regs[1] = DINA22; |
| cntrl_params.parameterAxis[0] = 1; |
| break; |
| case INV_YAW: |
| regs[1] = DINA42; |
| cntrl_params.parameterAxis[0] = 2; |
| break; |
| default: |
| return INV_ERROR_INVALID_PARAMETER; |
| } |
| result = inv_set_mpu_memory(KEY_CFG_3, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| break; |
| case INV_CONTROL_2: |
| cntrl_params.parameterArray[1] = parameterArray; |
| switch (parameterAxis) { |
| case INV_PITCH: |
| regs[1] += DINA0E; |
| cntrl_params.parameterAxis[1] = 0; |
| break; |
| case INV_ROLL: |
| regs[1] += DINA2E; |
| cntrl_params.parameterAxis[1] = 1; |
| break; |
| case INV_YAW: |
| regs[1] += DINA4E; |
| cntrl_params.parameterAxis[1] = 2; |
| break; |
| default: |
| return INV_ERROR_INVALID_PARAMETER; |
| } |
| result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| break; |
| case INV_CONTROL_3: |
| cntrl_params.parameterArray[2] = parameterArray; |
| switch (parameterAxis) { |
| case INV_PITCH: |
| regs[1] += DINA0E; |
| cntrl_params.parameterAxis[2] = 0; |
| break; |
| case INV_ROLL: |
| regs[1] += DINA2E; |
| cntrl_params.parameterAxis[2] = 1; |
| break; |
| case INV_YAW: |
| regs[1] += DINA4E; |
| cntrl_params.parameterAxis[2] = 2; |
| break; |
| default: |
| return INV_ERROR_INVALID_PARAMETER; |
| } |
| result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| break; |
| case INV_CONTROL_4: |
| cntrl_params.parameterArray[3] = parameterArray; |
| switch (parameterAxis) { |
| case INV_PITCH: |
| regs[1] += DINA0E; |
| cntrl_params.parameterAxis[3] = 0; |
| break; |
| case INV_ROLL: |
| regs[1] += DINA2E; |
| cntrl_params.parameterAxis[3] = 1; |
| break; |
| case INV_YAW: |
| regs[1] += DINA4E; |
| cntrl_params.parameterAxis[3] = 2; |
| break; |
| default: |
| return INV_ERROR_INVALID_PARAMETER; |
| } |
| result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| break; |
| default: |
| result = INV_ERROR_INVALID_PARAMETER; |
| break; |
| } |
| return result; |
| } |
| |
| /** |
| * @brief inv_get_control_data is used to get the current control data. |
| * |
| * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or |
| * inv_open_low_power_pedometer(). |
| * |
| * @param controlSignal Indicates which control signal is being queried. |
| * Must be one of: |
| * - INV_CONTROL_1, |
| * - INV_CONTROL_2, |
| * - INV_CONTROL_3 or |
| * - INV_CONTROL_4. |
| * |
| * @param gridNum A pointer to pass gridNum info back to the user. |
| * @param gridChange A pointer to pass gridChange info back to the user. |
| * |
| * @return Zero if the command is successful; an ML error code otherwise. |
| */ |
| |
| inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, |
| long *gridChange) |
| { |
| INVENSENSE_FUNC_START; |
| int_fast8_t i = 0; |
| |
| if (inv_get_state() != INV_STATE_DMP_STARTED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| for (i = 0; i < 4; i++) { |
| controlSignal[i] = cntrl_obj.controlInt[i]; |
| gridNum[i] = cntrl_obj.gridNum[i]; |
| gridChange[i] = cntrl_obj.gridChange[i]; |
| } |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @internal |
| * @brief Update the ML Control engine. This function should be called |
| * every time new data from the MPU becomes available. |
| * Control engine outputs are written to the cntrl_obj data |
| * structure. |
| * @return INV_SUCCESS or an error code. |
| **/ |
| inv_error_t inv_update_control(struct inv_obj_t * inv_obj) |
| { |
| INVENSENSE_FUNC_START; |
| unsigned char i; |
| long gridTmp; |
| long tmp; |
| |
| inv_get_cntrl_data(cntrl_obj.mlGridNumDMP); |
| |
| for (i = 0; i < 4; i++) { |
| if (cntrl_params.functions & INV_GRID) { |
| if (cntrl_params.functions & INV_HYSTERESIS) { |
| cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i]; |
| } |
| cntrl_obj.mlGridNumDMP[i] = |
| cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L; |
| cntrl_obj.controlInt[i] = |
| (cntrl_obj.mlGridNumDMP[i] % |
| (128 * cntrl_params.gridThreshold[i])) / 128; |
| gridTmp = |
| cntrl_obj.mlGridNumDMP[i] / (128 * |
| cntrl_params.gridThreshold[i]); |
| tmp = 1 + 16777216L / cntrl_params.gridThreshold[i]; |
| cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i]; |
| if (cntrl_obj.gridChange[i] > tmp / 2) { |
| cntrl_obj.gridChange[i] = |
| gridTmp - tmp - cntrl_obj.lastGridNum[i]; |
| } else if (cntrl_obj.gridChange[i] < -tmp / 2) { |
| cntrl_obj.gridChange[i] = |
| gridTmp + tmp - cntrl_obj.lastGridNum[i]; |
| } |
| if ((cntrl_params.functions & INV_HYSTERESIS) |
| && (cntrl_obj.gridChange[i] != 0)) { |
| if (cntrl_obj.gridChange[i] > 0) { |
| cntrl_obj.gridNumOffset[i] += |
| 128 * cntrl_params.gridThreshold[i]; |
| cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; |
| } |
| if (cntrl_obj.gridChange[i] < 0) { |
| cntrl_obj.gridNumOffset[i] -= |
| 128 * cntrl_params.gridThreshold[i]; |
| cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; |
| } |
| } |
| cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i]; |
| if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) { |
| cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i]; |
| if (cntrl_obj.controlInt[i] >= |
| cntrl_params.gridThreshold[i] / 2) { |
| cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; |
| } |
| } else if (cntrl_obj.gridNum[i] <= 0) { |
| cntrl_obj.gridNum[i] = 0; |
| if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) { |
| cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; |
| } |
| } |
| cntrl_obj.lastGridNum[i] = gridTmp; |
| if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) { |
| cntrl_params.gridCallback((INV_CONTROL_1 << i), |
| cntrl_obj.gridNum, |
| cntrl_obj.gridChange); |
| } |
| |
| } else { |
| cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i]; |
| } |
| |
| } |
| |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief Enables the INV_CONTROL engine. |
| * |
| * @note This function replaces MLEnable(INV_CONTROL) |
| * |
| * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must |
| * have been called. |
| * |
| * @return INV_SUCCESS or non-zero error code |
| */ |
| inv_error_t inv_enable_control(void) |
| { |
| INVENSENSE_FUNC_START; |
| |
| if (inv_get_state() != INV_STATE_DMP_OPENED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| memset(&cntrl_obj, 0, sizeof(cntrl_obj)); |
| |
| inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL); // fixme, someone needs to send control data to the fifo |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief Disables the INV_CONTROL engine. |
| * |
| * @note This function replaces MLDisable(INV_CONTROL) |
| * |
| * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must |
| * have been called. |
| * |
| * @return INV_SUCCESS or non-zero error code |
| */ |
| inv_error_t inv_disable_control(void) |
| { |
| INVENSENSE_FUNC_START; |
| |
| if (inv_get_state() < INV_STATE_DMP_STARTED) |
| return INV_ERROR_SM_IMPROPER_STATE; |
| |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @} |
| */ |
