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 */
inv_dmp_open(void)77 inv_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 */
inv_dmp_start(void)176 inv_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 */
inv_dmp_stop(void)213 inv_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 */
inv_dmp_close(void)255 inv_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