diff options
Diffstat (limited to 'libsensors/mlsdk/mllite/ml.c')
| -rw-r--r-- | libsensors/mlsdk/mllite/ml.c | 1791 |
1 files changed, 1791 insertions, 0 deletions
diff --git a/libsensors/mlsdk/mllite/ml.c b/libsensors/mlsdk/mllite/ml.c new file mode 100644 index 0000000..d830be7 --- /dev/null +++ b/libsensors/mlsdk/mllite/ml.c @@ -0,0 +1,1791 @@ +/* + $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: ml.c 5642 2011-06-14 02:26:20Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML + * @brief Motion Library APIs. + * The Motion Library processes gyroscopes, accelerometers, and + * compasses to provide a physical model of the movement for the + * sensors. + * The results of this processing may be used to control objects + * within a user interface environment, detect gestures, track 3D + * movement for gaming applications, and analyze the blur created + * due to hand movement while taking a picture. + * + * @{ + * @file ml.c + * @brief The Motion Library APIs. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include <string.h> + +#include "ml.h" +#include "mldl.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "compass.h" +#include "dmpKey.h" +#include "dmpDefault.h" +#include "mlstates.h" +#include "mlFIFO.h" +#include "mlFIFOHW.h" +#include "mlMathFunc.h" +#include "mlsupervisor.h" +#include "mlmath.h" +#include "mlcontrol.h" +#include "mldl_cfg.h" +#include "mpu.h" +#include "accel.h" +#include "mlos.h" +#include "mlsl.h" +#include "mlos.h" +#include "mlBiasNoMotion.h" +#include "mlSetGyroBias.h" +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-ml" + +#define ML_MOT_TYPE_NONE 0 +#define ML_MOT_TYPE_NO_MOTION 1 +#define ML_MOT_TYPE_MOTION_DETECTED 2 + +#define ML_MOT_STATE_MOVING 0 +#define ML_MOT_STATE_NO_MOTION 1 +#define ML_MOT_STATE_BIAS_IN_PROG 2 + +#define _mlDebug(x) //{x} + +/* Global Variables */ +const unsigned char mlVer[] = { INV_VERSION }; + +struct inv_params_obj inv_params_obj = { + INV_BIAS_UPDATE_FUNC_DEFAULT, // bias_mode + INV_ORIENTATION_MASK_DEFAULT, // orientation_mask + INV_PROCESSED_DATA_CALLBACK_DEFAULT, // fifo_processed_func + INV_ORIENTATION_CALLBACK_DEFAULT, // orientation_cb_func + INV_MOTION_CALLBACK_DEFAULT, // motion_cb_func + INV_STATE_SERIAL_CLOSED // starting state +}; + +struct inv_obj_t inv_obj; +void *g_mlsl_handle; + +typedef struct { + // These describe callbacks happening everythime a DMP interrupt is processed + int_fast8_t numInterruptProcesses; + // Array of function pointers, function being void function taking void + inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES]; + +} tMLXCallbackInterrupt; // MLX_callback_t + +tMLXCallbackInterrupt mlxCallbackInterrupt; + +/* --------------- */ +/* - Functions. - */ +/* --------------- */ + +inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient); +inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient); +unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); + +/** + * @brief Open serial connection with the MPU device. + * This is the entry point of the MPL and must be + * called prior to any other function call. + * + * @param port System handle for 'port' MPU device is found on. + * The significance of this parameter varies by + * platform. It is passed as 'port' to MLSLSerialOpen. + * + * @return INV_SUCCESS or error code. + */ +inv_error_t inv_serial_start(char const *port) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + + if (inv_get_state() >= INV_STATE_SERIAL_OPENED) + return INV_SUCCESS; + + result = inv_state_transition(INV_STATE_SERIAL_OPENED); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_serial_open(port, &g_mlsl_handle); + if (INV_SUCCESS != result) { + (void)inv_state_transition(INV_STATE_SERIAL_CLOSED); + } + + return result; +} + +/** + * @brief Close the serial communication. + * This function needs to be called explicitly to shut down + * the communication with the device. Calling inv_dmp_close() + * won't affect the established serial communication. + * @return INV_SUCCESS; non-zero error code otherwise. + */ +inv_error_t inv_serial_stop(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() == INV_STATE_SERIAL_CLOSED) + return INV_SUCCESS; + + result = inv_state_transition(INV_STATE_SERIAL_CLOSED); + if (INV_SUCCESS != result) { + MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result); + } + result = inv_serial_close(g_mlsl_handle); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result); + } + return result; +} + +/** + * @brief Get the serial file handle to the device. + * @return The serial file handle. + */ +void *inv_get_serial_handle(void) +{ + INVENSENSE_FUNC_START; + return g_mlsl_handle; +} + +/** + * @brief apply the choosen orientation and full scale range + * for gyroscopes, accelerometer, and compass. + * @return INV_SUCCESS if successful, a non-zero code otherwise. + */ +inv_error_t inv_apply_calibration(void) +{ + INVENSENSE_FUNC_START; + signed char gyroCal[9] = { 0 }; + signed char accelCal[9] = { 0 }; + signed char magCal[9] = { 0 }; + float gyroScale = 2000.f; + float accelScale = 0.f; + float magScale = 0.f; + + inv_error_t result; + int ii; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + for (ii = 0; ii < 9; ii++) { + gyroCal[ii] = mldl_cfg->pdata->orientation[ii]; + if (NULL != mldl_cfg->accel){ + accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii]; + } + if (NULL != mldl_cfg->compass){ + magCal[ii] = mldl_cfg->pdata->compass.orientation[ii]; + } + } + + switch (mldl_cfg->full_scale) { + case MPU_FS_250DPS: + gyroScale = 250.f; + break; + case MPU_FS_500DPS: + gyroScale = 500.f; + break; + case MPU_FS_1000DPS: + gyroScale = 1000.f; + break; + case MPU_FS_2000DPS: + gyroScale = 2000.f; + break; + default: + MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n", + mldl_cfg->full_scale); + return INV_ERROR_INVALID_PARAMETER; + break; + } + + if (NULL != mldl_cfg->accel){ + RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale); + inv_obj.accel_sens = (long)(accelScale * 65536L); + /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */ + inv_obj.accel_sens /= 2; + } + if (NULL != mldl_cfg->compass){ + RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale); + inv_obj.compass_sens = (long)(magScale * 32768); + } + + if (inv_get_state() == INV_STATE_DMP_OPENED) { + result = inv_set_gyro_calibration(gyroScale, gyroCal); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to set Gyro Calibration\n"); + return result; + } + if (NULL != mldl_cfg->accel){ + result = inv_set_accel_calibration(accelScale, accelCal); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to set Accel Calibration\n"); + return result; + } + } + if (NULL != mldl_cfg->compass){ + result = inv_set_compass_calibration(magScale, magCal); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to set Mag Calibration\n"); + return result; + } + } + } + return INV_SUCCESS; +} + +/** + * @brief Setup the DMP to handle the accelerometer endianess. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_apply_endian_accel(void) +{ + INVENSENSE_FUNC_START; + unsigned char regs[4] = { 0 }; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + int endian = mldl_cfg->accel->endian; + + if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) { + endian = EXT_SLAVE_BIG_ENDIAN; + } + switch (endian) { + case EXT_SLAVE_FS8_BIG_ENDIAN: + case EXT_SLAVE_FS16_BIG_ENDIAN: + case EXT_SLAVE_LITTLE_ENDIAN: + regs[0] = 0; + regs[1] = 64; + regs[2] = 0; + regs[3] = 0; + break; + case EXT_SLAVE_BIG_ENDIAN: + default: + regs[0] = 0; + regs[1] = 0; + regs[2] = 64; + regs[3] = 0; + } + + return inv_set_mpu_memory(KEY_D_1_236, 4, regs); +} + +/** + * @internal + * @brief Initialize MLX data. This should be called to setup the mlx + * output buffers before any motion processing is done. + */ +void inv_init_ml(void) +{ + INVENSENSE_FUNC_START; + int ii; + long tmp[COMPASS_NUM_AXES]; + // Set all values to zero by default + memset(&inv_obj, 0, sizeof(struct inv_obj_t)); + memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt)); + + inv_obj.compass_correction[0] = 1073741824L; + inv_obj.compass_correction_relative[0] = 1073741824L; + inv_obj.compass_disturb_correction[0] = 1073741824L; + inv_obj.compass_correction_offset[0] = 1073741824L; + inv_obj.relative_quat[0] = 1073741824L; + + //Not used with the ST accelerometer + inv_obj.no_motion_threshold = 20; // noMotionThreshold + //Not used with the ST accelerometer + inv_obj.motion_duration = 1536; // motionDuration + + inv_obj.motion_state = INV_MOTION; // Motion state + + inv_obj.bias_update_time = 8000; + inv_obj.bias_calc_time = 2000; + + inv_obj.internal_motion_state = ML_MOT_STATE_MOVING; + + inv_obj.start_time = inv_get_tick_count(); + + inv_obj.compass_cal[0] = 322122560L; + inv_obj.compass_cal[4] = 322122560L; + inv_obj.compass_cal[8] = 322122560L; + inv_obj.compass_sens = 322122560L; // Should only change when the sensor full-scale range (FSR) is changed. + + for (ii = 0; ii < COMPASS_NUM_AXES; ii++) { + inv_obj.compass_scale[ii] = 65536L; + inv_obj.compass_test_scale[ii] = 65536L; + inv_obj.compass_bias_error[ii] = P_INIT; + inv_obj.init_compass_bias[ii] = 0; + inv_obj.compass_asa[ii] = (1L << 30); + } + if (inv_compass_read_scale(tmp) == INV_SUCCESS) { + for (ii = 0; ii < COMPASS_NUM_AXES; ii++) + inv_obj.compass_asa[ii] = tmp[ii]; + } + inv_obj.got_no_motion_bias = 0; + inv_obj.got_compass_bias = 0; + inv_obj.compass_state = SF_UNCALIBRATED; + inv_obj.acc_state = SF_STARTUP_SETTLE; + + inv_obj.got_init_compass_bias = 0; + inv_obj.resetting_compass = 0; + + inv_obj.external_slave_callback = NULL; + inv_obj.compass_accuracy = 0; + + inv_obj.factory_temp_comp = 0; + inv_obj.got_coarse_heading = 0; + + inv_obj.compass_bias_ptr[0] = P_INIT; + inv_obj.compass_bias_ptr[4] = P_INIT; + inv_obj.compass_bias_ptr[8] = P_INIT; + + inv_obj.gyro_bias_err = 1310720; + + inv_obj.accel_lpf_gain = 1073744L; + inv_obj.no_motion_accel_threshold = 7000000L; +} + +/** + * @internal + * @brief This registers a function to be called for each time the DMP + * generates an an interrupt. + * It will be called after inv_register_fifo_rate_process() which gets called + * every time the FIFO data is processed. + * The FIFO does not have to be on for this callback. + * @param func Function to be called when a DMP interrupt occurs. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func) +{ + INVENSENSE_FUNC_START; + // Make sure we have not filled up our number of allowable callbacks + if (mlxCallbackInterrupt.numInterruptProcesses <= + MAX_INTERRUPT_PROCESSES - 1) { + int kk; + // Make sure we haven't registered this function already + for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { + if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { + return INV_ERROR_INVALID_PARAMETER; + } + } + // Add new callback + mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt. + numInterruptProcesses] = func; + mlxCallbackInterrupt.numInterruptProcesses++; + return INV_SUCCESS; + } + return INV_ERROR_MEMORY_EXAUSTED; +} + +/** + * @internal + * @brief This unregisters a function to be called for each DMP interrupt. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func) +{ + INVENSENSE_FUNC_START; + int kk, jj; + // Make sure we haven't registered this function already + for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { + if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { + // FIXME, we may need a thread block here + for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses; + ++jj) { + mlxCallbackInterrupt.processInterruptCb[jj - 1] = + mlxCallbackInterrupt.processInterruptCb[jj]; + } + mlxCallbackInterrupt.numInterruptProcesses--; + return INV_SUCCESS; + } + } + return INV_ERROR_INVALID_PARAMETER; +} + +/** + * @internal + * @brief Run the recorded interrupt process callbacks in the event + * of an interrupt. + */ +void inv_run_dmp_interupt_cb(void) +{ + int kk; + for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { + if (mlxCallbackInterrupt.processInterruptCb[kk]) + mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj); + } +} + +/** @internal +* Resets the Motion/No Motion state which should be called at startup and resume. +*/ +inv_error_t inv_reset_motion(void) +{ + unsigned char regs[8]; + inv_error_t result; + + inv_obj.motion_state = INV_MOTION; + inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; + inv_obj.no_motion_accel_time = inv_get_tick_count(); + + regs[0] = DINAD8 + 2; + regs[1] = DINA0C; + regs[2] = DINAD8 + 1; + result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); + regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); + result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + memset(regs, 0, 8); + result = inv_set_mpu_memory(KEY_D_1_96, 8, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = + inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + inv_set_motion_state(INV_MOTION); + return result; +} + +/** + * @internal + * @brief inv_start_bias_calc starts the bias calculation on the MPU. + */ +void inv_start_bias_calc(void) +{ + INVENSENSE_FUNC_START; + + inv_obj.suspend = 1; +} + +/** + * @internal + * @brief inv_stop_bias_calc stops the bias calculation on the MPU. + */ +void inv_stop_bias_calc(void) +{ + INVENSENSE_FUNC_START; + + inv_obj.suspend = 0; +} + +/** + * @brief inv_update_data fetches data from the fifo and updates the + * motion algorithms. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() must have been called. + * + * @note Motion algorithm data is constant between calls to inv_update_data + * + * @return + * - INV_SUCCESS + * - Non-zero error code + */ +inv_error_t inv_update_data(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + int_fast8_t got, ftry; + uint_fast8_t mpu_interrupt; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + // Set the maximum number of FIFO packets we want to process + if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { + ftry = 100; // Large enough to process all packets + } else { + ftry = 1; + } + + // Go and process at most ftry number of packets, probably less than ftry + result = inv_read_and_process_fifo(ftry, &got); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + // Process all interrupts + mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1); + if (mpu_interrupt) { + inv_clear_interrupt_trigger(INTSRC_AUX1); + } + // Check if interrupt was from MPU + mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU); + if (mpu_interrupt) { + inv_clear_interrupt_trigger(INTSRC_MPU); + } + // Take care of the callbacks that want to be notified when there was an MPU interrupt + if (mpu_interrupt) { + inv_run_dmp_interupt_cb(); + } + + result = inv_get_fifo_status(); + return result; +} + +/** + * @brief inv_check_flag returns the value of a flag. + * inv_check_flag can be used to check a number of flags, + * allowing users to poll flags rather than register callback + * functions. If a flag is set to True when inv_check_flag is called, + * the flag is automatically reset. + * The flags are: + * - INV_RAW_DATA_READY + * Indicates that new raw data is available. + * - INV_PROCESSED_DATA_READY + * Indicates that new processed data is available. + * - INV_GOT_GESTURE + * Indicates that a gesture has been detected by the gesture engine. + * - INV_MOTION_STATE_CHANGE + * Indicates that a change has been made from motion to no motion, + * or vice versa. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() must have been called. + * + * @param flag The flag to check. + * + * @return TRUE or FALSE state of the flag + */ +int inv_check_flag(int flag) +{ + INVENSENSE_FUNC_START; + int flagReturn = inv_obj.flags[flag]; + + inv_obj.flags[flag] = 0; + return flagReturn; +} + +/** + * @brief Enable generation of the DMP interrupt when Motion or no-motion + * is detected + * @param on + * Boolean to turn the interrupt on or off. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_set_motion_interrupt(unsigned char on) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char regs[2] = { 0 }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (on) { + result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_obj.interrupt_sources |= INV_INT_MOTION; + } else { + inv_obj.interrupt_sources &= ~INV_INT_MOTION; + if (!inv_obj.interrupt_sources) { + result = inv_get_dl_cfg_int(0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + + if (on) { + regs[0] = DINAFE; + } else { + regs[0] = DINAD8; + } + result = inv_set_mpu_memory(KEY_CFG_7, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * Enable generation of the DMP interrupt when a FIFO packet is ready + * + * @param on Boolean to turn the interrupt on or off + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_set_fifo_interrupt(unsigned char on) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char regs[2] = { 0 }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (on) { + result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_obj.interrupt_sources |= INV_INT_FIFO; + } else { + inv_obj.interrupt_sources &= ~INV_INT_FIFO; + if (!inv_obj.interrupt_sources) { + result = inv_get_dl_cfg_int(0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + + if (on) { + regs[0] = DINAFE; + } else { + regs[0] = DINAD8; + } + result = inv_set_mpu_memory(KEY_CFG_6, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * @brief Get the current set of DMP interrupt sources. + * These interrupts are generated by the DMP and can be + * routed to the MPU interrupt line via internal + * settings. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @return Currently enabled interrupt sources. The possible interrupts are: + * - INV_INT_FIFO, + * - INV_INT_MOTION, + * - INV_INT_TAP + */ +int inv_get_interrupts(void) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return 0; // error + + return inv_obj.interrupt_sources; +} + +/** + * @brief Sets up the Accelerometer calibration and scale factor. + * + * Please refer to the provided "9-Axis Sensor Fusion Application + * Note" document provided. Section 5, "Sensor Mounting Orientation" + * offers a good coverage on the mounting matrices and explains how + * to use them. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * @pre inv_dmp_start() must <b>NOT</b> have been called. + * + * @see inv_set_gyro_calibration(). + * @see inv_set_compass_calibration(). + * + * @param[in] range + * The range of the accelerometers in g's. An accelerometer + * that has a range of +2g's to -2g's should pass in 2. + * @param[in] orientation + * A 9 element matrix that represents how the accelerometers + * are oriented with respect to the device they are mounted + * in and the reference axis system. + * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. + * This example corresponds to a 3 x 3 identity matrix. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_accel_calibration(float range, signed char *orientation) +{ + INVENSENSE_FUNC_START; + float cal[9]; + float scale = range / 32768.f; + int kk; + unsigned long sf; + inv_error_t result; + unsigned char regs[4] = { 0, 0, 0, 0 }; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + /* Apply zero g-offset values */ + if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) { + regs[0] = 0x80; + regs[1] = 0x0; + regs[2] = 0x80; + regs[3] = 0x0; + } + + if (inv_dmpkey_supported(KEY_D_1_152)) { + result = inv_set_mpu_memory(KEY_D_1_152, 4, ®s[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + if (scale == 0) { + inv_obj.accel_sens = 0; + } + + if (mldl_cfg->accel->id) { + unsigned char tmp[3] = { DINA4C, DINACD, DINA6C }; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + unsigned char regs[3]; + unsigned short orient; + + for (kk = 0; kk < 9; ++kk) { + cal[kk] = scale * orientation[kk]; + inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); + } + + orient = inv_orientation_matrix_to_scalar(orientation); + regs[0] = tmp[orient & 3]; + regs[1] = tmp[(orient >> 3) & 3]; + regs[2] = tmp[(orient >> 6) & 3]; + result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + regs[0] = DINA26; + regs[1] = DINA46; + regs[2] = DINA66; + + if (orient & 4) + regs[0] |= 1; + if (orient & 0x20) + regs[1] |= 1; + if (orient & 0x100) + regs[2] |= 1; + + result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) { + result = inv_freescale_sensor_fusion_16bit(orient); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) { + result = inv_freescale_sensor_fusion_8bit(orient); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + + if (inv_obj.accel_sens != 0) { + sf = (1073741824L / inv_obj.accel_sens); + } else { + sf = 0; + } + regs[0] = (unsigned char)((sf >> 8) & 0xff); + regs[1] = (unsigned char)(sf & 0xff); + result = inv_set_mpu_memory(KEY_D_0_108, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/** + * @brief Sets up the Gyro calibration and scale factor. + * + * Please refer to the provided "9-Axis Sensor Fusion Application + * Note" document provided. Section 5, "Sensor Mounting Orientation" + * offers a good coverage on the mounting matrices and explains + * how to use them. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * @pre inv_dmp_start() must have <b>NOT</b> been called. + * + * @see inv_set_accel_calibration(). + * @see inv_set_compass_calibration(). + * + * @param[in] range + * The range of the gyros in degrees per second. A gyro + * that has a range of +2000 dps to -2000 dps should pass in + * 2000. + * @param[in] orientation + * A 9 element matrix that represents how the gyro are oriented + * with respect to the device they are mounted in. A typical + * set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This + * example corresponds to a 3 x 3 identity matrix. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_gyro_calibration(float range, signed char *orientation) +{ + INVENSENSE_FUNC_START; + + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + int kk; + float scale; + inv_error_t result; + + unsigned char regs[12] = { 0 }; + unsigned char maxVal = 0; + unsigned char tmpPtr = 0; + unsigned char tmpSign = 0; + unsigned char i = 0; + int sf = 0; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (mldl_cfg->gyro_sens_trim != 0) { + /* adjust the declared range to consider the gyro_sens_trim + of this part when different from the default (first dividend) */ + range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim; + } + + scale = range / 32768.f; // inverse of sensitivity for the given full scale range + inv_obj.gyro_sens = (long)(range * 32768L); + + for (kk = 0; kk < 9; ++kk) { + inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30)); + inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30)); + } + + { + unsigned char tmpD = DINAC9; + unsigned char tmpE = DINA2C; + unsigned char tmpF = DINACB; + regs[3] = DINA36; + regs[4] = DINA56; + regs[5] = DINA76; + + for (i = 0; i < 3; i++) { + maxVal = 0; + tmpSign = 0; + if (inv_obj.gyro_orient[0 + 3 * i] < 0) + tmpSign = 1; + if (ABS(inv_obj.gyro_orient[1 + 3 * i]) > + ABS(inv_obj.gyro_orient[0 + 3 * i])) { + maxVal = 1; + if (inv_obj.gyro_orient[1 + 3 * i] < 0) + tmpSign = 1; + } + if (ABS(inv_obj.gyro_orient[2 + 3 * i]) > + ABS(inv_obj.gyro_orient[1 + 3 * i])) { + tmpSign = 0; + maxVal = 2; + if (inv_obj.gyro_orient[2 + 3 * i] < 0) + tmpSign = 1; + } + if (maxVal == 0) { + regs[tmpPtr++] = tmpD; + if (tmpSign) + regs[tmpPtr + 2] |= 0x01; + } else if (maxVal == 1) { + regs[tmpPtr++] = tmpE; + if (tmpSign) + regs[tmpPtr + 2] |= 0x01; + } else { + regs[tmpPtr++] = tmpF; + if (tmpSign) + regs[tmpPtr + 2] |= 0x01; + } + } + result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_mpu_memory(KEY_FCFG_3, 3, ®s[3]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384 + inv_obj.gyro_sf = + (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL); + result = + inv_set_mpu_memory(KEY_D_0_104, 4, + inv_int32_to_big8(inv_obj.gyro_sf, regs)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (inv_obj.gyro_sens != 0) { + sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens); + } else { + sf = 0; + } + + result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + return INV_SUCCESS; +} + +/** + * @brief Sets up the Compass calibration and scale factor. + * + * Please refer to the provided "9-Axis Sensor Fusion Application + * Note" document provided. Section 5, "Sensor Mounting Orientation" + * offers a good coverage on the mounting matrices and explains + * how to use them. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * @pre inv_dmp_start() must have <b>NOT</b> been called. + * + * @see inv_set_gyro_calibration(). + * @see inv_set_accel_calibration(). + * + * @param[in] range + * The range of the compass. + * @param[in] orientation + * A 9 element matrix that represents how the compass is + * oriented with respect to the device they are mounted in. + * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. + * This example corresponds to a 3 x 3 identity matrix. + * The matrix describes how to go from the chip mounting to + * the body of the device. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_compass_calibration(float range, signed char *orientation) +{ + INVENSENSE_FUNC_START; + float cal[9]; + float scale = range / 32768.f; + int kk; + unsigned short compassId = 0; + + compassId = inv_get_compass_id(); + + if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883) + || (compassId == COMPASS_ID_LSM303M)) { + scale /= 32.0f; + } + + for (kk = 0; kk < 9; ++kk) { + cal[kk] = scale * orientation[kk]; + inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); + } + + inv_obj.compass_sens = (long)(scale * 1073741824L); + + if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) { + unsigned char reg0[4] = { 0, 0, 0, 0 }; + unsigned char regp[4] = { 64, 0, 0, 0 }; + unsigned char regn[4] = { 64 + 128, 0, 0, 0 }; + unsigned char *reg; + int_fast8_t kk; + unsigned short keyList[9] = + { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02, + KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12, + KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22 + }; + + for (kk = 0; kk < 9; ++kk) { + + if (orientation[kk] == 1) + reg = regp; + else if (orientation[kk] == -1) + reg = regn; + else + reg = reg0; + inv_set_mpu_memory(keyList[kk], 4, reg); + } + } + + return INV_SUCCESS; +} + +/** +* @internal +* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup. +*/ +inv_error_t inv_set_dead_zone(void) +{ + unsigned char reg; + inv_error_t result; + extern struct control_params cntrl_params; + + if (cntrl_params.functions & INV_DEAD_ZONE) { + reg = 0x08; + } else { + if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { + reg = 0x2; + } else { + reg = 0; + } + } + + result = inv_set_mpu_memory(KEY_D_0_163, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * @brief inv_set_bias_update is used to register which algorithms will be + * used to automatically reset the gyroscope bias. + * The engine INV_BIAS_UPDATE must be enabled for these algorithms to + * run. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must <b>NOT</b> have been called. + * + * @param function A function or bitwise OR of functions that determine + * how the gyroscope bias will be automatically updated. + * Functions include: + * - INV_NONE or 0, + * - INV_BIAS_FROM_NO_MOTION, + * - INV_BIAS_FROM_GRAVITY, + * - INV_BIAS_FROM_TEMPERATURE, + \ifnot UMPL + * - INV_BIAS_FROM_LPF, + * - INV_MAG_BIAS_FROM_MOTION, + * - INV_MAG_BIAS_FROM_GYRO, + * - INV_ALL. + * \endif + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_bias_update(unsigned short function) +{ + INVENSENSE_FUNC_START; + unsigned char regs[4]; + long tmp[3] = { 0, 0, 0 }; + inv_error_t result = INV_SUCCESS; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + /* do not allow progressive no motion bias tracker to run - + it's not fully debugged */ + function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround + MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n"); + + // You must use EnableFastNoMotion() to get this feature + function &= ~INV_BIAS_FROM_FAST_NO_MOTION; + + // You must use DisableFastNoMotion() to turn this feature off + if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) + function |= INV_BIAS_FROM_FAST_NO_MOTION; + + /*--- remove magnetic components from bias tracking + if there is no compass ---*/ + if (!inv_compass_present()) { + function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION); + } else { + function &= ~(INV_BIAS_FROM_LPF); + } + + // Enable function sets bias from no motion + inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION); + + if (function & INV_BIAS_FROM_NO_MOTION) { + inv_enable_bias_no_motion(); + } else { + inv_disable_bias_no_motion(); + } + + if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { + regs[0] = DINA80 + 2; + regs[1] = DINA2D; + regs[2] = DINA55; + regs[3] = DINA7D; + } else { + regs[0] = DINA80 + 7; + regs[1] = DINA2D; + regs[2] = DINA35; + regs[3] = DINA3D; + } + result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_dead_zone(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) && + !inv_compass_present()) { + result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else { + result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + inv_obj.factory_temp_comp = 0; // FIXME, workaround + if ((mldl_cfg->offset_tc[0] != 0) || + (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) { + inv_obj.factory_temp_comp = 1; + } + + if (inv_obj.factory_temp_comp == 0) { + if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) { + result = inv_set_gyro_temp_slope(inv_obj.temp_slope); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else { + result = inv_set_gyro_temp_slope(tmp); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } else { + inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE; + MPL_LOGV("factory temperature compensation coefficients available - " + "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n"); + } + + /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on + compass and accel data, is to have accelerometer data delivered in the + FIFO ----*/ + if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) + && inv_compass_present()) + || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) + || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) { + inv_send_accel(INV_ALL, INV_32_BIT); + inv_send_gyro(INV_ALL, INV_32_BIT); + } + + return result; +} + +/** + * @brief inv_get_motion_state is used to determine if the device is in + * a 'motion' or 'no motion' state. + * inv_get_motion_state returns INV_MOTION of the device is moving, + * or INV_NO_MOTION if the device is not moving. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must have been called. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +int inv_get_motion_state(void) +{ + INVENSENSE_FUNC_START; + return inv_obj.motion_state; +} + +/** + * @brief inv_set_no_motion_thresh is used to set the threshold for + * detecting INV_NO_MOTION + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param thresh A threshold scaled in degrees per second. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_no_motion_thresh(float thresh) +{ + inv_error_t result = INV_SUCCESS; + unsigned char regs[4] = { 0 }; + long tmp; + INVENSENSE_FUNC_START; + + tmp = (long)(thresh * thresh * 2.045f); + if (tmp < 0) { + return INV_ERROR; + } else if (tmp > 8180000L) { + return INV_ERROR; + } + inv_obj.no_motion_threshold = tmp; + + regs[0] = (unsigned char)(tmp >> 24); + regs[1] = (unsigned char)((tmp >> 16) & 0xff); + regs[2] = (unsigned char)((tmp >> 8) & 0xff); + regs[3] = (unsigned char)(tmp & 0xff); + + result = inv_set_mpu_memory(KEY_D_1_108, 4, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_reset_motion(); + return result; +} + +/** + * @brief inv_set_no_motion_threshAccel is used to set the threshold for + * detecting INV_NO_MOTION with accelerometers when Gyros have + * been turned off + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param thresh A threshold in g's scaled by 2^32 + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_no_motion_threshAccel(long thresh) +{ + INVENSENSE_FUNC_START; + + inv_obj.no_motion_accel_threshold = thresh; + + return INV_SUCCESS; +} + +/** + * @brief inv_set_no_motion_time is used to set the time required for + * detecting INV_NO_MOTION + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param time A time in seconds. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_no_motion_time(float time) +{ + inv_error_t result = INV_SUCCESS; + unsigned char regs[2] = { 0 }; + long tmp; + + INVENSENSE_FUNC_START; + + tmp = (long)(time * 200); + if (tmp < 0) { + return INV_ERROR; + } else if (tmp > 65535L) { + return INV_ERROR; + } + inv_obj.motion_duration = (unsigned short)tmp; + + regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); + regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); + result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_reset_motion(); + return result; +} + +/** + * @brief inv_get_version is used to get the ML version. + * + * @pre inv_get_version can be called at any time. + * + * @param version inv_get_version writes the ML version + * string pointer to version. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_get_version(unsigned char **version) +{ + INVENSENSE_FUNC_START; + + *version = (unsigned char *)mlVer; //fixme we are wiping const + + return INV_SUCCESS; +} + +/** + * @brief Check for the presence of the gyro sensor. + * + * This is not a physical check but a logical check and the value can change + * dynamically based on calls to inv_set_mpu_sensors(). + * + * @return TRUE if the gyro is enabled FALSE otherwise. + */ +int inv_get_gyro_present(void) +{ + return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO | + INV_Z_GYRO); +} + +static unsigned short inv_row_2_scale(const signed char *row) +{ + unsigned short b; + + if (row[0] > 0) + b = 0; + else if (row[0] < 0) + b = 4; + else if (row[1] > 0) + b = 1; + else if (row[1] < 0) + b = 5; + else if (row[2] > 0) + b = 2; + else if (row[2] < 0) + b = 6; + else + b = 7; // error + return b; +} + +unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) +{ + unsigned short scalar; + /* + XYZ 010_001_000 Identity Matrix + XZY 001_010_000 + YXZ 010_000_001 + YZX 000_010_001 + ZXY 001_000_010 + ZYX 000_001_010 + */ + + scalar = inv_row_2_scale(mtx); + scalar |= inv_row_2_scale(mtx + 3) << 3; + scalar |= inv_row_2_scale(mtx + 6) << 6; + + return scalar; +} + +/* Setups up the Freescale 16-bit accel for Sensor Fusion +* @param[in] orient A scalar representation of the orientation +* that describes how to go from the Chip Orientation +* to the Board Orientation often times called the Body Orientation in Invensense Documentation. +* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. +*/ +inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient) +{ + unsigned char rr[3]; + inv_error_t result; + + orient = orient & 0xdb; + switch (orient) { + default: + // Typically 0x88 + rr[0] = DINACC; + rr[1] = DINACF; + rr[2] = DINA0E; + break; + case 0x50: + rr[0] = DINACE; + rr[1] = DINA0E; + rr[2] = DINACD; + break; + case 0x81: + rr[0] = DINACE; + rr[1] = DINACB; + rr[2] = DINA0E; + break; + case 0x11: + rr[0] = DINACC; + rr[1] = DINA0E; + rr[2] = DINACB; + break; + case 0x42: + rr[0] = DINA0A; + rr[1] = DINACF; + rr[2] = DINACB; + break; + case 0x0a: + rr[0] = DINA0A; + rr[1] = DINACB; + rr[2] = DINACD; + break; + } + result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr); + return result; +} + +/* Setups up the Freescale 8-bit accel for Sensor Fusion +* @param[in] orient A scalar representation of the orientation +* that describes how to go from the Chip Orientation +* to the Board Orientation often times called the Body Orientation in Invensense Documentation. +* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. +*/ +inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient) +{ + unsigned char regs[27]; + inv_error_t result; + uint_fast8_t kk; + + orient = orient & 0xdb; + kk = 0; + + regs[kk++] = DINAC3; + regs[kk++] = DINA90 + 14; + regs[kk++] = DINAA0 + 9; + regs[kk++] = DINA3E; + regs[kk++] = DINA5E; + regs[kk++] = DINA7E; + + regs[kk++] = DINAC2; + regs[kk++] = DINAA0 + 9; + regs[kk++] = DINA90 + 9; + regs[kk++] = DINAF8 + 2; + + switch (orient) { + default: + // Typically 0x88 + regs[kk++] = DINACB; + + regs[kk++] = DINA54; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + + regs[kk++] = DINACD; + break; + case 0x50: + regs[kk++] = DINACB; + + regs[kk++] = DINACF; + + regs[kk++] = DINA7C; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + break; + case 0x81: + regs[kk++] = DINA2C; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + + regs[kk++] = DINACD; + + regs[kk++] = DINACB; + break; + case 0x11: + regs[kk++] = DINA2C; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINACB; + regs[kk++] = DINACF; + break; + case 0x42: + regs[kk++] = DINACF; + regs[kk++] = DINACD; + + regs[kk++] = DINA7C; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + break; + case 0x0a: + regs[kk++] = DINACD; + + regs[kk++] = DINA54; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + + regs[kk++] = DINACF; + break; + } + + regs[kk++] = DINA90 + 7; + regs[kk++] = DINAF8 + 3; + regs[kk++] = DINAA0 + 9; + regs[kk++] = DINA0E; + regs[kk++] = DINA0E; + regs[kk++] = DINA0E; + + regs[kk++] = DINAF8 + 1; // filler + + result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/** + * Controlls each sensor and each axis when the motion processing unit is + * running. When it is not running, simply records the state for later. + * + * NOTE: In this version only full sensors controll is allowed. Independent + * axis control will return an error. + * + * @param sensors Bit field of each axis desired to be turned on or off + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_set_mpu_sensors(unsigned long sensors) +{ + INVENSENSE_FUNC_START; + unsigned char state = inv_get_state(); + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + inv_error_t result = INV_SUCCESS; + unsigned short fifoRate; + + if (state < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) && + ((sensors & INV_THREE_AXIS_ACCEL) != 0)) { + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + if (((sensors & INV_THREE_AXIS_ACCEL) != 0) && + (mldl_cfg->pdata->accel.get_slave_descr == 0)) { + return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; + } + + if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) && + ((sensors & INV_THREE_AXIS_COMPASS) != 0)) { + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + if (((sensors & INV_THREE_AXIS_COMPASS) != 0) && + (mldl_cfg->pdata->compass.get_slave_descr == 0)) { + return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; + } + + if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) && + ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) { + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) && + (mldl_cfg->pdata->pressure.get_slave_descr == 0)) { + return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; + } + + /* DMP was off, and is turning on */ + if (sensors & INV_DMP_PROCESSOR && + !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { + struct ext_slave_config config; + long odr; + config.key = MPU_SLAVE_CONFIG_ODR_RESUME; + config.len = sizeof(long); + config.apply = (state == INV_STATE_DMP_STARTED); + config.data = &odr; + + odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000); + result = inv_mpu_config_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; + odr = MPU_SLAVE_IRQ_TYPE_NONE; + result = inv_mpu_config_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_init_fifo_hardare(); + } + + if (inv_obj.mode_change_func) { + result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + /* Get the fifo rate before changing sensors so if we need to match it */ + fifoRate = inv_get_fifo_rate(); + mldl_cfg->requested_sensors = sensors; + + /* inv_dmp_start will turn the sensors on */ + if (state == INV_STATE_DMP_STARTED) { + result = inv_dl_start(sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_reset_motion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_dl_stop(~sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + inv_set_fifo_rate(fifoRate); + + if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) { + struct ext_slave_config config; + long data; + + config.len = sizeof(long); + config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; + config.apply = (state == INV_STATE_DMP_STARTED); + config.data = &data; + data = MPU_SLAVE_IRQ_TYPE_DATA_READY; + result = inv_mpu_config_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + return result; +} + +void inv_set_mode_change(inv_error_t(*mode_change_func) + (unsigned long, unsigned long)) +{ + inv_obj.mode_change_func = mode_change_func; +} + +/** + * @} + */ |