1/*
2 $License:
3   Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16  $
17 */
18
19/******************************************************************************
20 *
21 * $Id: mldmp.c 5629 2011-06-11 03:13:08Z mcaramello $
22 *
23 *****************************************************************************/
24
25/**
26 * @addtogroup MLDMP
27 *
28 * @{
29 *      @file     mldmp.c
30 *      @brief    Shared functions between all the different DMP versions
31**/
32
33#include <stdio.h>
34
35#include "mltypes.h"
36#include "mlinclude.h"
37#include "mltypes.h"
38#include "ml.h"
39#include "mldl_cfg.h"
40#include "mldl.h"
41#include "compass.h"
42#include "mlSetGyroBias.h"
43#include "mlsl.h"
44#include "mlFIFO.h"
45#include "mldmp.h"
46#include "mlstates.h"
47#include "dmpDefault.h"
48#include "mlFIFOHW.h"
49#include "mlsupervisor.h"
50
51#include "log.h"
52#undef MPL_LOG_TAG
53#define MPL_LOG_TAG "MPL-dmp"
54
55/**
56 *  @brief  Open the default motion sensor engine.
57 *          This function is used to open the default MPL engine,
58 *          featuring, for example, sensor fusion (6 axes and 9 axes),
59 *          sensor calibration, accelerometer data byte swapping, among
60 *          others.
61 *          Compare with the other provided engines.
62 *
63 *  @pre    inv_serial_start() must have been called to instantiate the serial
64 *          communication.
65 *
66 *  Example:
67 *  @code
68 *    result = inv_dmp_open( );
69 *    if (INV_SUCCESS != result) {
70 *        // Handle the error case
71 *    }
72 *  @endcode
73 *
74 *  @return Zero on success; Error Code on any failure.
75 *
76 */
77inv_error_t inv_dmp_open(void)
78{
79    INVENSENSE_FUNC_START;
80    inv_error_t result;
81    unsigned char state = inv_get_state();
82    struct mldl_cfg *mldl_cfg;
83    unsigned long requested_sensors;
84
85    /*************************************************************
86     * Common operations before calling DMPOpen
87     ************************************************************/
88    if (state == INV_STATE_DMP_OPENED)
89        return INV_SUCCESS;
90
91    if (state == INV_STATE_DMP_STARTED) {
92        return inv_dmp_stop();
93    }
94
95    result = inv_state_transition(INV_STATE_DMP_OPENED);
96    if (result) {
97        LOG_RESULT_LOCATION(result);
98        return result;
99    }
100
101    result = inv_dl_open(inv_get_serial_handle());
102    if (result) {
103        LOG_RESULT_LOCATION(result);
104        return result;
105    }
106#ifdef ML_USE_DMP_SIM
107    do {
108        void setup_univ();
109        setup_univ();           /* hijack the read and write paths
110                                   and re-direct them to the simulator */
111    } while (0);
112#endif
113
114    result = inv_setup_dmp();
115    if (result) {
116        LOG_RESULT_LOCATION(result);
117        return result;
118    }
119
120    // Init vars.
121    inv_init_ml();
122
123    result = inv_init_fifo_param();
124    if (result) {
125        LOG_RESULT_LOCATION(result);
126        return result;
127    }
128    result = inv_enable_set_bias();
129    if (result) {
130        LOG_RESULT_LOCATION(result);
131        return result;
132    }
133    inv_init_fifo_hardare();
134    mldl_cfg = inv_get_dl_config();
135    requested_sensors = INV_THREE_AXIS_GYRO;
136    if (mldl_cfg->accel && mldl_cfg->accel->resume)
137        requested_sensors |= INV_THREE_AXIS_ACCEL;
138
139    if (mldl_cfg->compass && mldl_cfg->compass->resume)
140        requested_sensors |= INV_THREE_AXIS_COMPASS;
141
142    if (mldl_cfg->pressure && mldl_cfg->pressure->resume)
143        requested_sensors |= INV_THREE_AXIS_PRESSURE;
144
145    result = inv_init_requested_sensors(requested_sensors);
146    if (result) {
147        LOG_RESULT_LOCATION(result);
148        return result;
149    }
150    result = inv_apply_calibration();
151    if (result) {
152        LOG_RESULT_LOCATION(result);
153        return result;
154    }
155    if (NULL != mldl_cfg->accel){
156        result = inv_apply_endian_accel();
157    }
158
159    return result;
160}
161
162/**
163 *  @brief  Start the DMP.
164 *
165 *  @pre    inv_dmp_open() must have been called.
166 *
167 *  @code
168 *     result = inv_dmp_start();
169 *     if (INV_SUCCESS != result) {
170 *         // Handle the error case
171 *     }
172 *  @endcode
173 *
174 *  @return INV_SUCCESS if successful, or Non-zero error code otherwise.
175 */
176inv_error_t inv_dmp_start(void)
177{
178    INVENSENSE_FUNC_START;
179    inv_error_t result;
180
181    if (inv_get_state() == INV_STATE_DMP_STARTED)
182        return INV_SUCCESS;
183
184    result = inv_state_transition(INV_STATE_DMP_STARTED);
185    if (result) {
186        LOG_RESULT_LOCATION(result);
187        return result;
188    }
189    inv_init_sensor_fusion_supervisor();
190    result = inv_dl_start(inv_get_dl_config()->requested_sensors);
191    if (result) {
192        LOG_RESULT_LOCATION(result);
193        return result;
194    }
195    /* This is done after the start since it will modify DMP memory, which
196     * will cause a full reset is most cases */
197    result = inv_reset_motion();
198    if (result) {
199        LOG_RESULT_LOCATION(result);
200        return result;
201    }
202
203    return result;
204}
205
206/**
207 *  @brief  Stops the DMP and puts it in low power.
208 *
209 *  @pre    inv_dmp_start() must have been called.
210 *
211 *  @return INV_SUCCESS, Non-zero error code otherwise.
212 */
213inv_error_t inv_dmp_stop(void)
214{
215    INVENSENSE_FUNC_START;
216    inv_error_t result;
217
218    if (inv_get_state() == INV_STATE_DMP_OPENED)
219        return INV_SUCCESS;
220
221    result = inv_state_transition(INV_STATE_DMP_OPENED);
222    if (result) {
223        LOG_RESULT_LOCATION(result);
224        return result;
225    }
226    result = inv_dl_stop(INV_ALL_SENSORS);
227    if (result) {
228        LOG_RESULT_LOCATION(result);
229        return result;
230    }
231
232    return result;
233}
234
235/**
236 *  @brief  Closes the motion sensor engine.
237 *          Does not close the serial communication. To do that,
238 *          call inv_serial_stop().
239 *          After calling inv_dmp_close() another DMP module can be
240 *          loaded in the MPL with the corresponding necessary
241 *          intialization and configurations, via any of the
242 *          MLDmpXXXOpen functions.
243 *
244 *  @pre    inv_dmp_open() must have been called.
245 *
246 *  @code
247 *     result = inv_dmp_close();
248 *     if (INV_SUCCESS != result) {
249 *         // Handle the error case
250 *     }
251 *  @endcode
252 *
253 *  @return INV_SUCCESS, Non-zero error code otherwise.
254 */
255inv_error_t inv_dmp_close(void)
256{
257    INVENSENSE_FUNC_START;
258    inv_error_t result;
259    inv_error_t firstError = INV_SUCCESS;
260
261    if (inv_get_state() <= INV_STATE_DMP_CLOSED)
262        return INV_SUCCESS;
263
264    result = inv_disable_set_bias();
265    ERROR_CHECK_FIRST(firstError, result);
266
267    result = inv_dl_stop(INV_ALL_SENSORS);
268    ERROR_CHECK_FIRST(firstError, result);
269
270    result = inv_close_fifo();
271    ERROR_CHECK_FIRST(firstError, result);
272
273    result = inv_dl_close();
274    ERROR_CHECK_FIRST(firstError, result);
275
276    result = inv_state_transition(INV_STATE_SERIAL_OPENED);
277    ERROR_CHECK_FIRST(firstError, result);
278
279    return result;
280}
281
282/**
283 *  @}
284 */
285