1/*
2 $License:
3   Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16  $
17 */
18/*******************************************************************************
19 *
20 * $Id: accel.c 4595 2011-01-25 01:43:03Z mcaramello $
21 *
22 *******************************************************************************/
23
24/**
25 *  @defgroup ACCELDL
26 *  @brief  Motion Library - Accel Driver Layer.
27 *          Provides the interface to setup and handle an accel
28 *          connected to either the primary or the seconday I2C interface
29 *          of the gyroscope.
30 *
31 *  @{
32 *      @file   accel.c
33 *      @brief  Accel setup and handling methods.
34**/
35
36/* ------------------ */
37/* - Include Files. - */
38/* ------------------ */
39
40#include <string.h>
41
42#include "ml.h"
43#include "mlinclude.h"
44#include "dmpKey.h"
45#include "mlFIFO.h"
46#include "mldl.h"
47#include "mldl_cfg.h"
48#include "mlMathFunc.h"
49#include "mlsl.h"
50#include "mlos.h"
51
52#include "log.h"
53#undef MPL_LOG_TAG
54#define MPL_LOG_TAG "MPL-accel"
55
56#define ACCEL_DEBUG 0
57
58/* --------------------- */
59/* - Global Variables. - */
60/* --------------------- */
61
62/* --------------------- */
63/* - Static Variables. - */
64/* --------------------- */
65
66/* --------------- */
67/* - Prototypes. - */
68/* --------------- */
69
70/* -------------- */
71/* - Externs.   - */
72/* -------------- */
73
74/* -------------- */
75/* - Functions. - */
76/* -------------- */
77
78/**
79 *  @brief  Used to determine if an accel is configured and
80 *          used by the MPL.
81 *  @return INV_SUCCESS if the accel is present.
82 */
83unsigned char inv_accel_present(void)
84{
85    INVENSENSE_FUNC_START;
86    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
87    if (NULL != mldl_cfg->accel &&
88        NULL != mldl_cfg->accel->resume &&
89        mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL)
90        return TRUE;
91    else
92        return FALSE;
93}
94
95/**
96 *  @brief   Query the accel slave address.
97 *  @return  The 7-bit accel slave address.
98 */
99unsigned char inv_get_slave_addr(void)
100{
101    INVENSENSE_FUNC_START;
102    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
103    if (NULL != mldl_cfg->pdata)
104        return mldl_cfg->pdata->accel.address;
105    else
106        return 0;
107}
108
109/**
110 *  @brief   Get the ID of the accel in use.
111 *  @return  ID of the accel in use.
112 */
113unsigned short inv_get_accel_id(void)
114{
115    INVENSENSE_FUNC_START;
116    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
117    if (NULL != mldl_cfg->accel) {
118        return mldl_cfg->accel->id;
119    }
120    return ID_INVALID;
121}
122
123/**
124 *  @brief  Get a sample of accel data from the device.
125 *  @param  data
126 *              the buffer to store the accel raw data for
127 *              X, Y, and Z axes.
128 *  @return INV_SUCCESS or a non-zero error code.
129 */
130inv_error_t inv_get_accel_data(long *data)
131{
132    struct mldl_cfg *mldl_cfg = inv_get_dl_config();
133    inv_error_t result;
134    unsigned char raw_data[2 * ACCEL_NUM_AXES];
135    long tmp[ACCEL_NUM_AXES];
136    int ii;
137    signed char *mtx = mldl_cfg->pdata->accel.orientation;
138    char accelId = mldl_cfg->accel->id;
139
140    if (NULL == data)
141        return INV_ERROR_INVALID_PARAMETER;
142
143    if (mldl_cfg->accel->read_len > sizeof(raw_data))
144        return INV_ERROR_ASSERTION_FAILURE;
145
146    result = (inv_error_t) inv_mpu_read_accel(mldl_cfg,
147                                              inv_get_serial_handle(),
148                                              inv_get_serial_handle(),
149                                              raw_data);
150    if (result == INV_ERROR_ACCEL_DATA_NOT_READY) {
151        return result;
152    }
153    if (result) {
154        LOG_RESULT_LOCATION(result);
155        return result;
156    }
157
158    for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
159        if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) {
160            tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256;
161            tmp[ii] += (long)((unsigned char)raw_data[2 * ii]);
162        } else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) ||
163                   (EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) {
164            tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256;
165            tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]);
166            if (accelId == ACCEL_ID_KXSD9) {
167                tmp[ii] = (long)((short)(((unsigned short)tmp[ii])
168                                         + ((unsigned short)0x8000)));
169            }
170        } else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) {
171            tmp[ii] = (long)((signed char)raw_data[ii]) * 256;
172        } else {
173            result = INV_ERROR_FEATURE_NOT_IMPLEMENTED;
174        }
175    }
176
177    for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) {
178        data[ii] = ((long)tmp[0] * mtx[3 * ii] +
179                    (long)tmp[1] * mtx[3 * ii + 1] +
180                    (long)tmp[2] * mtx[3 * ii + 2]);
181    }
182
183    //MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]);
184    return result;
185}
186
187/**
188 *  @}
189 */
190