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 */ 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 */ 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 */ 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 */ 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
