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 * $Id: ml.c 5642 2011-06-14 02:26:20Z mcaramello $
21 *
22 *****************************************************************************/
23
24 /**
25 * @defgroup ML
26 * @brief Motion Library APIs.
27 * The Motion Library processes gyroscopes, accelerometers, and
28 * compasses to provide a physical model of the movement for the
29 * sensors.
30 * The results of this processing may be used to control objects
31 * within a user interface environment, detect gestures, track 3D
32 * movement for gaming applications, and analyze the blur created
33 * due to hand movement while taking a picture.
34 *
35 * @{
36 * @file ml.c
37 * @brief The Motion Library APIs.
38 */
39
40 /* ------------------ */
41 /* - Include Files. - */
42 /* ------------------ */
43
44 #include <string.h>
45
46 #include "ml.h"
47 #include "mldl.h"
48 #include "mltypes.h"
49 #include "mlinclude.h"
50 #include "compass.h"
51 #include "dmpKey.h"
52 #include "dmpDefault.h"
53 #include "mlstates.h"
54 #include "mlFIFO.h"
55 #include "mlFIFOHW.h"
56 #include "mlMathFunc.h"
57 #include "mlsupervisor.h"
58 #include "mlmath.h"
59 #include "mlcontrol.h"
60 #include "mldl_cfg.h"
61 #include "mpu.h"
62 #include "accel.h"
63 #include "mlos.h"
64 #include "mlsl.h"
65 #include "mlos.h"
66 #include "mlBiasNoMotion.h"
67 #include "log.h"
68 #undef MPL_LOG_TAG
69 #define MPL_LOG_TAG "MPL-ml"
70
71 #define ML_MOT_TYPE_NONE 0
72 #define ML_MOT_TYPE_NO_MOTION 1
73 #define ML_MOT_TYPE_MOTION_DETECTED 2
74
75 #define ML_MOT_STATE_MOVING 0
76 #define ML_MOT_STATE_NO_MOTION 1
77 #define ML_MOT_STATE_BIAS_IN_PROG 2
78
79 #define _mlDebug(x) //{x}
80
81 /* Global Variables */
82 const unsigned char mlVer[] = { INV_VERSION };
83
84 struct inv_params_obj inv_params_obj = {
85 INV_BIAS_UPDATE_FUNC_DEFAULT, // bias_mode
86 INV_ORIENTATION_MASK_DEFAULT, // orientation_mask
87 INV_PROCESSED_DATA_CALLBACK_DEFAULT, // fifo_processed_func
88 INV_ORIENTATION_CALLBACK_DEFAULT, // orientation_cb_func
89 INV_MOTION_CALLBACK_DEFAULT, // motion_cb_func
90 INV_STATE_SERIAL_CLOSED // starting state
91 };
92
93 struct inv_obj_t inv_obj;
94 void *g_mlsl_handle;
95
96 typedef struct {
97 // These describe callbacks happening everythime a DMP interrupt is processed
98 int_fast8_t numInterruptProcesses;
99 // Array of function pointers, function being void function taking void
100 inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES];
101
102 } tMLXCallbackInterrupt; // MLX_callback_t
103
104 tMLXCallbackInterrupt mlxCallbackInterrupt;
105
106 /* --------------- */
107 /* - Functions. - */
108 /* --------------- */
109
110 inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient);
111 inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient);
112 unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx);
113
114 /**
115 * @brief Open serial connection with the MPU device.
116 * This is the entry point of the MPL and must be
117 * called prior to any other function call.
118 *
119 * @param port System handle for 'port' MPU device is found on.
120 * The significance of this parameter varies by
121 * platform. It is passed as 'port' to MLSLSerialOpen.
122 *
123 * @return INV_SUCCESS or error code.
124 */
inv_serial_start(char const * port)125 inv_error_t inv_serial_start(char const *port)
126 {
127 INVENSENSE_FUNC_START;
128 inv_error_t result;
129
130 if (inv_get_state() >= INV_STATE_SERIAL_OPENED)
131 return INV_SUCCESS;
132
133 result = inv_state_transition(INV_STATE_SERIAL_OPENED);
134 if (result) {
135 LOG_RESULT_LOCATION(result);
136 return result;
137 }
138
139 result = inv_serial_open(port, &g_mlsl_handle);
140 if (INV_SUCCESS != result) {
141 (void)inv_state_transition(INV_STATE_SERIAL_CLOSED);
142 }
143
144 return result;
145 }
146
147 /**
148 * @brief Close the serial communication.
149 * This function needs to be called explicitly to shut down
150 * the communication with the device. Calling inv_dmp_close()
151 * won't affect the established serial communication.
152 * @return INV_SUCCESS; non-zero error code otherwise.
153 */
inv_serial_stop(void)154 inv_error_t inv_serial_stop(void)
155 {
156 INVENSENSE_FUNC_START;
157 inv_error_t result = INV_SUCCESS;
158
159 if (inv_get_state() == INV_STATE_SERIAL_CLOSED)
160 return INV_SUCCESS;
161
162 result = inv_state_transition(INV_STATE_SERIAL_CLOSED);
163 if (INV_SUCCESS != result) {
164 MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result);
165 }
166 result = inv_serial_close(g_mlsl_handle);
167 if (INV_SUCCESS != result) {
168 MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result);
169 }
170 return result;
171 }
172
173 /**
174 * @brief Get the serial file handle to the device.
175 * @return The serial file handle.
176 */
inv_get_serial_handle(void)177 void *inv_get_serial_handle(void)
178 {
179 INVENSENSE_FUNC_START;
180 return g_mlsl_handle;
181 }
182
183 /**
184 * @brief apply the choosen orientation and full scale range
185 * for gyroscopes, accelerometer, and compass.
186 * @return INV_SUCCESS if successful, a non-zero code otherwise.
187 */
inv_apply_calibration(void)188 inv_error_t inv_apply_calibration(void)
189 {
190 INVENSENSE_FUNC_START;
191 signed char gyroCal[9] = { 0 };
192 signed char accelCal[9] = { 0 };
193 signed char magCal[9] = { 0 };
194 float gyroScale = 2000.f;
195 float accelScale = 0.f;
196 float magScale = 0.f;
197
198 inv_error_t result;
199 int ii;
200 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
201
202 for (ii = 0; ii < 9; ii++) {
203 gyroCal[ii] = mldl_cfg->pdata->orientation[ii];
204 accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii];
205 magCal[ii] = mldl_cfg->pdata->compass.orientation[ii];
206 }
207
208 switch (mldl_cfg->full_scale) {
209 case MPU_FS_250DPS:
210 gyroScale = 250.f;
211 break;
212 case MPU_FS_500DPS:
213 gyroScale = 500.f;
214 break;
215 case MPU_FS_1000DPS:
216 gyroScale = 1000.f;
217 break;
218 case MPU_FS_2000DPS:
219 gyroScale = 2000.f;
220 break;
221 default:
222 MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n",
223 mldl_cfg->full_scale);
224 return INV_ERROR_INVALID_PARAMETER;
225 break;
226 }
227
228 RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale);
229 inv_obj.accel_sens = (long)(accelScale * 65536L);
230 /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */
231 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
232 defined CONFIG_MPU_SENSORS_MPU6050B1
233 inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim;
234 #else
235 inv_obj.accel_sens /= 2;
236 #endif
237
238 RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale);
239 inv_obj.compass_sens = (long)(magScale * 32768);
240
241 if (inv_get_state() == INV_STATE_DMP_OPENED) {
242 result = inv_set_gyro_calibration(gyroScale, gyroCal);
243 if (INV_SUCCESS != result) {
244 MPL_LOGE("Unable to set Gyro Calibration\n");
245 return result;
246 }
247 result = inv_set_accel_calibration(accelScale, accelCal);
248 if (INV_SUCCESS != result) {
249 MPL_LOGE("Unable to set Accel Calibration\n");
250 return result;
251 }
252 result = inv_set_compass_calibration(magScale, magCal);
253 if (INV_SUCCESS != result) {
254 MPL_LOGE("Unable to set Mag Calibration\n");
255 return result;
256 }
257 }
258 return INV_SUCCESS;
259 }
260
261 /**
262 * @brief Setup the DMP to handle the accelerometer endianess.
263 * @return INV_SUCCESS if successful, a non-zero error code otherwise.
264 */
inv_apply_endian_accel(void)265 inv_error_t inv_apply_endian_accel(void)
266 {
267 INVENSENSE_FUNC_START;
268 unsigned char regs[4] = { 0 };
269 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
270 int endian = mldl_cfg->accel->endian;
271
272 if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) {
273 endian = EXT_SLAVE_BIG_ENDIAN;
274 }
275 switch (endian) {
276 case EXT_SLAVE_FS8_BIG_ENDIAN:
277 case EXT_SLAVE_FS16_BIG_ENDIAN:
278 case EXT_SLAVE_LITTLE_ENDIAN:
279 regs[0] = 0;
280 regs[1] = 64;
281 regs[2] = 0;
282 regs[3] = 0;
283 break;
284 case EXT_SLAVE_BIG_ENDIAN:
285 default:
286 regs[0] = 0;
287 regs[1] = 0;
288 regs[2] = 64;
289 regs[3] = 0;
290 }
291
292 return inv_set_mpu_memory(KEY_D_1_236, 4, regs);
293 }
294
295 /**
296 * @internal
297 * @brief Initialize MLX data. This should be called to setup the mlx
298 * output buffers before any motion processing is done.
299 */
inv_init_ml(void)300 void inv_init_ml(void)
301 {
302 INVENSENSE_FUNC_START;
303 int ii;
304 long tmp[COMPASS_NUM_AXES];
305 // Set all values to zero by default
306 memset(&inv_obj, 0, sizeof(struct inv_obj_t));
307 memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt));
308
309 inv_obj.compass_correction[0] = 1073741824L;
310 inv_obj.compass_correction_relative[0] = 1073741824L;
311 inv_obj.compass_disturb_correction[0] = 1073741824L;
312 inv_obj.compass_correction_offset[0] = 1073741824L;
313 inv_obj.relative_quat[0] = 1073741824L;
314
315 //Not used with the ST accelerometer
316 inv_obj.no_motion_threshold = 20; // noMotionThreshold
317 //Not used with the ST accelerometer
318 inv_obj.motion_duration = 1536; // motionDuration
319
320 inv_obj.motion_state = INV_MOTION; // Motion state
321
322 inv_obj.bias_update_time = 8000;
323 inv_obj.bias_calc_time = 2000;
324
325 inv_obj.internal_motion_state = ML_MOT_STATE_MOVING;
326
327 inv_obj.start_time = inv_get_tick_count();
328
329 inv_obj.compass_cal[0] = 322122560L;
330 inv_obj.compass_cal[4] = 322122560L;
331 inv_obj.compass_cal[8] = 322122560L;
332 inv_obj.compass_sens = 322122560L; // Should only change when the sensor full-scale range (FSR) is changed.
333
334 for (ii = 0; ii < COMPASS_NUM_AXES; ii++) {
335 inv_obj.compass_scale[ii] = 65536L;
336 inv_obj.compass_test_scale[ii] = 65536L;
337 inv_obj.compass_bias_error[ii] = P_INIT;
338 inv_obj.init_compass_bias[ii] = 0;
339 inv_obj.compass_asa[ii] = (1L << 30);
340 }
341 if (inv_compass_read_scale(tmp) == INV_SUCCESS) {
342 for (ii = 0; ii < COMPASS_NUM_AXES; ii++)
343 inv_obj.compass_asa[ii] = tmp[ii];
344 }
345 inv_obj.got_no_motion_bias = 0;
346 inv_obj.got_compass_bias = 0;
347 inv_obj.compass_state = SF_UNCALIBRATED;
348 inv_obj.acc_state = SF_STARTUP_SETTLE;
349
350 inv_obj.got_init_compass_bias = 0;
351 inv_obj.resetting_compass = 0;
352
353 inv_obj.external_slave_callback = NULL;
354 inv_obj.compass_accuracy = 0;
355
356 inv_obj.factory_temp_comp = 0;
357 inv_obj.got_coarse_heading = 0;
358
359 inv_obj.compass_bias_ptr[0] = P_INIT;
360 inv_obj.compass_bias_ptr[4] = P_INIT;
361 inv_obj.compass_bias_ptr[8] = P_INIT;
362
363 inv_obj.gyro_bias_err = 1310720;
364
365 inv_obj.accel_lpf_gain = 1073744L;
366 inv_obj.no_motion_accel_threshold = 7000000L;
367 }
368
369 /**
370 * @internal
371 * @brief This registers a function to be called for each time the DMP
372 * generates an an interrupt.
373 * It will be called after inv_register_fifo_rate_process() which gets called
374 * every time the FIFO data is processed.
375 * The FIFO does not have to be on for this callback.
376 * @param func Function to be called when a DMP interrupt occurs.
377 * @return INV_SUCCESS or non-zero error code.
378 */
inv_register_dmp_interupt_cb(inv_obj_func func)379 inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func)
380 {
381 INVENSENSE_FUNC_START;
382 // Make sure we have not filled up our number of allowable callbacks
383 if (mlxCallbackInterrupt.numInterruptProcesses <=
384 MAX_INTERRUPT_PROCESSES - 1) {
385 int kk;
386 // Make sure we haven't registered this function already
387 for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
388 if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
389 return INV_ERROR_INVALID_PARAMETER;
390 }
391 }
392 // Add new callback
393 mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt.
394 numInterruptProcesses] = func;
395 mlxCallbackInterrupt.numInterruptProcesses++;
396 return INV_SUCCESS;
397 }
398 return INV_ERROR_MEMORY_EXAUSTED;
399 }
400
401 /**
402 * @internal
403 * @brief This unregisters a function to be called for each DMP interrupt.
404 * @return INV_SUCCESS or non-zero error code.
405 */
inv_unregister_dmp_interupt_cb(inv_obj_func func)406 inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func)
407 {
408 INVENSENSE_FUNC_START;
409 int kk, jj;
410 // Make sure we haven't registered this function already
411 for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
412 if (mlxCallbackInterrupt.processInterruptCb[kk] == func) {
413 // FIXME, we may need a thread block here
414 for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses;
415 ++jj) {
416 mlxCallbackInterrupt.processInterruptCb[jj - 1] =
417 mlxCallbackInterrupt.processInterruptCb[jj];
418 }
419 mlxCallbackInterrupt.numInterruptProcesses--;
420 return INV_SUCCESS;
421 }
422 }
423 return INV_ERROR_INVALID_PARAMETER;
424 }
425
426 /**
427 * @internal
428 * @brief Run the recorded interrupt process callbacks in the event
429 * of an interrupt.
430 */
inv_run_dmp_interupt_cb(void)431 void inv_run_dmp_interupt_cb(void)
432 {
433 int kk;
434 for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) {
435 if (mlxCallbackInterrupt.processInterruptCb[kk])
436 mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj);
437 }
438 }
439
440 /** @internal
441 * Resets the Motion/No Motion state which should be called at startup and resume.
442 */
inv_reset_motion(void)443 inv_error_t inv_reset_motion(void)
444 {
445 unsigned char regs[8];
446 inv_error_t result;
447
448 inv_obj.motion_state = INV_MOTION;
449 inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION;
450 inv_obj.no_motion_accel_time = inv_get_tick_count();
451 #if defined CONFIG_MPU_SENSORS_MPU3050
452 regs[0] = DINAD8 + 2;
453 regs[1] = DINA0C;
454 regs[2] = DINAD8 + 1;
455 result = inv_set_mpu_memory(KEY_CFG_18, 3, regs);
456 if (result) {
457 LOG_RESULT_LOCATION(result);
458 return result;
459 }
460 #else
461 #endif
462 regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
463 regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
464 result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
465 if (result) {
466 LOG_RESULT_LOCATION(result);
467 return result;
468 }
469 memset(regs, 0, 8);
470 result = inv_set_mpu_memory(KEY_D_1_96, 8, regs);
471 if (result) {
472 LOG_RESULT_LOCATION(result);
473 return result;
474 }
475
476 result =
477 inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs));
478 if (result) {
479 LOG_RESULT_LOCATION(result);
480 return result;
481 }
482
483 inv_set_motion_state(INV_MOTION);
484 return result;
485 }
486
487 /**
488 * @internal
489 * @brief inv_start_bias_calc starts the bias calculation on the MPU.
490 */
inv_start_bias_calc(void)491 void inv_start_bias_calc(void)
492 {
493 INVENSENSE_FUNC_START;
494
495 inv_obj.suspend = 1;
496 }
497
498 /**
499 * @internal
500 * @brief inv_stop_bias_calc stops the bias calculation on the MPU.
501 */
inv_stop_bias_calc(void)502 void inv_stop_bias_calc(void)
503 {
504 INVENSENSE_FUNC_START;
505
506 inv_obj.suspend = 0;
507 }
508
509 /**
510 * @brief inv_update_data fetches data from the fifo and updates the
511 * motion algorithms.
512 *
513 * @pre inv_dmp_open()
514 * @ifnot MPL_MF
515 * or inv_open_low_power_pedometer()
516 * or inv_eis_open_dmp()
517 * @endif
518 * and inv_dmp_start() must have been called.
519 *
520 * @note Motion algorithm data is constant between calls to inv_update_data
521 *
522 * @return
523 * - INV_SUCCESS
524 * - Non-zero error code
525 */
inv_update_data(void)526 inv_error_t inv_update_data(void)
527 {
528 INVENSENSE_FUNC_START;
529 inv_error_t result = INV_SUCCESS;
530 int_fast8_t got, ftry;
531 uint_fast8_t mpu_interrupt;
532 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
533
534 if (inv_get_state() != INV_STATE_DMP_STARTED)
535 return INV_ERROR_SM_IMPROPER_STATE;
536
537 // Set the maximum number of FIFO packets we want to process
538 if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) {
539 ftry = 100; // Large enough to process all packets
540 } else {
541 ftry = 1;
542 }
543
544 // Go and process at most ftry number of packets, probably less than ftry
545 result = inv_read_and_process_fifo(ftry, &got);
546 if (result) {
547 LOG_RESULT_LOCATION(result);
548 return result;
549 }
550
551 // Process all interrupts
552 mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1);
553 if (mpu_interrupt) {
554 inv_clear_interrupt_trigger(INTSRC_AUX1);
555 }
556 // Check if interrupt was from MPU
557 mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU);
558 if (mpu_interrupt) {
559 inv_clear_interrupt_trigger(INTSRC_MPU);
560 }
561 // Take care of the callbacks that want to be notified when there was an MPU interrupt
562 if (mpu_interrupt) {
563 inv_run_dmp_interupt_cb();
564 }
565
566 result = inv_get_fifo_status();
567 return result;
568 }
569
570 /**
571 * @brief inv_check_flag returns the value of a flag.
572 * inv_check_flag can be used to check a number of flags,
573 * allowing users to poll flags rather than register callback
574 * functions. If a flag is set to True when inv_check_flag is called,
575 * the flag is automatically reset.
576 * The flags are:
577 * - INV_RAW_DATA_READY
578 * Indicates that new raw data is available.
579 * - INV_PROCESSED_DATA_READY
580 * Indicates that new processed data is available.
581 * - INV_GOT_GESTURE
582 * Indicates that a gesture has been detected by the gesture engine.
583 * - INV_MOTION_STATE_CHANGE
584 * Indicates that a change has been made from motion to no motion,
585 * or vice versa.
586 *
587 * @pre inv_dmp_open()
588 * @ifnot MPL_MF
589 * or inv_open_low_power_pedometer()
590 * or inv_eis_open_dmp()
591 * @endif
592 * and inv_dmp_start() must have been called.
593 *
594 * @param flag The flag to check.
595 *
596 * @return TRUE or FALSE state of the flag
597 */
inv_check_flag(int flag)598 int inv_check_flag(int flag)
599 {
600 INVENSENSE_FUNC_START;
601 int flagReturn = inv_obj.flags[flag];
602
603 inv_obj.flags[flag] = 0;
604 return flagReturn;
605 }
606
607 /**
608 * @brief Enable generation of the DMP interrupt when Motion or no-motion
609 * is detected
610 * @param on
611 * Boolean to turn the interrupt on or off.
612 * @return INV_SUCCESS or non-zero error code.
613 */
inv_set_motion_interrupt(unsigned char on)614 inv_error_t inv_set_motion_interrupt(unsigned char on)
615 {
616 INVENSENSE_FUNC_START;
617 inv_error_t result;
618 unsigned char regs[2] = { 0 };
619
620 if (inv_get_state() < INV_STATE_DMP_OPENED)
621 return INV_ERROR_SM_IMPROPER_STATE;
622
623 if (on) {
624 result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
625 if (result) {
626 LOG_RESULT_LOCATION(result);
627 return result;
628 }
629 inv_obj.interrupt_sources |= INV_INT_MOTION;
630 } else {
631 inv_obj.interrupt_sources &= ~INV_INT_MOTION;
632 if (!inv_obj.interrupt_sources) {
633 result = inv_get_dl_cfg_int(0);
634 if (result) {
635 LOG_RESULT_LOCATION(result);
636 return result;
637 }
638 }
639 }
640
641 if (on) {
642 regs[0] = DINAFE;
643 } else {
644 regs[0] = DINAD8;
645 }
646 result = inv_set_mpu_memory(KEY_CFG_7, 1, regs);
647 if (result) {
648 LOG_RESULT_LOCATION(result);
649 return result;
650 }
651 return result;
652 }
653
654 /**
655 * Enable generation of the DMP interrupt when a FIFO packet is ready
656 *
657 * @param on Boolean to turn the interrupt on or off
658 *
659 * @return INV_SUCCESS or non-zero error code
660 */
inv_set_fifo_interrupt(unsigned char on)661 inv_error_t inv_set_fifo_interrupt(unsigned char on)
662 {
663 INVENSENSE_FUNC_START;
664 inv_error_t result;
665 unsigned char regs[2] = { 0 };
666
667 if (inv_get_state() < INV_STATE_DMP_OPENED)
668 return INV_ERROR_SM_IMPROPER_STATE;
669
670 if (on) {
671 result = inv_get_dl_cfg_int(BIT_DMP_INT_EN);
672 if (result) {
673 LOG_RESULT_LOCATION(result);
674 return result;
675 }
676 inv_obj.interrupt_sources |= INV_INT_FIFO;
677 } else {
678 inv_obj.interrupt_sources &= ~INV_INT_FIFO;
679 if (!inv_obj.interrupt_sources) {
680 result = inv_get_dl_cfg_int(0);
681 if (result) {
682 LOG_RESULT_LOCATION(result);
683 return result;
684 }
685 }
686 }
687
688 if (on) {
689 regs[0] = DINAFE;
690 } else {
691 regs[0] = DINAD8;
692 }
693 result = inv_set_mpu_memory(KEY_CFG_6, 1, regs);
694 if (result) {
695 LOG_RESULT_LOCATION(result);
696 return result;
697 }
698 return result;
699 }
700
701 /**
702 * @brief Get the current set of DMP interrupt sources.
703 * These interrupts are generated by the DMP and can be
704 * routed to the MPU interrupt line via internal
705 * settings.
706 *
707 * @pre inv_dmp_open()
708 * @ifnot MPL_MF
709 * or inv_open_low_power_pedometer()
710 * or inv_eis_open_dmp()
711 * @endif
712 * must have been called.
713 *
714 * @return Currently enabled interrupt sources. The possible interrupts are:
715 * - INV_INT_FIFO,
716 * - INV_INT_MOTION,
717 * - INV_INT_TAP
718 */
inv_get_interrupts(void)719 int inv_get_interrupts(void)
720 {
721 INVENSENSE_FUNC_START;
722
723 if (inv_get_state() < INV_STATE_DMP_OPENED)
724 return 0; // error
725
726 return inv_obj.interrupt_sources;
727 }
728
729 /**
730 * @brief Sets up the Accelerometer calibration and scale factor.
731 *
732 * Please refer to the provided "9-Axis Sensor Fusion Application
733 * Note" document provided. Section 5, "Sensor Mounting Orientation"
734 * offers a good coverage on the mounting matrices and explains how
735 * to use them.
736 *
737 * @pre inv_dmp_open()
738 * @ifnot MPL_MF
739 * or inv_open_low_power_pedometer()
740 * or inv_eis_open_dmp()
741 * @endif
742 * must have been called.
743 * @pre inv_dmp_start() must <b>NOT</b> have been called.
744 *
745 * @see inv_set_gyro_calibration().
746 * @see inv_set_compass_calibration().
747 *
748 * @param[in] range
749 * The range of the accelerometers in g's. An accelerometer
750 * that has a range of +2g's to -2g's should pass in 2.
751 * @param[in] orientation
752 * A 9 element matrix that represents how the accelerometers
753 * are oriented with respect to the device they are mounted
754 * in and the reference axis system.
755 * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
756 * This example corresponds to a 3 x 3 identity matrix.
757 *
758 * @return INV_SUCCESS if successful; a non-zero error code otherwise.
759 */
inv_set_accel_calibration(float range,signed char * orientation)760 inv_error_t inv_set_accel_calibration(float range, signed char *orientation)
761 {
762 INVENSENSE_FUNC_START;
763 float cal[9];
764 float scale = range / 32768.f;
765 int kk;
766 unsigned long sf;
767 inv_error_t result;
768 unsigned char regs[4] = { 0, 0, 0, 0 };
769 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
770
771 if (inv_get_state() != INV_STATE_DMP_OPENED)
772 return INV_ERROR_SM_IMPROPER_STATE;
773
774 /* Apply zero g-offset values */
775 if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) {
776 regs[0] = 0x80;
777 regs[1] = 0x0;
778 regs[2] = 0x80;
779 regs[3] = 0x0;
780 }
781
782 if (inv_dmpkey_supported(KEY_D_1_152)) {
783 result = inv_set_mpu_memory(KEY_D_1_152, 4, ®s[0]);
784 if (result) {
785 LOG_RESULT_LOCATION(result);
786 return result;
787 }
788 }
789
790 if (scale == 0) {
791 inv_obj.accel_sens = 0;
792 }
793
794 if (mldl_cfg->accel->id) {
795 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
796 defined CONFIG_MPU_SENSORS_MPU6050B1
797 unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C };
798 #else
799 unsigned char tmp[3] = { DINA4C, DINACD, DINA6C };
800 #endif
801 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
802 unsigned char regs[3];
803 unsigned short orient;
804
805 for (kk = 0; kk < 9; ++kk) {
806 cal[kk] = scale * orientation[kk];
807 inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
808 }
809
810 orient = inv_orientation_matrix_to_scalar(orientation);
811 regs[0] = tmp[orient & 3];
812 regs[1] = tmp[(orient >> 3) & 3];
813 regs[2] = tmp[(orient >> 6) & 3];
814 result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs);
815 if (result) {
816 LOG_RESULT_LOCATION(result);
817 return result;
818 }
819
820 regs[0] = DINA26;
821 regs[1] = DINA46;
822 #if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2
823 regs[2] = DINA66;
824 #else
825 if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision)
826 == MPU_PRODUCT_KEY_B1_E1_5)
827 regs[2] = DINA76;
828 else
829 regs[2] = DINA66;
830 #endif
831 if (orient & 4)
832 regs[0] |= 1;
833 if (orient & 0x20)
834 regs[1] |= 1;
835 if (orient & 0x100)
836 regs[2] |= 1;
837
838 result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs);
839 if (result) {
840 LOG_RESULT_LOCATION(result);
841 return result;
842 }
843
844 if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) {
845 result = inv_freescale_sensor_fusion_16bit(orient);
846 if (result) {
847 LOG_RESULT_LOCATION(result);
848 return result;
849 }
850 } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) {
851 result = inv_freescale_sensor_fusion_8bit(orient);
852 if (result) {
853 LOG_RESULT_LOCATION(result);
854 return result;
855 }
856 }
857 }
858
859 if (inv_obj.accel_sens != 0) {
860 sf = (1073741824L / inv_obj.accel_sens);
861 } else {
862 sf = 0;
863 }
864 regs[0] = (unsigned char)((sf >> 8) & 0xff);
865 regs[1] = (unsigned char)(sf & 0xff);
866 result = inv_set_mpu_memory(KEY_D_0_108, 2, regs);
867 if (result) {
868 LOG_RESULT_LOCATION(result);
869 return result;
870 }
871
872 return result;
873 }
874
875 /**
876 * @brief Sets up the Gyro calibration and scale factor.
877 *
878 * Please refer to the provided "9-Axis Sensor Fusion Application
879 * Note" document provided. Section 5, "Sensor Mounting Orientation"
880 * offers a good coverage on the mounting matrices and explains
881 * how to use them.
882 *
883 * @pre inv_dmp_open()
884 * @ifnot MPL_MF
885 * or inv_open_low_power_pedometer()
886 * or inv_eis_open_dmp()
887 * @endif
888 * must have been called.
889 * @pre inv_dmp_start() must have <b>NOT</b> been called.
890 *
891 * @see inv_set_accel_calibration().
892 * @see inv_set_compass_calibration().
893 *
894 * @param[in] range
895 * The range of the gyros in degrees per second. A gyro
896 * that has a range of +2000 dps to -2000 dps should pass in
897 * 2000.
898 * @param[in] orientation
899 * A 9 element matrix that represents how the gyro are oriented
900 * with respect to the device they are mounted in. A typical
901 * set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This
902 * example corresponds to a 3 x 3 identity matrix.
903 *
904 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
905 */
inv_set_gyro_calibration(float range,signed char * orientation)906 inv_error_t inv_set_gyro_calibration(float range, signed char *orientation)
907 {
908 INVENSENSE_FUNC_START;
909
910 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
911 int kk;
912 float scale;
913 inv_error_t result;
914
915 unsigned char regs[12] = { 0 };
916 unsigned char maxVal = 0;
917 unsigned char tmpPtr = 0;
918 unsigned char tmpSign = 0;
919 unsigned char i = 0;
920 int sf = 0;
921
922 if (inv_get_state() != INV_STATE_DMP_OPENED)
923 return INV_ERROR_SM_IMPROPER_STATE;
924
925 if (mldl_cfg->gyro_sens_trim != 0) {
926 /* adjust the declared range to consider the gyro_sens_trim
927 of this part when different from the default (first dividend) */
928 range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim;
929 }
930
931 scale = range / 32768.f; // inverse of sensitivity for the given full scale range
932 inv_obj.gyro_sens = (long)(range * 32768L);
933
934 for (kk = 0; kk < 9; ++kk) {
935 inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30));
936 inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30));
937 }
938
939 {
940 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
941 defined CONFIG_MPU_SENSORS_MPU6050B1
942 unsigned char tmpD = DINA4C;
943 unsigned char tmpE = DINACD;
944 unsigned char tmpF = DINA6C;
945 #else
946 unsigned char tmpD = DINAC9;
947 unsigned char tmpE = DINA2C;
948 unsigned char tmpF = DINACB;
949 #endif
950 regs[3] = DINA36;
951 regs[4] = DINA56;
952 regs[5] = DINA76;
953
954 for (i = 0; i < 3; i++) {
955 maxVal = 0;
956 tmpSign = 0;
957 if (inv_obj.gyro_orient[0 + 3 * i] < 0)
958 tmpSign = 1;
959 if (ABS(inv_obj.gyro_orient[1 + 3 * i]) >
960 ABS(inv_obj.gyro_orient[0 + 3 * i])) {
961 maxVal = 1;
962 if (inv_obj.gyro_orient[1 + 3 * i] < 0)
963 tmpSign = 1;
964 }
965 if (ABS(inv_obj.gyro_orient[2 + 3 * i]) >
966 ABS(inv_obj.gyro_orient[1 + 3 * i])) {
967 tmpSign = 0;
968 maxVal = 2;
969 if (inv_obj.gyro_orient[2 + 3 * i] < 0)
970 tmpSign = 1;
971 }
972 if (maxVal == 0) {
973 regs[tmpPtr++] = tmpD;
974 if (tmpSign)
975 regs[tmpPtr + 2] |= 0x01;
976 } else if (maxVal == 1) {
977 regs[tmpPtr++] = tmpE;
978 if (tmpSign)
979 regs[tmpPtr + 2] |= 0x01;
980 } else {
981 regs[tmpPtr++] = tmpF;
982 if (tmpSign)
983 regs[tmpPtr + 2] |= 0x01;
984 }
985 }
986 result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs);
987 if (result) {
988 LOG_RESULT_LOCATION(result);
989 return result;
990 }
991 result = inv_set_mpu_memory(KEY_FCFG_3, 3, ®s[3]);
992 if (result) {
993 LOG_RESULT_LOCATION(result);
994 return result;
995 }
996
997 //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384
998 inv_obj.gyro_sf =
999 (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL);
1000 result =
1001 inv_set_mpu_memory(KEY_D_0_104, 4,
1002 inv_int32_to_big8(inv_obj.gyro_sf, regs));
1003 if (result) {
1004 LOG_RESULT_LOCATION(result);
1005 return result;
1006 }
1007
1008 if (inv_obj.gyro_sens != 0) {
1009 sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens);
1010 } else {
1011 sf = 0;
1012 }
1013
1014 result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs));
1015 if (result) {
1016 LOG_RESULT_LOCATION(result);
1017 return result;
1018 }
1019 }
1020 return INV_SUCCESS;
1021 }
1022
1023 /**
1024 * @brief Sets up the Compass calibration and scale factor.
1025 *
1026 * Please refer to the provided "9-Axis Sensor Fusion Application
1027 * Note" document provided. Section 5, "Sensor Mounting Orientation"
1028 * offers a good coverage on the mounting matrices and explains
1029 * how to use them.
1030 *
1031 * @pre inv_dmp_open()
1032 * @ifnot MPL_MF
1033 * or inv_open_low_power_pedometer()
1034 * or inv_eis_open_dmp()
1035 * @endif
1036 * must have been called.
1037 * @pre inv_dmp_start() must have <b>NOT</b> been called.
1038 *
1039 * @see inv_set_gyro_calibration().
1040 * @see inv_set_accel_calibration().
1041 *
1042 * @param[in] range
1043 * The range of the compass.
1044 * @param[in] orientation
1045 * A 9 element matrix that represents how the compass is
1046 * oriented with respect to the device they are mounted in.
1047 * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}.
1048 * This example corresponds to a 3 x 3 identity matrix.
1049 * The matrix describes how to go from the chip mounting to
1050 * the body of the device.
1051 *
1052 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
1053 */
inv_set_compass_calibration(float range,signed char * orientation)1054 inv_error_t inv_set_compass_calibration(float range, signed char *orientation)
1055 {
1056 INVENSENSE_FUNC_START;
1057 float cal[9];
1058 float scale = range / 32768.f;
1059 int kk;
1060 unsigned short compassId = 0;
1061
1062 compassId = inv_get_compass_id();
1063
1064 if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883)
1065 || (compassId == COMPASS_ID_LSM303M)) {
1066 scale /= 32.0f;
1067 }
1068
1069 for (kk = 0; kk < 9; ++kk) {
1070 cal[kk] = scale * orientation[kk];
1071 inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30));
1072 }
1073
1074 inv_obj.compass_sens = (long)(scale * 1073741824L);
1075
1076 if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) {
1077 unsigned char reg0[4] = { 0, 0, 0, 0 };
1078 unsigned char regp[4] = { 64, 0, 0, 0 };
1079 unsigned char regn[4] = { 64 + 128, 0, 0, 0 };
1080 unsigned char *reg;
1081 int_fast8_t kk;
1082 unsigned short keyList[9] =
1083 { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02,
1084 KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12,
1085 KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22
1086 };
1087
1088 for (kk = 0; kk < 9; ++kk) {
1089
1090 if (orientation[kk] == 1)
1091 reg = regp;
1092 else if (orientation[kk] == -1)
1093 reg = regn;
1094 else
1095 reg = reg0;
1096 inv_set_mpu_memory(keyList[kk], 4, reg);
1097 }
1098 }
1099
1100 return INV_SUCCESS;
1101 }
1102
1103 /**
1104 * @internal
1105 * @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup.
1106 */
inv_set_dead_zone(void)1107 inv_error_t inv_set_dead_zone(void)
1108 {
1109 unsigned char reg;
1110 inv_error_t result;
1111 extern struct control_params cntrl_params;
1112
1113 if (cntrl_params.functions & INV_DEAD_ZONE) {
1114 reg = 0x08;
1115 } else {
1116 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
1117 defined CONFIG_MPU_SENSORS_MPU6050B1
1118 reg = 0;
1119 #else
1120 if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
1121 reg = 0x2;
1122 } else {
1123 reg = 0;
1124 }
1125 #endif
1126 }
1127
1128 result = inv_set_mpu_memory(KEY_D_0_163, 1, ®);
1129 if (result) {
1130 LOG_RESULT_LOCATION(result);
1131 return result;
1132 }
1133 return result;
1134 }
1135
1136 /**
1137 * @brief inv_set_bias_update is used to register which algorithms will be
1138 * used to automatically reset the gyroscope bias.
1139 * The engine INV_BIAS_UPDATE must be enabled for these algorithms to
1140 * run.
1141 *
1142 * @pre inv_dmp_open()
1143 * @ifnot MPL_MF
1144 * or inv_open_low_power_pedometer()
1145 * or inv_eis_open_dmp()
1146 * @endif
1147 * and inv_dmp_start()
1148 * must <b>NOT</b> have been called.
1149 *
1150 * @param function A function or bitwise OR of functions that determine
1151 * how the gyroscope bias will be automatically updated.
1152 * Functions include:
1153 * - INV_NONE or 0,
1154 * - INV_BIAS_FROM_NO_MOTION,
1155 * - INV_BIAS_FROM_GRAVITY,
1156 * - INV_BIAS_FROM_TEMPERATURE,
1157 \ifnot UMPL
1158 * - INV_BIAS_FROM_LPF,
1159 * - INV_MAG_BIAS_FROM_MOTION,
1160 * - INV_MAG_BIAS_FROM_GYRO,
1161 * - INV_ALL.
1162 * \endif
1163 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
1164 */
inv_set_bias_update(unsigned short function)1165 inv_error_t inv_set_bias_update(unsigned short function)
1166 {
1167 INVENSENSE_FUNC_START;
1168 unsigned char regs[4];
1169 long tmp[3] = { 0, 0, 0 };
1170 inv_error_t result = INV_SUCCESS;
1171 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
1172
1173 if (inv_get_state() != INV_STATE_DMP_OPENED)
1174 return INV_ERROR_SM_IMPROPER_STATE;
1175
1176 /* do not allow progressive no motion bias tracker to run -
1177 it's not fully debugged */
1178 function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround
1179 MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n");
1180
1181 // You must use EnableFastNoMotion() to get this feature
1182 function &= ~INV_BIAS_FROM_FAST_NO_MOTION;
1183
1184 // You must use DisableFastNoMotion() to turn this feature off
1185 if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION)
1186 function |= INV_BIAS_FROM_FAST_NO_MOTION;
1187
1188 /*--- remove magnetic components from bias tracking
1189 if there is no compass ---*/
1190 if (!inv_compass_present()) {
1191 function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION);
1192 } else {
1193 function &= ~(INV_BIAS_FROM_LPF);
1194 }
1195
1196 // Enable function sets bias from no motion
1197 inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION);
1198
1199 if (function & INV_BIAS_FROM_NO_MOTION) {
1200 inv_enable_bias_no_motion();
1201 } else {
1202 inv_disable_bias_no_motion();
1203 }
1204
1205 if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) {
1206 regs[0] = DINA80 + 2;
1207 regs[1] = DINA2D;
1208 regs[2] = DINA55;
1209 regs[3] = DINA7D;
1210 } else {
1211 regs[0] = DINA80 + 7;
1212 regs[1] = DINA2D;
1213 regs[2] = DINA35;
1214 regs[3] = DINA3D;
1215 }
1216 result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs);
1217 if (result) {
1218 LOG_RESULT_LOCATION(result);
1219 return result;
1220 }
1221 result = inv_set_dead_zone();
1222 if (result) {
1223 LOG_RESULT_LOCATION(result);
1224 return result;
1225 }
1226
1227 if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) &&
1228 !inv_compass_present()) {
1229 result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION);
1230 if (result) {
1231 LOG_RESULT_LOCATION(result);
1232 return result;
1233 }
1234 } else {
1235 result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW);
1236 if (result) {
1237 LOG_RESULT_LOCATION(result);
1238 return result;
1239 }
1240 }
1241
1242 inv_obj.factory_temp_comp = 0; // FIXME, workaround
1243 if ((mldl_cfg->offset_tc[0] != 0) ||
1244 (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) {
1245 inv_obj.factory_temp_comp = 1;
1246 }
1247
1248 if (inv_obj.factory_temp_comp == 0) {
1249 if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) {
1250 result = inv_set_gyro_temp_slope(inv_obj.temp_slope);
1251 if (result) {
1252 LOG_RESULT_LOCATION(result);
1253 return result;
1254 }
1255 } else {
1256 result = inv_set_gyro_temp_slope(tmp);
1257 if (result) {
1258 LOG_RESULT_LOCATION(result);
1259 return result;
1260 }
1261 }
1262 } else {
1263 inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE;
1264 MPL_LOGV("factory temperature compensation coefficients available - "
1265 "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n");
1266 }
1267
1268 /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on
1269 compass and accel data, is to have accelerometer data delivered in the
1270 FIFO ----*/
1271 if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY)
1272 && inv_compass_present())
1273 || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO)
1274 || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) {
1275 inv_send_accel(INV_ALL, INV_32_BIT);
1276 inv_send_gyro(INV_ALL, INV_32_BIT);
1277 }
1278
1279 return result;
1280 }
1281
1282 /**
1283 * @brief inv_get_motion_state is used to determine if the device is in
1284 * a 'motion' or 'no motion' state.
1285 * inv_get_motion_state returns INV_MOTION of the device is moving,
1286 * or INV_NO_MOTION if the device is not moving.
1287 *
1288 * @pre inv_dmp_open()
1289 * @ifnot MPL_MF
1290 * or inv_open_low_power_pedometer()
1291 * or inv_eis_open_dmp()
1292 * @endif
1293 * and inv_dmp_start()
1294 * must have been called.
1295 *
1296 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
1297 */
inv_get_motion_state(void)1298 int inv_get_motion_state(void)
1299 {
1300 INVENSENSE_FUNC_START;
1301 return inv_obj.motion_state;
1302 }
1303
1304 /**
1305 * @brief inv_set_no_motion_thresh is used to set the threshold for
1306 * detecting INV_NO_MOTION
1307 *
1308 * @pre inv_dmp_open()
1309 * @ifnot MPL_MF
1310 * or inv_open_low_power_pedometer()
1311 * or inv_eis_open_dmp()
1312 * @endif
1313 * must have been called.
1314 *
1315 * @param thresh A threshold scaled in degrees per second.
1316 *
1317 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
1318 */
inv_set_no_motion_thresh(float thresh)1319 inv_error_t inv_set_no_motion_thresh(float thresh)
1320 {
1321 inv_error_t result = INV_SUCCESS;
1322 unsigned char regs[4] = { 0 };
1323 long tmp;
1324 INVENSENSE_FUNC_START;
1325
1326 tmp = (long)(thresh * thresh * 2.045f);
1327 if (tmp < 0) {
1328 return INV_ERROR;
1329 } else if (tmp > 8180000L) {
1330 return INV_ERROR;
1331 }
1332 inv_obj.no_motion_threshold = tmp;
1333
1334 regs[0] = (unsigned char)(tmp >> 24);
1335 regs[1] = (unsigned char)((tmp >> 16) & 0xff);
1336 regs[2] = (unsigned char)((tmp >> 8) & 0xff);
1337 regs[3] = (unsigned char)(tmp & 0xff);
1338
1339 result = inv_set_mpu_memory(KEY_D_1_108, 4, regs);
1340 if (result) {
1341 LOG_RESULT_LOCATION(result);
1342 return result;
1343 }
1344 result = inv_reset_motion();
1345 return result;
1346 }
1347
1348 /**
1349 * @brief inv_set_no_motion_threshAccel is used to set the threshold for
1350 * detecting INV_NO_MOTION with accelerometers when Gyros have
1351 * been turned off
1352 *
1353 * @pre inv_dmp_open()
1354 * @ifnot MPL_MF
1355 * or inv_open_low_power_pedometer()
1356 * or inv_eis_open_dmp()
1357 * @endif
1358 * must have been called.
1359 *
1360 * @param thresh A threshold in g's scaled by 2^32
1361 *
1362 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
1363 */
inv_set_no_motion_threshAccel(long thresh)1364 inv_error_t inv_set_no_motion_threshAccel(long thresh)
1365 {
1366 INVENSENSE_FUNC_START;
1367
1368 inv_obj.no_motion_accel_threshold = thresh;
1369
1370 return INV_SUCCESS;
1371 }
1372
1373 /**
1374 * @brief inv_set_no_motion_time is used to set the time required for
1375 * detecting INV_NO_MOTION
1376 *
1377 * @pre inv_dmp_open()
1378 * @ifnot MPL_MF
1379 * or inv_open_low_power_pedometer()
1380 * or inv_eis_open_dmp()
1381 * @endif
1382 * must have been called.
1383 *
1384 * @param time A time in seconds.
1385 *
1386 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
1387 */
inv_set_no_motion_time(float time)1388 inv_error_t inv_set_no_motion_time(float time)
1389 {
1390 inv_error_t result = INV_SUCCESS;
1391 unsigned char regs[2] = { 0 };
1392 long tmp;
1393
1394 INVENSENSE_FUNC_START;
1395
1396 tmp = (long)(time * 200);
1397 if (tmp < 0) {
1398 return INV_ERROR;
1399 } else if (tmp > 65535L) {
1400 return INV_ERROR;
1401 }
1402 inv_obj.motion_duration = (unsigned short)tmp;
1403
1404 regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff);
1405 regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff);
1406 result = inv_set_mpu_memory(KEY_D_1_106, 2, regs);
1407 if (result) {
1408 LOG_RESULT_LOCATION(result);
1409 return result;
1410 }
1411 result = inv_reset_motion();
1412 return result;
1413 }
1414
1415 /**
1416 * @brief inv_get_version is used to get the ML version.
1417 *
1418 * @pre inv_get_version can be called at any time.
1419 *
1420 * @param version inv_get_version writes the ML version
1421 * string pointer to version.
1422 *
1423 * @return INV_SUCCESS if successful or Non-zero error code otherwise.
1424 */
inv_get_version(unsigned char ** version)1425 inv_error_t inv_get_version(unsigned char **version)
1426 {
1427 INVENSENSE_FUNC_START;
1428
1429 *version = (unsigned char *)mlVer; //fixme we are wiping const
1430
1431 return INV_SUCCESS;
1432 }
1433
1434 /**
1435 * @brief Check for the presence of the gyro sensor.
1436 *
1437 * This is not a physical check but a logical check and the value can change
1438 * dynamically based on calls to inv_set_mpu_sensors().
1439 *
1440 * @return TRUE if the gyro is enabled FALSE otherwise.
1441 */
inv_get_gyro_present(void)1442 int inv_get_gyro_present(void)
1443 {
1444 return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO |
1445 INV_Z_GYRO);
1446 }
1447
inv_row_2_scale(const signed char * row)1448 static unsigned short inv_row_2_scale(const signed char *row)
1449 {
1450 unsigned short b;
1451
1452 if (row[0] > 0)
1453 b = 0;
1454 else if (row[0] < 0)
1455 b = 4;
1456 else if (row[1] > 0)
1457 b = 1;
1458 else if (row[1] < 0)
1459 b = 5;
1460 else if (row[2] > 0)
1461 b = 2;
1462 else if (row[2] < 0)
1463 b = 6;
1464 else
1465 b = 7; // error
1466 return b;
1467 }
1468
inv_orientation_matrix_to_scalar(const signed char * mtx)1469 unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
1470 {
1471 unsigned short scalar;
1472 /*
1473 XYZ 010_001_000 Identity Matrix
1474 XZY 001_010_000
1475 YXZ 010_000_001
1476 YZX 000_010_001
1477 ZXY 001_000_010
1478 ZYX 000_001_010
1479 */
1480
1481 scalar = inv_row_2_scale(mtx);
1482 scalar |= inv_row_2_scale(mtx + 3) << 3;
1483 scalar |= inv_row_2_scale(mtx + 6) << 6;
1484
1485 return scalar;
1486 }
1487
1488 /* Setups up the Freescale 16-bit accel for Sensor Fusion
1489 * @param[in] orient A scalar representation of the orientation
1490 * that describes how to go from the Chip Orientation
1491 * to the Board Orientation often times called the Body Orientation in Invensense Documentation.
1492 * inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
1493 */
inv_freescale_sensor_fusion_16bit(unsigned short orient)1494 inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient)
1495 {
1496 unsigned char rr[3];
1497 inv_error_t result;
1498
1499 orient = orient & 0xdb;
1500 switch (orient) {
1501 default:
1502 // Typically 0x88
1503 rr[0] = DINACC;
1504 rr[1] = DINACF;
1505 rr[2] = DINA0E;
1506 break;
1507 case 0x50:
1508 rr[0] = DINACE;
1509 rr[1] = DINA0E;
1510 rr[2] = DINACD;
1511 break;
1512 case 0x81:
1513 rr[0] = DINACE;
1514 rr[1] = DINACB;
1515 rr[2] = DINA0E;
1516 break;
1517 case 0x11:
1518 rr[0] = DINACC;
1519 rr[1] = DINA0E;
1520 rr[2] = DINACB;
1521 break;
1522 case 0x42:
1523 rr[0] = DINA0A;
1524 rr[1] = DINACF;
1525 rr[2] = DINACB;
1526 break;
1527 case 0x0a:
1528 rr[0] = DINA0A;
1529 rr[1] = DINACB;
1530 rr[2] = DINACD;
1531 break;
1532 }
1533 result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr);
1534 return result;
1535 }
1536
1537 /* Setups up the Freescale 8-bit accel for Sensor Fusion
1538 * @param[in] orient A scalar representation of the orientation
1539 * that describes how to go from the Chip Orientation
1540 * to the Board Orientation often times called the Body Orientation in Invensense Documentation.
1541 * inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar.
1542 */
inv_freescale_sensor_fusion_8bit(unsigned short orient)1543 inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient)
1544 {
1545 unsigned char regs[27];
1546 inv_error_t result;
1547 uint_fast8_t kk;
1548
1549 orient = orient & 0xdb;
1550 kk = 0;
1551
1552 regs[kk++] = DINAC3;
1553 regs[kk++] = DINA90 + 14;
1554 regs[kk++] = DINAA0 + 9;
1555 regs[kk++] = DINA3E;
1556 regs[kk++] = DINA5E;
1557 regs[kk++] = DINA7E;
1558
1559 regs[kk++] = DINAC2;
1560 regs[kk++] = DINAA0 + 9;
1561 regs[kk++] = DINA90 + 9;
1562 regs[kk++] = DINAF8 + 2;
1563
1564 switch (orient) {
1565 default:
1566 // Typically 0x88
1567 regs[kk++] = DINACB;
1568
1569 regs[kk++] = DINA54;
1570 regs[kk++] = DINA50;
1571 regs[kk++] = DINA50;
1572 regs[kk++] = DINA50;
1573 regs[kk++] = DINA50;
1574 regs[kk++] = DINA50;
1575 regs[kk++] = DINA50;
1576 regs[kk++] = DINA50;
1577
1578 regs[kk++] = DINACD;
1579 break;
1580 case 0x50:
1581 regs[kk++] = DINACB;
1582
1583 regs[kk++] = DINACF;
1584
1585 regs[kk++] = DINA7C;
1586 regs[kk++] = DINA78;
1587 regs[kk++] = DINA78;
1588 regs[kk++] = DINA78;
1589 regs[kk++] = DINA78;
1590 regs[kk++] = DINA78;
1591 regs[kk++] = DINA78;
1592 regs[kk++] = DINA78;
1593 break;
1594 case 0x81:
1595 regs[kk++] = DINA2C;
1596 regs[kk++] = DINA28;
1597 regs[kk++] = DINA28;
1598 regs[kk++] = DINA28;
1599 regs[kk++] = DINA28;
1600 regs[kk++] = DINA28;
1601 regs[kk++] = DINA28;
1602 regs[kk++] = DINA28;
1603
1604 regs[kk++] = DINACD;
1605
1606 regs[kk++] = DINACB;
1607 break;
1608 case 0x11:
1609 regs[kk++] = DINA2C;
1610 regs[kk++] = DINA28;
1611 regs[kk++] = DINA28;
1612 regs[kk++] = DINA28;
1613 regs[kk++] = DINA28;
1614 regs[kk++] = DINA28;
1615 regs[kk++] = DINA28;
1616 regs[kk++] = DINA28;
1617 regs[kk++] = DINACB;
1618 regs[kk++] = DINACF;
1619 break;
1620 case 0x42:
1621 regs[kk++] = DINACF;
1622 regs[kk++] = DINACD;
1623
1624 regs[kk++] = DINA7C;
1625 regs[kk++] = DINA78;
1626 regs[kk++] = DINA78;
1627 regs[kk++] = DINA78;
1628 regs[kk++] = DINA78;
1629 regs[kk++] = DINA78;
1630 regs[kk++] = DINA78;
1631 regs[kk++] = DINA78;
1632 break;
1633 case 0x0a:
1634 regs[kk++] = DINACD;
1635
1636 regs[kk++] = DINA54;
1637 regs[kk++] = DINA50;
1638 regs[kk++] = DINA50;
1639 regs[kk++] = DINA50;
1640 regs[kk++] = DINA50;
1641 regs[kk++] = DINA50;
1642 regs[kk++] = DINA50;
1643 regs[kk++] = DINA50;
1644
1645 regs[kk++] = DINACF;
1646 break;
1647 }
1648
1649 regs[kk++] = DINA90 + 7;
1650 regs[kk++] = DINAF8 + 3;
1651 regs[kk++] = DINAA0 + 9;
1652 regs[kk++] = DINA0E;
1653 regs[kk++] = DINA0E;
1654 regs[kk++] = DINA0E;
1655
1656 regs[kk++] = DINAF8 + 1; // filler
1657
1658 result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs);
1659 if (result) {
1660 LOG_RESULT_LOCATION(result);
1661 return result;
1662 }
1663
1664 return result;
1665 }
1666
1667 /**
1668 * Controlls each sensor and each axis when the motion processing unit is
1669 * running. When it is not running, simply records the state for later.
1670 *
1671 * NOTE: In this version only full sensors controll is allowed. Independent
1672 * axis control will return an error.
1673 *
1674 * @param sensors Bit field of each axis desired to be turned on or off
1675 *
1676 * @return INV_SUCCESS or non-zero error code
1677 */
inv_set_mpu_sensors(unsigned long sensors)1678 inv_error_t inv_set_mpu_sensors(unsigned long sensors)
1679 {
1680 INVENSENSE_FUNC_START;
1681 unsigned char state = inv_get_state();
1682 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
1683 inv_error_t result = INV_SUCCESS;
1684 unsigned short fifoRate;
1685
1686 if (state < INV_STATE_DMP_OPENED)
1687 return INV_ERROR_SM_IMPROPER_STATE;
1688
1689 if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) &&
1690 ((sensors & INV_THREE_AXIS_ACCEL) != 0)) {
1691 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1692 }
1693 if (((sensors & INV_THREE_AXIS_ACCEL) != 0) &&
1694 (mldl_cfg->pdata->accel.get_slave_descr == 0)) {
1695 return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
1696 }
1697
1698 if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) &&
1699 ((sensors & INV_THREE_AXIS_COMPASS) != 0)) {
1700 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1701 }
1702 if (((sensors & INV_THREE_AXIS_COMPASS) != 0) &&
1703 (mldl_cfg->pdata->compass.get_slave_descr == 0)) {
1704 return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
1705 }
1706
1707 if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) &&
1708 ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) {
1709 return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
1710 }
1711 if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) &&
1712 (mldl_cfg->pdata->pressure.get_slave_descr == 0)) {
1713 return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED;
1714 }
1715
1716 /* DMP was off, and is turning on */
1717 if (sensors & INV_DMP_PROCESSOR &&
1718 !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
1719 struct ext_slave_config config;
1720 long odr;
1721 config.key = MPU_SLAVE_CONFIG_ODR_RESUME;
1722 config.len = sizeof(long);
1723 config.apply = (state == INV_STATE_DMP_STARTED);
1724 config.data = &odr;
1725
1726 odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000);
1727 result = inv_mpu_config_accel(mldl_cfg,
1728 inv_get_serial_handle(),
1729 inv_get_serial_handle(), &config);
1730 if (result) {
1731 LOG_RESULT_LOCATION(result);
1732 return result;
1733 }
1734
1735 config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
1736 odr = MPU_SLAVE_IRQ_TYPE_NONE;
1737 result = inv_mpu_config_accel(mldl_cfg,
1738 inv_get_serial_handle(),
1739 inv_get_serial_handle(), &config);
1740 if (result) {
1741 LOG_RESULT_LOCATION(result);
1742 return result;
1743 }
1744 inv_init_fifo_hardare();
1745 }
1746
1747 if (inv_obj.mode_change_func) {
1748 result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors);
1749 if (result) {
1750 LOG_RESULT_LOCATION(result);
1751 return result;
1752 }
1753 }
1754
1755 /* Get the fifo rate before changing sensors so if we need to match it */
1756 fifoRate = inv_get_fifo_rate();
1757 mldl_cfg->requested_sensors = sensors;
1758
1759 /* inv_dmp_start will turn the sensors on */
1760 if (state == INV_STATE_DMP_STARTED) {
1761 result = inv_dl_start(sensors);
1762 if (result) {
1763 LOG_RESULT_LOCATION(result);
1764 return result;
1765 }
1766 result = inv_reset_motion();
1767 if (result) {
1768 LOG_RESULT_LOCATION(result);
1769 return result;
1770 }
1771 result = inv_dl_stop(~sensors);
1772 if (result) {
1773 LOG_RESULT_LOCATION(result);
1774 return result;
1775 }
1776 }
1777
1778 if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) {
1779 struct ext_slave_config config;
1780 long data;
1781 inv_set_fifo_rate(fifoRate);
1782
1783 config.len = sizeof(long);
1784 config.key = MPU_SLAVE_CONFIG_IRQ_RESUME;
1785 config.apply = (state == INV_STATE_DMP_STARTED);
1786 config.data = &data;
1787 data = MPU_SLAVE_IRQ_TYPE_DATA_READY;
1788 result = inv_mpu_config_accel(mldl_cfg,
1789 inv_get_serial_handle(),
1790 inv_get_serial_handle(), &config);
1791 if (result) {
1792 LOG_RESULT_LOCATION(result);
1793 return result;
1794 }
1795 }
1796
1797 return result;
1798 }
1799
inv_set_mode_change(inv_error_t (* mode_change_func)(unsigned long,unsigned long))1800 void inv_set_mode_change(inv_error_t(*mode_change_func)
1801 (unsigned long, unsigned long))
1802 {
1803 inv_obj.mode_change_func = mode_change_func;
1804 }
1805
1806 /**
1807 * Mantis setup
1808 */
1809 #ifdef CONFIG_MPU_SENSORS_MPU6050B1
inv_set_mpu_6050_config()1810 inv_error_t inv_set_mpu_6050_config()
1811 {
1812 long temp;
1813 inv_error_t result;
1814 unsigned char big8[4];
1815 unsigned char atc[4];
1816 long s[3], s2[3];
1817 int kk;
1818 struct mldl_cfg *mldlCfg = inv_get_dl_config();
1819
1820 result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(),
1821 0x0d, 4, atc);
1822 if (result) {
1823 LOG_RESULT_LOCATION(result);
1824 return result;
1825 }
1826
1827 temp = atc[3] & 0x3f;
1828 if (temp >= 32)
1829 temp = temp - 64;
1830 temp = (temp << 21) | 0x100000;
1831 temp += (1L << 29);
1832 temp = -temp;
1833 result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8));
1834 if (result) {
1835 LOG_RESULT_LOCATION(result);
1836 return result;
1837 }
1838
1839 for (kk = 0; kk < 3; ++kk) {
1840 s[kk] = atc[kk] & 0x3f;
1841 if (s[kk] > 32)
1842 s[kk] = s[kk] - 64;
1843 s[kk] *= 2516582L;
1844 }
1845
1846 for (kk = 0; kk < 3; ++kk) {
1847 s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] +
1848 mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] +
1849 mldlCfg->pdata->orientation[kk * 3 + 2] * s[2];
1850 }
1851 result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8));
1852 if (result) {
1853 LOG_RESULT_LOCATION(result);
1854 return result;
1855 }
1856 result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8));
1857 if (result) {
1858 LOG_RESULT_LOCATION(result);
1859 return result;
1860 }
1861 result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8));
1862 if (result) {
1863 LOG_RESULT_LOCATION(result);
1864 return result;
1865 }
1866
1867 return result;
1868 }
1869 #endif
1870
1871 /**
1872 * @}
1873 */
1874