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 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 */ 174 inv_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 */ 211 inv_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 */ 253 inv_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
