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    result = inv_apply_endian_accel();
156
157    return result;
158}
159
160/**
161 *  @brief  Start the DMP.
162 *
163 *  @pre    inv_dmp_open() must have been called.
164 *
165 *  @code
166 *     result = inv_dmp_start();
167 *     if (INV_SUCCESS != result) {
168 *         // Handle the error case
169 *     }
170 *  @endcode
171 *
172 *  @return INV_SUCCESS if successful, or Non-zero error code otherwise.
173 */
174inv_error_t inv_dmp_start(void)
175{
176    INVENSENSE_FUNC_START;
177    inv_error_t result;
178
179    if (inv_get_state() == INV_STATE_DMP_STARTED)
180        return INV_SUCCESS;
181
182    result = inv_state_transition(INV_STATE_DMP_STARTED);
183    if (result) {
184        LOG_RESULT_LOCATION(result);
185        return result;
186    }
187    inv_init_sensor_fusion_supervisor();
188    result = inv_dl_start(inv_get_dl_config()->requested_sensors);
189    if (result) {
190        LOG_RESULT_LOCATION(result);
191        return result;
192    }
193    /* This is done after the start since it will modify DMP memory, which
194     * will cause a full reset is most cases */
195    result = inv_reset_motion();
196    if (result) {
197        LOG_RESULT_LOCATION(result);
198        return result;
199    }
200
201    return result;
202}
203
204/**
205 *  @brief  Stops the DMP and puts it in low power.
206 *
207 *  @pre    inv_dmp_start() must have been called.
208 *
209 *  @return INV_SUCCESS, Non-zero error code otherwise.
210 */
211inv_error_t inv_dmp_stop(void)
212{
213    INVENSENSE_FUNC_START;
214    inv_error_t result;
215
216    if (inv_get_state() == INV_STATE_DMP_OPENED)
217        return INV_SUCCESS;
218
219    result = inv_state_transition(INV_STATE_DMP_OPENED);
220    if (result) {
221        LOG_RESULT_LOCATION(result);
222        return result;
223    }
224    result = inv_dl_stop(INV_ALL_SENSORS);
225    if (result) {
226        LOG_RESULT_LOCATION(result);
227        return result;
228    }
229
230    return result;
231}
232
233/**
234 *  @brief  Closes the motion sensor engine.
235 *          Does not close the serial communication. To do that,
236 *          call inv_serial_stop().
237 *          After calling inv_dmp_close() another DMP module can be
238 *          loaded in the MPL with the corresponding necessary
239 *          intialization and configurations, via any of the
240 *          MLDmpXXXOpen functions.
241 *
242 *  @pre    inv_dmp_open() must have been called.
243 *
244 *  @code
245 *     result = inv_dmp_close();
246 *     if (INV_SUCCESS != result) {
247 *         // Handle the error case
248 *     }
249 *  @endcode
250 *
251 *  @return INV_SUCCESS, Non-zero error code otherwise.
252 */
253inv_error_t inv_dmp_close(void)
254{
255    INVENSENSE_FUNC_START;
256    inv_error_t result;
257    inv_error_t firstError = INV_SUCCESS;
258
259    if (inv_get_state() <= INV_STATE_DMP_CLOSED)
260        return INV_SUCCESS;
261
262    result = inv_disable_set_bias();
263    ERROR_CHECK_FIRST(firstError, result);
264
265    result = inv_dl_stop(INV_ALL_SENSORS);
266    ERROR_CHECK_FIRST(firstError, result);
267
268    result = inv_close_fifo();
269    ERROR_CHECK_FIRST(firstError, result);
270
271    result = inv_dl_close();
272    ERROR_CHECK_FIRST(firstError, result);
273
274    result = inv_state_transition(INV_STATE_SERIAL_OPENED);
275    ERROR_CHECK_FIRST(firstError, result);
276
277    return result;
278}
279
280/**
281 *  @}
282 */
283