fuquan/android_mt6572_jiabo
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions
android_mt6572_jiabo/hardware/invensense/60xx/mlsdk/mllite/ml.c
FuQuan 3ff2876a30 first commit
2025-09-05 16:56:03 +08:00

1885 lines
55 KiB
C
Raw Permalink Blame History

/*
$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 "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) */
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
defined CONFIG_MPU_SENSORS_MPU6050B1
inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim;
#else
inv_obj.accel_sens /= 2;
#endif
}
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();
#if defined CONFIG_MPU_SENSORS_MPU3050
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;
}
#else
#endif
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, &regs[0]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
if (scale == 0) {
inv_obj.accel_sens = 0;
}
if (mldl_cfg->accel->id) {
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
defined CONFIG_MPU_SENSORS_MPU6050B1
unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C };
#else
unsigned char tmp[3] = { DINA4C, DINACD, DINA6C };
#endif
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;
#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2
regs[2] = DINA66;
#else
if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision)
== MPU_PRODUCT_KEY_B1_E1_5)
regs[2] = DINA76;
else
regs[2] = DINA66;
#endif
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));
}
{
#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
defined CONFIG_MPU_SENSORS_MPU6050B1
unsigned char tmpD = DINA4C;
unsigned char tmpE = DINACD;
unsigned char tmpF = DINA6C;
#else
unsigned char tmpD = DINAC9;
unsigned char tmpE = DINA2C;
unsigned char tmpF = DINACB;
#endif
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, &regs[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 defined CONFIG_MPU_SENSORS_MPU6050A2 || \
defined CONFIG_MPU_SENSORS_MPU6050B1
reg = 0;
#else
if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
reg = 0x2;
} else {
reg = 0;
}
#endif
}
result = inv_set_mpu_memory(KEY_D_0_163, 1, &reg);
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;
}
/**
* Mantis setup
*/
#ifdef CONFIG_MPU_SENSORS_MPU6050B1
inv_error_t inv_set_mpu_6050_config()
{
long temp;
inv_error_t result;
unsigned char big8[4];
unsigned char atc[4];
long s[3], s2[3];
int kk;
struct mldl_cfg *mldlCfg = inv_get_dl_config();
result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
0x0d, 4, atc);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
temp = atc[3] & 0x3f;
if (temp >= 32)
temp = temp - 64;
temp = (temp << 21) | 0x100000;
temp += (1L << 29);
temp = -temp;
result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
for (kk = 0; kk < 3; ++kk) {
s[kk] = atc[kk] & 0x3f;
if (s[kk] > 32)
s[kk] = s[kk] - 64;
s[kk] *= 2516582L;
}
for (kk = 0; kk < 3; ++kk) {
s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] +
mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] +
mldlCfg->pdata->orientation[kk * 3 + 2] * s[2];
}
result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8));
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
#endif
/**
* @}
*/
Reference in a new issue View git blame Copy permalink