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