From 901b63272e2eb758fe2f4a588e6e9f308fe50f6c Mon Sep 17 00:00:00 2001 From: Scott Warner Date: Sun, 11 Oct 2015 08:30:01 -0400 Subject: Revert "Remove files for unsupported devices." This reverts commit f5f584ee173faef40f226c6e0e8580a2ecbe079b. Change-Id: I4e1b41922b5ccaac2314dac7f43df5740e2e9361 --- 60xx/Android.mk | 8 + 60xx/libsensors/Android.mk | 47 + 60xx/libsensors/MPLSensor.cpp | 1304 +++++++++ 60xx/libsensors/MPLSensor.h | 142 + 60xx/libsensors/SensorBase.cpp | 129 + 60xx/libsensors/SensorBase.h | 65 + 60xx/libsensors/sensor_params.h | 134 + 60xx/libsensors/sensors.h | 53 + 60xx/libsensors_iio/Android.mk | 118 + 60xx/libsensors_iio/CompassSensor.IIO.9150.cpp | 390 +++ 60xx/libsensors_iio/CompassSensor.IIO.9150.h | 93 + 60xx/libsensors_iio/InputEventReader.cpp | 106 + 60xx/libsensors_iio/InputEventReader.h | 49 + 60xx/libsensors_iio/License.txt | 217 ++ 60xx/libsensors_iio/MPLSensor.cpp | 2764 ++++++++++++++++++++ 60xx/libsensors_iio/MPLSensor.h | 346 +++ 60xx/libsensors_iio/MPLSupport.cpp | 190 ++ 60xx/libsensors_iio/MPLSupport.h | 33 + 60xx/libsensors_iio/SensorBase.cpp | 135 + 60xx/libsensors_iio/SensorBase.h | 61 + 60xx/libsensors_iio/libmllite.so | Bin 0 -> 88258 bytes 60xx/libsensors_iio/libmplmpu.so | Bin 0 -> 164995 bytes 60xx/libsensors_iio/local_log_def.h | 49 + 60xx/libsensors_iio/sensor_params.h | 194 ++ 60xx/libsensors_iio/sensors.h | 97 + 60xx/libsensors_iio/sensors_mpl.cpp | 260 ++ .../software/build/android/common.mk | 87 + .../software/build/android/shared.mk | 74 + .../software/core/driver/include/log.h | 364 +++ .../software/core/driver/include/mlinclude.h | 38 + .../software/core/driver/include/mlmath.h | 95 + .../software/core/driver/include/mlsl.h | 283 ++ .../software/core/driver/include/mltypes.h | 235 ++ .../core/driver/include/stdint_invensense.h | 41 + .../core/mllite/build/android/libmllite.so | Bin 0 -> 88258 bytes .../software/core/mllite/build/android/shared.mk | 87 + .../software/core/mllite/build/filelist.mk | 42 + .../software/core/mllite/data_builder.c | 1228 +++++++++ .../software/core/mllite/data_builder.h | 240 ++ .../software/core/mllite/hal_outputs.c | 497 ++++ .../software/core/mllite/hal_outputs.h | 49 + .../software/core/mllite/invensense.h | 28 + .../software/core/mllite/linux/ml_load_dmp.c | 279 ++ .../software/core/mllite/linux/ml_load_dmp.h | 33 + .../software/core/mllite/linux/ml_stored_data.c | 353 +++ .../software/core/mllite/linux/ml_stored_data.h | 53 + .../software/core/mllite/linux/ml_sysfs_helper.c | 423 +++ .../software/core/mllite/linux/ml_sysfs_helper.h | 36 + .../software/core/mllite/linux/mlos.h | 99 + .../software/core/mllite/linux/mlos_linux.c | 190 ++ .../software/core/mllite/message_layer.c | 59 + .../software/core/mllite/message_layer.h | 35 + .../software/core/mllite/ml_math_func.c | 706 +++++ .../software/core/mllite/ml_math_func.h | 120 + 60xx/libsensors_iio/software/core/mllite/mpl.c | 72 + 60xx/libsensors_iio/software/core/mllite/mpl.h | 24 + .../software/core/mllite/results_holder.c | 522 ++++ .../software/core/mllite/results_holder.h | 81 + .../software/core/mllite/start_manager.c | 105 + .../software/core/mllite/start_manager.h | 27 + .../software/core/mllite/storage_manager.c | 204 ++ .../software/core/mllite/storage_manager.h | 30 + .../software/core/mpl/accel_auto_cal.h | 38 + .../software/core/mpl/authenticate.h | 21 + .../software/core/mpl/build/android/libmplmpu.so | Bin 0 -> 168684 bytes .../software/core/mpl/build/android/shared.mk | 88 + .../software/core/mpl/build/filelist.mk | 46 + .../software/core/mpl/compass_bias_w_gyro.h | 31 + .../libsensors_iio/software/core/mpl/compass_fit.h | 28 + .../software/core/mpl/compass_vec_cal.h | 34 + .../software/core/mpl/fast_no_motion.h | 46 + .../software/core/mpl/fusion_9axis.h | 37 + 60xx/libsensors_iio/software/core/mpl/gyro_tc.h | 43 + .../software/core/mpl/heading_from_gyro.h | 33 + 60xx/libsensors_iio/software/core/mpl/inv_math.h | 8 + .../software/core/mpl/invensense_adv.h | 31 + .../libsensors_iio/software/core/mpl/mag_disturb.h | 37 + .../software/core/mpl/motion_no_motion.h | 28 + .../software/core/mpl/no_gyro_fusion.h | 34 + .../software/core/mpl/quat_accuracy_monitor.h | 71 + .../software/core/mpl/quaternion_supervisor.h | 27 + 60xx/libsensors_iio/software/core/mpl/shake.h | 94 + 60xx/mlsdk/Android.mk | 101 + 60xx/mlsdk/mllite/accel.c | 189 ++ 60xx/mlsdk/mllite/accel.h | 57 + 60xx/mlsdk/mllite/compass.c | 579 ++++ 60xx/mlsdk/mllite/compass.h | 92 + 60xx/mlsdk/mllite/dmpDefault.c | 417 +++ 60xx/mlsdk/mllite/dmpDefault.h | 42 + 60xx/mlsdk/mllite/dmpDefaultMantis.c | 467 ++++ 60xx/mlsdk/mllite/dmpKey.h | 432 +++ 60xx/mlsdk/mllite/dmpconfig.txt | 3 + 60xx/mlsdk/mllite/dmpmap.h | 276 ++ 60xx/mlsdk/mllite/invensense.h | 24 + 60xx/mlsdk/mllite/ml.c | 1885 +++++++++++++ 60xx/mlsdk/mllite/ml.h | 596 +++++ 60xx/mlsdk/mllite/mlBiasNoMotion.c | 393 +++ 60xx/mlsdk/mllite/mlBiasNoMotion.h | 40 + 60xx/mlsdk/mllite/mlFIFO.c | 2265 ++++++++++++++++ 60xx/mlsdk/mllite/mlFIFO.h | 150 ++ 60xx/mlsdk/mllite/mlFIFOHW.c | 328 +++ 60xx/mlsdk/mllite/mlFIFOHW.h | 48 + 60xx/mlsdk/mllite/mlMathFunc.c | 377 +++ 60xx/mlsdk/mllite/mlMathFunc.h | 68 + 60xx/mlsdk/mllite/mlSetGyroBias.c | 198 ++ 60xx/mlsdk/mllite/mlSetGyroBias.h | 49 + 60xx/mlsdk/mllite/ml_mputest.c | 184 ++ 60xx/mlsdk/mllite/ml_mputest.h | 49 + 60xx/mlsdk/mllite/ml_stored_data.c | 1586 +++++++++++ 60xx/mlsdk/mllite/ml_stored_data.h | 62 + 60xx/mlsdk/mllite/mlarray.c | 2524 ++++++++++++++++++ 60xx/mlsdk/mllite/mlarray_legacy.c | 587 +++++ 60xx/mlsdk/mllite/mlcontrol.c | 797 ++++++ 60xx/mlsdk/mllite/mlcontrol.h | 217 ++ 60xx/mlsdk/mllite/mldl.c | 1092 ++++++++ 60xx/mlsdk/mllite/mldl.h | 176 ++ 60xx/mlsdk/mllite/mldl_cfg.h | 336 +++ 60xx/mlsdk/mllite/mldl_cfg_mpu.c | 477 ++++ 60xx/mlsdk/mllite/mldmp.c | 284 ++ 60xx/mlsdk/mllite/mldmp.h | 96 + 60xx/mlsdk/mllite/mlinclude.h | 50 + 60xx/mlsdk/mllite/mlstates.c | 269 ++ 60xx/mlsdk/mllite/mlstates.h | 58 + 60xx/mlsdk/mllite/mlsupervisor.c | 597 +++++ 60xx/mlsdk/mllite/mlsupervisor.h | 71 + 60xx/mlsdk/mllite/pressure.c | 166 ++ 60xx/mlsdk/mllite/pressure.h | 71 + 60xx/mlsdk/mlutils/checksum.c | 16 + 60xx/mlsdk/mlutils/checksum.h | 18 + 60xx/mlsdk/mlutils/mputest.c | 1396 ++++++++++ 60xx/mlsdk/mlutils/mputest.h | 54 + 60xx/mlsdk/mlutils/slave.h | 188 ++ 60xx/mlsdk/platform/include/i2c.h | 133 + 60xx/mlsdk/platform/include/linux/mpu.h | 335 +++ 60xx/mlsdk/platform/include/log.h | 344 +++ 60xx/mlsdk/platform/include/mlmath.h | 107 + 60xx/mlsdk/platform/include/mlos.h | 109 + 60xx/mlsdk/platform/include/mlsl.h | 290 ++ 60xx/mlsdk/platform/include/mltypes.h | 265 ++ 60xx/mlsdk/platform/include/mpu3050.h | 255 ++ 60xx/mlsdk/platform/include/stdint_invensense.h | 51 + 60xx/mlsdk/platform/linux/kernel/mpuirq.h | 41 + 60xx/mlsdk/platform/linux/kernel/slaveirq.h | 35 + 60xx/mlsdk/platform/linux/kernel/timerirq.h | 29 + 60xx/mlsdk/platform/linux/log_linux.c | 114 + 60xx/mlsdk/platform/linux/log_printf_linux.c | 43 + 60xx/mlsdk/platform/linux/mlos_linux.c | 206 ++ 60xx/mlsdk/platform/linux/mlsl_linux_mpu.c | 497 ++++ Android.mk | 9 +- 149 files changed, 37957 insertions(+), 1 deletion(-) create mode 100644 60xx/Android.mk create mode 100644 60xx/libsensors/Android.mk create mode 100644 60xx/libsensors/MPLSensor.cpp create mode 100644 60xx/libsensors/MPLSensor.h create mode 100644 60xx/libsensors/SensorBase.cpp create mode 100644 60xx/libsensors/SensorBase.h create mode 100644 60xx/libsensors/sensor_params.h create mode 100644 60xx/libsensors/sensors.h create mode 100644 60xx/libsensors_iio/Android.mk create mode 100644 60xx/libsensors_iio/CompassSensor.IIO.9150.cpp create mode 100644 60xx/libsensors_iio/CompassSensor.IIO.9150.h create mode 100644 60xx/libsensors_iio/InputEventReader.cpp create mode 100644 60xx/libsensors_iio/InputEventReader.h create mode 100644 60xx/libsensors_iio/License.txt create mode 100644 60xx/libsensors_iio/MPLSensor.cpp create mode 100644 60xx/libsensors_iio/MPLSensor.h create mode 100644 60xx/libsensors_iio/MPLSupport.cpp create mode 100644 60xx/libsensors_iio/MPLSupport.h create mode 100644 60xx/libsensors_iio/SensorBase.cpp create mode 100644 60xx/libsensors_iio/SensorBase.h create mode 100755 60xx/libsensors_iio/libmllite.so create mode 100644 60xx/libsensors_iio/libmplmpu.so create mode 100644 60xx/libsensors_iio/local_log_def.h create mode 100644 60xx/libsensors_iio/sensor_params.h create mode 100644 60xx/libsensors_iio/sensors.h create mode 100644 60xx/libsensors_iio/sensors_mpl.cpp create mode 100644 60xx/libsensors_iio/software/build/android/common.mk create mode 100644 60xx/libsensors_iio/software/build/android/shared.mk create mode 100644 60xx/libsensors_iio/software/core/driver/include/log.h create mode 100644 60xx/libsensors_iio/software/core/driver/include/mlinclude.h create mode 100644 60xx/libsensors_iio/software/core/driver/include/mlmath.h create mode 100644 60xx/libsensors_iio/software/core/driver/include/mlsl.h create mode 100644 60xx/libsensors_iio/software/core/driver/include/mltypes.h create mode 100644 60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h create mode 100755 60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so create mode 100644 60xx/libsensors_iio/software/core/mllite/build/android/shared.mk create mode 100644 60xx/libsensors_iio/software/core/mllite/build/filelist.mk create mode 100644 60xx/libsensors_iio/software/core/mllite/data_builder.c create mode 100644 60xx/libsensors_iio/software/core/mllite/data_builder.h create mode 100644 60xx/libsensors_iio/software/core/mllite/hal_outputs.c create mode 100644 60xx/libsensors_iio/software/core/mllite/hal_outputs.h create mode 100644 60xx/libsensors_iio/software/core/mllite/invensense.h create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/mlos.h create mode 100644 60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c create mode 100644 60xx/libsensors_iio/software/core/mllite/message_layer.c create mode 100644 60xx/libsensors_iio/software/core/mllite/message_layer.h create mode 100644 60xx/libsensors_iio/software/core/mllite/ml_math_func.c create mode 100644 60xx/libsensors_iio/software/core/mllite/ml_math_func.h create mode 100644 60xx/libsensors_iio/software/core/mllite/mpl.c create mode 100644 60xx/libsensors_iio/software/core/mllite/mpl.h create mode 100644 60xx/libsensors_iio/software/core/mllite/results_holder.c create mode 100644 60xx/libsensors_iio/software/core/mllite/results_holder.h create mode 100644 60xx/libsensors_iio/software/core/mllite/start_manager.c create mode 100644 60xx/libsensors_iio/software/core/mllite/start_manager.h create mode 100644 60xx/libsensors_iio/software/core/mllite/storage_manager.c create mode 100644 60xx/libsensors_iio/software/core/mllite/storage_manager.h create mode 100644 60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h create mode 100644 60xx/libsensors_iio/software/core/mpl/authenticate.h create mode 100644 60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so create mode 100644 60xx/libsensors_iio/software/core/mpl/build/android/shared.mk create mode 100644 60xx/libsensors_iio/software/core/mpl/build/filelist.mk create mode 100644 60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h create mode 100644 60xx/libsensors_iio/software/core/mpl/compass_fit.h create mode 100644 60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h create mode 100644 60xx/libsensors_iio/software/core/mpl/fast_no_motion.h create mode 100644 60xx/libsensors_iio/software/core/mpl/fusion_9axis.h create mode 100644 60xx/libsensors_iio/software/core/mpl/gyro_tc.h create mode 100644 60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h create mode 100644 60xx/libsensors_iio/software/core/mpl/inv_math.h create mode 100644 60xx/libsensors_iio/software/core/mpl/invensense_adv.h create mode 100644 60xx/libsensors_iio/software/core/mpl/mag_disturb.h create mode 100644 60xx/libsensors_iio/software/core/mpl/motion_no_motion.h create mode 100644 60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h create mode 100644 60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h create mode 100644 60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h create mode 100644 60xx/libsensors_iio/software/core/mpl/shake.h create mode 100644 60xx/mlsdk/Android.mk create mode 100644 60xx/mlsdk/mllite/accel.c create mode 100644 60xx/mlsdk/mllite/accel.h create mode 100644 60xx/mlsdk/mllite/compass.c create mode 100644 60xx/mlsdk/mllite/compass.h create mode 100644 60xx/mlsdk/mllite/dmpDefault.c create mode 100644 60xx/mlsdk/mllite/dmpDefault.h create mode 100644 60xx/mlsdk/mllite/dmpDefaultMantis.c create mode 100644 60xx/mlsdk/mllite/dmpKey.h create mode 100644 60xx/mlsdk/mllite/dmpconfig.txt create mode 100644 60xx/mlsdk/mllite/dmpmap.h create mode 100644 60xx/mlsdk/mllite/invensense.h create mode 100644 60xx/mlsdk/mllite/ml.c create mode 100644 60xx/mlsdk/mllite/ml.h create mode 100644 60xx/mlsdk/mllite/mlBiasNoMotion.c create mode 100644 60xx/mlsdk/mllite/mlBiasNoMotion.h create mode 100644 60xx/mlsdk/mllite/mlFIFO.c create mode 100644 60xx/mlsdk/mllite/mlFIFO.h create mode 100644 60xx/mlsdk/mllite/mlFIFOHW.c create mode 100644 60xx/mlsdk/mllite/mlFIFOHW.h create mode 100644 60xx/mlsdk/mllite/mlMathFunc.c create mode 100644 60xx/mlsdk/mllite/mlMathFunc.h create mode 100644 60xx/mlsdk/mllite/mlSetGyroBias.c create mode 100644 60xx/mlsdk/mllite/mlSetGyroBias.h create mode 100644 60xx/mlsdk/mllite/ml_mputest.c create mode 100644 60xx/mlsdk/mllite/ml_mputest.h create mode 100644 60xx/mlsdk/mllite/ml_stored_data.c create mode 100644 60xx/mlsdk/mllite/ml_stored_data.h create mode 100644 60xx/mlsdk/mllite/mlarray.c create mode 100644 60xx/mlsdk/mllite/mlarray_legacy.c create mode 100644 60xx/mlsdk/mllite/mlcontrol.c create mode 100644 60xx/mlsdk/mllite/mlcontrol.h create mode 100644 60xx/mlsdk/mllite/mldl.c create mode 100644 60xx/mlsdk/mllite/mldl.h create mode 100644 60xx/mlsdk/mllite/mldl_cfg.h create mode 100644 60xx/mlsdk/mllite/mldl_cfg_mpu.c create mode 100644 60xx/mlsdk/mllite/mldmp.c create mode 100644 60xx/mlsdk/mllite/mldmp.h create mode 100644 60xx/mlsdk/mllite/mlinclude.h create mode 100644 60xx/mlsdk/mllite/mlstates.c create mode 100644 60xx/mlsdk/mllite/mlstates.h create mode 100644 60xx/mlsdk/mllite/mlsupervisor.c create mode 100644 60xx/mlsdk/mllite/mlsupervisor.h create mode 100644 60xx/mlsdk/mllite/pressure.c create mode 100644 60xx/mlsdk/mllite/pressure.h create mode 100644 60xx/mlsdk/mlutils/checksum.c create mode 100644 60xx/mlsdk/mlutils/checksum.h create mode 100644 60xx/mlsdk/mlutils/mputest.c create mode 100644 60xx/mlsdk/mlutils/mputest.h create mode 100644 60xx/mlsdk/mlutils/slave.h create mode 100644 60xx/mlsdk/platform/include/i2c.h create mode 100644 60xx/mlsdk/platform/include/linux/mpu.h create mode 100644 60xx/mlsdk/platform/include/log.h create mode 100644 60xx/mlsdk/platform/include/mlmath.h create mode 100644 60xx/mlsdk/platform/include/mlos.h create mode 100644 60xx/mlsdk/platform/include/mlsl.h create mode 100644 60xx/mlsdk/platform/include/mltypes.h create mode 100644 60xx/mlsdk/platform/include/mpu3050.h create mode 100644 60xx/mlsdk/platform/include/stdint_invensense.h create mode 100644 60xx/mlsdk/platform/linux/kernel/mpuirq.h create mode 100644 60xx/mlsdk/platform/linux/kernel/slaveirq.h create mode 100644 60xx/mlsdk/platform/linux/kernel/timerirq.h create mode 100644 60xx/mlsdk/platform/linux/log_linux.c create mode 100644 60xx/mlsdk/platform/linux/log_printf_linux.c create mode 100644 60xx/mlsdk/platform/linux/mlos_linux.c create mode 100644 60xx/mlsdk/platform/linux/mlsl_linux_mpu.c diff --git a/60xx/Android.mk b/60xx/Android.mk new file mode 100644 index 0000000..427bbb4 --- /dev/null +++ b/60xx/Android.mk @@ -0,0 +1,8 @@ +# Can't have both manta and non-manta libsensors. +ifneq ($(filter manta, $(TARGET_DEVICE)),) +# libsensors_iio expects IIO drivers for an MPU6050+AK8963 which are only available on manta. +include $(call all-named-subdir-makefiles,libsensors_iio) +else +# libsensors expects an non-IIO MPU3050. +include $(call all-named-subdir-makefiles,mlsdk libsensors) +endif diff --git a/60xx/libsensors/Android.mk b/60xx/libsensors/Android.mk new file mode 100644 index 0000000..326e096 --- /dev/null +++ b/60xx/libsensors/Android.mk @@ -0,0 +1,47 @@ +# Copyright (C) 2008 The Android Open Source Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# Modified 2011 by InvenSense, Inc + + +LOCAL_PATH := $(call my-dir) + +ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false) + +# InvenSense fragment of the HAL +include $(CLEAR_VARS) + +LOCAL_MODULE := libinvensense_hal + +LOCAL_MODULE_TAGS := optional + +LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall +LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050=1 + +LOCAL_SRC_FILES := SensorBase.cpp MPLSensor.cpp + +LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include +LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/include/linux +LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/platform/linux +LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mllite +LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/mldmp +LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/aichi +LOCAL_C_INCLUDES += hardware/invensense/60xx/mlsdk/external/akmd + +LOCAL_SHARED_LIBRARIES := liblog libcutils libutils libdl libmllite libmlplatform +LOCAL_CPPFLAGS+=-DLINUX=1 +LOCAL_LDFLAGS:=-rdynamic + +include $(BUILD_SHARED_LIBRARY) + +endif diff --git a/60xx/libsensors/MPLSensor.cpp b/60xx/libsensors/MPLSensor.cpp new file mode 100644 index 0000000..4676d0e --- /dev/null +++ b/60xx/libsensors/MPLSensor.cpp @@ -0,0 +1,1304 @@ +/* + * Copyright (C) 2011 Invensense, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +//#define LOG_NDEBUG 0 +//see also the EXTRA_VERBOSE define, below + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "MPLSensor.h" + +#include "math.h" +#include "ml.h" +#include "mlFIFO.h" +#include "mlsl.h" +#include "mlos.h" +#include "ml_mputest.h" +#include "ml_stored_data.h" +#include "mldl_cfg.h" +#include "mldl.h" + +#include "mpu.h" +#include "accel.h" +#include "compass.h" +#include "kernel/timerirq.h" +#include "kernel/mpuirq.h" +#include "kernel/slaveirq.h" + +extern "C" { +#include "mlsupervisor.h" +} + +#include "mlcontrol.h" +#include "sensor_params.h" + +#define EXTRA_VERBOSE (0) +//#define FUNC_LOG ALOGV("%s", __PRETTY_FUNCTION__) +#define FUNC_LOG +#define VFUNC_LOG ALOGV_IF(EXTRA_VERBOSE, "%s", __PRETTY_FUNCTION__) +/* this mask must turn on only the sensors that are present and managed by the MPL */ +#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_COMPASS | INV_THREE_AXIS_GYRO) + +#define CALL_MEMBER_FN(pobject,ptrToMember) ((pobject)->*(ptrToMember)) + +/******************************************/ + +/* Base values for the sensor list, these need to be in the order defined in MPLSensor.h */ +static struct sensor_t sSensorList[] = + { { "MPL Gyroscope", "Invensense", 1, + SENSORS_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, + { "MPL Accelerometer", "Invensense", 1, + SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, + { "MPL Magnetic Field", "Invensense", 1, + SENSORS_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, + { "MPL Orientation", "Invensense", 1, + SENSORS_ORIENTATION_HANDLE, + SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, { } }, + { "MPL Rotation Vector", "Invensense", 1, + SENSORS_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, + { "MPL Linear Acceleration", "Invensense", 1, + SENSORS_LINEAR_ACCEL_HANDLE, + SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, + { "MPL Gravity", "Invensense", 1, + SENSORS_GRAVITY_HANDLE, + SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, { } }, +}; + +static unsigned long long irq_timestamp = 0; +/* *************************************************************************** + * MPL interface misc. + */ +//static pointer to the object that will handle callbacks +static MPLSensor* gMPLSensor = NULL; + +/* we need to pass some callbacks to the MPL. The mpl is a C library, so + * wrappers for the C++ callback implementations are required. + */ +extern "C" { +//callback wrappers go here +void mot_cb_wrapper(uint16_t val) +{ + if (gMPLSensor) { + gMPLSensor->cbOnMotion(val); + } +} + +void procData_cb_wrapper() +{ + if(gMPLSensor) { + gMPLSensor->cbProcData(); + } +} + +} //end of extern C + +void setCallbackObject(MPLSensor* gbpt) +{ + gMPLSensor = gbpt; +} + + +/***************************************************************************** + * sensor class implementation + */ + +#define GY_ENABLED ((1< 0) { + irq_set[i] = true; + irq_timestamp = irqdata.irqtime; + } + } + mPollFds[i].revents = 0; + } +} + +/* set the power states of the various sensors based on the bits set in the + * enabled_sensors parameter. + * this function modifies globalish state variables. It must be called with the mMplMutex held. */ +void MPLSensor::setPowerStates(int enabled_sensors) +{ + FUNC_LOG; + bool irq_set[5] = { false, false, false, false, false }; + + do { + + if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { + mLocalSensorMask = ALL_MPL_SENSORS_NP; + break; + } + + if (!A_ENABLED && !M_ENABLED && !GY_ENABLED) { + mLocalSensorMask = 0; + break; + } + + if (GY_ENABLED) { + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } else { + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + + if (A_ENABLED) { + mLocalSensorMask |= (INV_THREE_AXIS_ACCEL); + } else { + mLocalSensorMask &= ~(INV_THREE_AXIS_ACCEL); + } + + if (M_ENABLED) { + mLocalSensorMask |= INV_THREE_AXIS_COMPASS; + } else { + mLocalSensorMask &= ~(INV_THREE_AXIS_COMPASS); + } + + } while (0); + + //record the new sensor state + inv_error_t rv; + + unsigned long sen_mask = mLocalSensorMask & mMasterSensorMask; + + bool changing_sensors = ((inv_get_dl_config()->requested_sensors + != sen_mask) && (sen_mask != 0)); + bool restart = (!mDmpStarted) && (sen_mask != 0); + + if (changing_sensors || restart) { + + ALOGV_IF(EXTRA_VERBOSE, "cs:%d rs:%d ", changing_sensors, restart); + + if (mDmpStarted) { + inv_dmp_stop(); + clearIrqData(irq_set); + mDmpStarted = false; + } + + if (sen_mask != inv_get_dl_config()->requested_sensors) { + rv = inv_set_mpu_sensors(sen_mask); + ALOGE_IF(rv != INV_SUCCESS, + "error: unable to set MPL sensor power states (sens=%ld retcode = %d)", + sen_mask, rv); + } + + if (((mUsetimerIrqCompass && (sen_mask == INV_THREE_AXIS_COMPASS)) + || (mUseTimerIrqAccel && (sen_mask & INV_THREE_AXIS_ACCEL))) + && ((sen_mask & INV_DMP_PROCESSOR) == 0)) { + ALOGV_IF(EXTRA_VERBOSE, "Allowing TimerIRQ"); + mUseTimerirq = true; + } else { + if (mUseTimerirq) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); + clearIrqData(irq_set); + } + ALOGV_IF(EXTRA_VERBOSE, "Not allowing TimerIRQ"); + mUseTimerirq = false; + } + + if (!mDmpStarted) { + if (mHaveGoodMpuCal || mHaveGoodCompassCal) { + rv = inv_store_calibration(); + ALOGE_IF(rv != INV_SUCCESS, + "error: unable to store MPL calibration file"); + mHaveGoodMpuCal = false; + mHaveGoodCompassCal = false; + } + rv = inv_dmp_start(); + ALOGE_IF(rv != INV_SUCCESS, "unable to start dmp"); + mDmpStarted = true; + } + } + + //check if we should stop the DMP + if (mDmpStarted && (sen_mask == 0)) { + rv = inv_dmp_stop(); + ALOGE_IF(rv != INV_SUCCESS, "error: unable to stop DMP (retcode = %d)", + rv); + if (mUseTimerirq) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); + } + clearIrqData(irq_set); + + mDmpStarted = false; + mPollTime = -1; + mCurFifoRate = -1; + } + +} + +/** + * container function for all the calls we make once to set up the MPL. + */ +void MPLSensor::initMPL() +{ + FUNC_LOG; + inv_error_t result; + unsigned short bias_update_mask = 0xFFFF; + + if (inv_dmp_open() != INV_SUCCESS) { + ALOGE("Fatal Error : could not open DMP correctly.\n"); + } + + result = inv_set_mpu_sensors(ALL_MPL_SENSORS_NP); //default to all sensors, also makes 9axis enable work + ALOGE_IF(result != INV_SUCCESS, + "Fatal Error : could not set enabled sensors."); + + if (inv_load_calibration() != INV_SUCCESS) { + ALOGE("could not open MPL calibration file"); + } + + //check for the 9axis fusion library: if available load it and start 9x + void* h_dmp_lib=dlopen("libinvensense_mpl.so", RTLD_NOW); + if(h_dmp_lib) { + const char* error; + error = dlerror(); + inv_error_t (*fp_inv_enable_9x_fusion)() = + (inv_error_t(*)()) dlsym(h_dmp_lib, "inv_enable_9x_fusion"); + if((error = dlerror()) != NULL) { + ALOGE("%s %s", error, "inv_enable_9x_fusion"); + } else if ((*fp_inv_enable_9x_fusion)() != INV_SUCCESS) { + ALOGE( "Warning : 9 axis sensor fusion not available " + "- No compass detected.\n"); + } else { + /* 9axis is loaded and enabled */ + /* this variable is used for coming up with sensor list */ + mNineAxisEnabled = true; + } + } else { + const char* error = dlerror(); + ALOGE("libinvensense_mpl.so not found, 9x sensor fusion disabled (%s)",error); + } + + if (inv_set_bias_update(bias_update_mask) != INV_SUCCESS) { + ALOGE("Error : Bias update function could not be set.\n"); + } + + if (inv_set_motion_interrupt(1) != INV_SUCCESS) { + ALOGE("Error : could not set motion interrupt"); + } + + if (inv_set_fifo_interrupt(1) != INV_SUCCESS) { + ALOGE("Error : could not set fifo interrupt"); + } + + result = inv_set_fifo_rate(6); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_set_fifo_rate returned %d\n", result); + } + + setupCallbacks(); + +} + +/** setup the fifo contents. + */ +void MPLSensor::setupFIFO() +{ + FUNC_LOG; + inv_error_t result; + + result = inv_send_accel(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_accel returned %d\n", result); + } + + result = inv_send_quaternion(INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_quaternion returned %d\n", result); + } + + result = inv_send_linear_accel(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_linear_accel returned %d\n", result); + } + + result = inv_send_linear_accel_in_world(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_linear_accel_in_world returned %d\n", + result); + } + + result = inv_send_gravity(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_gravity returned %d\n", result); + } + + result = inv_send_gyro(INV_ALL, INV_32_BIT); + if (result != INV_SUCCESS) { + ALOGE("Fatal error: inv_send_gyro returned %d\n", result); + } + +} + +/** + * set up the callbacks that we use in all cases (outside of gestures, etc) + */ +void MPLSensor::setupCallbacks() +{ + FUNC_LOG; + if (inv_set_motion_callback(mot_cb_wrapper) != INV_SUCCESS) { + ALOGE("Error : Motion callback could not be set.\n"); + + } + + if (inv_set_fifo_processed_callback(procData_cb_wrapper) != INV_SUCCESS) { + ALOGE("Error : Processed data callback could not be set."); + + } +} + +/** + * handle the motion/no motion output from the MPL. + */ +void MPLSensor::cbOnMotion(uint16_t val) +{ + FUNC_LOG; + //after the first no motion, the gyro should be calibrated well + if (val == 2) { + if ((inv_get_dl_config()->requested_sensors) & INV_THREE_AXIS_GYRO) { + //if gyros are on and we got a no motion, set a flag + // indicating that the cal file can be written. + mHaveGoodMpuCal = true; + } + } + + return; +} + + +void MPLSensor::cbProcData() +{ + mNewData = 1; + mSampleCount++; +} + +//these handlers transform mpl data into one of the Android sensor types +// scaling and coordinate transforms should be done in the handlers + +void MPLSensor::gyroHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_GYROS, s->gyro.v); + s->gyro.v[0] = s->gyro.v[0] * M_PI / 180.0; + s->gyro.v[1] = s->gyro.v[1] * M_PI / 180.0; + s->gyro.v[2] = s->gyro.v[2] * M_PI / 180.0; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::accelHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + //VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_ACCELS, s->acceleration.v); + //res = inv_get_accel_float(s->acceleration.v); + s->acceleration.v[0] = s->acceleration.v[0] * 9.81; + s->acceleration.v[1] = s->acceleration.v[1] * 9.81; + s->acceleration.v[2] = s->acceleration.v[2] * 9.81; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +int MPLSensor::estimateCompassAccuracy() +{ + inv_error_t res; + int rv; + + res = inv_get_compass_accuracy(&rv); + if(rv >= SENSOR_STATUS_ACCURACY_MEDIUM) { + mHaveGoodCompassCal = true; + } + ALOGE_IF(res != INV_SUCCESS, "error returned from inv_get_compass_accuracy"); + + return rv; +} + +void MPLSensor::compassHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + + res = inv_get_float_array(INV_MAGNETOMETER, s->magnetic.v); + + if (res != INV_SUCCESS) { + ALOGW( + "compass_handler inv_get_float_array(INV_MAGNETOMETER) returned %d", + res); + } + + s->magnetic.status = estimateCompassAccuracy(); + + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::rvHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + float quat[4]; + float norm = 0; + inv_error_t r; + + r = inv_get_float_array(INV_QUATERNION, quat); + + if (r != INV_SUCCESS) { + *pending_mask &= ~(1 << index); + return; + } else { + *pending_mask |= (1 << index); + } + + norm = quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3] + + FLT_EPSILON; + + if (norm > 1.0f) { + //renormalize + norm = sqrtf(norm); + float inv_norm = 1.0f / norm; + quat[1] = quat[1] * inv_norm; + quat[2] = quat[2] * inv_norm; + quat[3] = quat[3] * inv_norm; + } + + if (quat[0] < 0.0) { + quat[1] = -quat[1]; + quat[2] = -quat[2]; + quat[3] = -quat[3]; + } + + s->gyro.v[0] = quat[1]; + s->gyro.v[1] = quat[2]; + s->gyro.v[2] = quat[3]; + +} + +void MPLSensor::laHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_LINEAR_ACCELERATION, s->gyro.v); + s->gyro.v[0] *= 9.81; + s->gyro.v[1] *= 9.81; + s->gyro.v[2] *= 9.81; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::gravHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) +{ + VFUNC_LOG; + inv_error_t res; + res = inv_get_float_array(INV_GRAVITY, s->gyro.v); + s->gyro.v[0] *= 9.81; + s->gyro.v[1] *= 9.81; + s->gyro.v[2] *= 9.81; + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); +} + +void MPLSensor::calcOrientationSensor(float *R, float *values) +{ + float tmp; + + //Azimuth + if ((R[7] > 0.7071067f) || ((R[8] < 0) && (fabs(R[7]) > fabs(R[6])))) { + values[0] = (float) atan2f(-R[3], R[0]); + } else { + values[0] = (float) atan2f(R[1], R[4]); + } + values[0] *= 57.295779513082320876798154814105f; + if (values[0] < 0) { + values[0] += 360.0f; + } + //Pitch + tmp = R[7]; + if (tmp > 1.0f) + tmp = 1.0f; + if (tmp < -1.0f) + tmp = -1.0f; + values[1] = -asinf(tmp) * 57.295779513082320876798154814105f; + if (R[8] < 0) { + values[1] = 180.0f - values[1]; + } + if (values[1] > 180.0f) { + values[1] -= 360.0f; + } + //Roll + if ((R[7] > 0.7071067f)) { + values[2] = (float) atan2f(R[6], R[7]); + } else { + values[2] = (float) atan2f(R[6], R[8]); + } + + values[2] *= 57.295779513082320876798154814105f; + if (values[2] > 90.0f) { + values[2] = 180.0f - values[2]; + } + if (values[2] < -90.0f) { + values[2] = -180.0f - values[2]; + } +} + +void MPLSensor::orienHandler(sensors_event_t* s, uint32_t* pending_mask, + int index) //note that this is the handler for the android 'orientation' sensor, not the mpl orientation output +{ + VFUNC_LOG; + inv_error_t res; + float rot_mat[9]; + + res = inv_get_float_array(INV_ROTATION_MATRIX, rot_mat); + + calcOrientationSensor(rot_mat, s->orientation.v); + + s->orientation.status = estimateCompassAccuracy(); + + if (res == INV_SUCCESS) + *pending_mask |= (1 << index); + else + ALOGW("orienHandler: data not valid (%d)", (int) res); + +} + +int MPLSensor::enable(int32_t handle, int en) +{ + FUNC_LOG; + + int what = -1; + + switch (handle) { + case ID_A: + what = Accelerometer; + break; + case ID_M: + what = MagneticField; + break; + case ID_O: + what = Orientation; + break; + case ID_GY: + what = Gyro; + break; + case ID_GR: + what = Gravity; + break; + case ID_RV: + what = RotationVector; + break; + case ID_LA: + what = LinearAccel; + break; + default: //this takes care of all the gestures + what = handle; + break; + } + + if (uint32_t(what) >= numSensors) + return -EINVAL; + + int newState = en ? 1 : 0; + int err = 0; + + pthread_mutex_lock(&mMplMutex); + if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { + short flags = newState; + mEnabled &= ~(1 << what); + mEnabled |= (uint32_t(flags) << what); + ALOGV_IF(EXTRA_VERBOSE, "mEnabled = %x", mEnabled); + setPowerStates(mEnabled); + pthread_mutex_unlock(&mMplMutex); + if (!newState) + update_delay(); + return err; + } + pthread_mutex_unlock(&mMplMutex); + return err; +} + +int MPLSensor::setDelay(int32_t handle, int64_t ns) +{ + FUNC_LOG; + ALOGV_IF(EXTRA_VERBOSE, + " setDelay handle: %d rate %d", handle, (int) (ns / 1000000LL)); + int what = -1; + switch (handle) { + case ID_A: + what = Accelerometer; + break; + case ID_M: + what = MagneticField; + break; + case ID_O: + what = Orientation; + break; + case ID_GY: + what = Gyro; + break; + case ID_GR: + what = Gravity; + break; + case ID_RV: + what = RotationVector; + break; + case ID_LA: + what = LinearAccel; + break; + default: + what = handle; + break; + } + + if (uint32_t(what) >= numSensors) + return -EINVAL; + + if (ns < 0) + return -EINVAL; + + pthread_mutex_lock(&mMplMutex); + mDelays[what] = ns; + pthread_mutex_unlock(&mMplMutex); + return update_delay(); +} + +int MPLSensor::update_delay() +{ + FUNC_LOG; + int rv = 0; + bool irq_set[5]; + + pthread_mutex_lock(&mMplMutex); + + if (mEnabled) { + uint64_t wanted = -1LLU; + for (int i = 0; i < numSensors; i++) { + if (mEnabled & (1 << i)) { + uint64_t ns = mDelays[i]; + wanted = wanted < ns ? wanted : ns; + } + } + + //Limit all rates to 100Hz max. 100Hz = 10ms = 10000000ns + if (wanted < 10000000LLU) { + wanted = 10000000LLU; + } + + int rate = ((wanted) / 5000000LLU) - ((wanted % 5000000LLU == 0) ? 1 + : 0); //mpu fifo rate is in increments of 5ms + if (rate == 0) //KLP disallow fifo rate 0 + rate = 1; + + if (rate != mCurFifoRate) { + inv_error_t res; // = inv_dmp_stop(); + res = inv_set_fifo_rate(rate); + ALOGE_IF(res != INV_SUCCESS, "error setting FIFO rate"); + + mCurFifoRate = rate; + rv = (res == INV_SUCCESS); + } + + if (((inv_get_dl_config()->requested_sensors & INV_DMP_PROCESSOR) == 0)) { + if (mUseTimerirq) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_STOP, 0); + clearIrqData(irq_set); + if (inv_get_dl_config()->requested_sensors + == INV_THREE_AXIS_COMPASS) { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, + (unsigned long) (wanted / 1000000LLU)); + ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", + (int) (wanted / 1000000LLU)); + } else { + ioctl(mIrqFds.valueFor(TIMERIRQ_FD), TIMERIRQ_START, + (unsigned long) inv_get_sample_step_size_ms()); + ALOGV_IF(EXTRA_VERBOSE, "updated timerirq period to %d", + (int) inv_get_sample_step_size_ms()); + } + } + } + + } + pthread_mutex_unlock(&mMplMutex); + return rv; +} + +/* return the current time in nanoseconds */ +int64_t MPLSensor::now_ns(void) +{ + //FUNC_LOG; + struct timespec ts; + + clock_gettime(CLOCK_MONOTONIC, &ts); + return (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; +} + +int MPLSensor::readEvents(sensors_event_t* data, int count) +{ + //VFUNC_LOG; + bool irq_set[5] = { false, false, false, false, false }; + inv_error_t rv; + if (count < 1) + return -EINVAL; + int numEventReceived = 0; + + clearIrqData(irq_set); + + pthread_mutex_lock(&mMplMutex); + if (mDmpStarted) { + rv = inv_update_data(); + ALOGE_IF(rv != INV_SUCCESS, "inv_update_data error (code %d)", (int) rv); + } + + else { + //probably just one extra read after shutting down + ALOGV_IF(EXTRA_VERBOSE, + "MPLSensor::readEvents called, but there's nothing to do."); + } + + pthread_mutex_unlock(&mMplMutex); + + if (!mNewData) { + ALOGV_IF(EXTRA_VERBOSE, "no new data"); + return 0; + } + mNewData = 0; + + /* google timestamp */ + pthread_mutex_lock(&mMplMutex); + for (int i = 0; i < numSensors; i++) { + if (mEnabled & (1 << i)) { + CALL_MEMBER_FN(this,mHandlers[i])(mPendingEvents + i, + &mPendingMask, i); + mPendingEvents[i].timestamp = irq_timestamp; + } + } + + for (int j = 0; count && mPendingMask && j < numSensors; j++) { + if (mPendingMask & (1 << j)) { + mPendingMask &= ~(1 << j); + if (mEnabled & (1 << j)) { + *data++ = mPendingEvents[j]; + count--; + numEventReceived++; + } + } + + } + + pthread_mutex_unlock(&mMplMutex); + return numEventReceived; +} + +int MPLSensor::getFd() const +{ + return data_fd; +} + +int MPLSensor::getAccelFd() const +{ + return accel_fd; +} + +int MPLSensor::getTimerFd() const +{ + return timer_fd; +} + +int MPLSensor::getPowerFd() const +{ + int hdl = (uintptr_t) inv_get_serial_handle(); + return hdl; +} + +int MPLSensor::getPollTime() +{ + return mPollTime; +} + +bool MPLSensor::hasPendingEvents() const +{ + //if we are using the polling workaround, force the main loop to check for data every time + return (mPollTime != -1); +} + +void MPLSensor::handlePowerEvent() +{ + VFUNC_LOG; + mpuirq_data irqd; + + int fd = (uintptr_t) inv_get_serial_handle(); + read(fd, &irqd, sizeof(irqd)); + + if (irqd.data == MPU_PM_EVENT_SUSPEND_PREPARE) { + //going to sleep + sleepEvent(); + } else if (irqd.data == MPU_PM_EVENT_POST_SUSPEND) { + //waking up + wakeEvent(); + } + + ioctl(fd, MPU_PM_EVENT_HANDLED, 0); +} + +void MPLSensor::sleepEvent() +{ + VFUNC_LOG; + pthread_mutex_lock(&mMplMutex); + if (mEnabled != 0) { + mForceSleep = true; + mOldEnabledMask = mEnabled; + setPowerStates(0); + } + pthread_mutex_unlock(&mMplMutex); +} + +void MPLSensor::wakeEvent() +{ + VFUNC_LOG; + pthread_mutex_lock(&mMplMutex); + if (mForceSleep) { + setPowerStates((mOldEnabledMask | mEnabled)); + } + mForceSleep = false; + pthread_mutex_unlock(&mMplMutex); +} + +/** fill in the sensor list based on which sensors are configured. + * return the number of configured sensors. + * parameter list must point to a memory region of at least 7*sizeof(sensor_t) + * parameter len gives the length of the buffer pointed to by list + */ + +int MPLSensor::populateSensorList(struct sensor_t *list, size_t len) +{ + int numsensors; + + if(len < 7*sizeof(sensor_t)) { + ALOGE("sensor list too small, not populating."); + return 0; + } + + /* fill in the base values */ + memcpy(list, sSensorList, sizeof (struct sensor_t) * 7); + + /* first add gyro, accel and compass to the list */ + + /* fill in accel values */ + unsigned short accelId = inv_get_accel_id(); + if(accelId == 0) { + ALOGE("Can not get accel id"); + } + fillAccel(accelId, list); + + /* fill in compass values */ + unsigned short compassId = inv_get_compass_id(); + if(compassId == 0) { + ALOGE("Can not get compass id"); + } + fillCompass(compassId, list); + + /* fill in gyro values */ + fillGyro(MPU_NAME, list); + + if(mNineAxisEnabled) + { + numsensors = 7; + /* all sensors will be added to the list */ + /* fill in orientation values */ + fillOrientation(list); + + /* fill in rotation vector values */ + fillRV(list); + + /* fill in gravity values */ + fillGravity(list); + + /* fill in Linear accel values */ + fillLinearAccel(list); + } else { + /* no 9-axis sensors, zero fill that part of the list */ + numsensors = 3; + memset(list+3, 0, 4*sizeof(struct sensor_t)); + } + + return numsensors; +} + +void MPLSensor::fillAccel(unsigned char accel, struct sensor_t *list) +{ + switch (accel) { + case ACCEL_ID_LIS331: + list[Accelerometer].maxRange = ACCEL_LIS331_RANGE; + list[Accelerometer].resolution = ACCEL_LIS331_RESOLUTION; + list[Accelerometer].power = ACCEL_LIS331_POWER; + break; + + case ACCEL_ID_LIS3DH: + list[Accelerometer].maxRange = ACCEL_LIS3DH_RANGE; + list[Accelerometer].resolution = ACCEL_LIS3DH_RESOLUTION; + list[Accelerometer].power = ACCEL_LIS3DH_POWER; + break; + + case ACCEL_ID_KXSD9: + list[Accelerometer].maxRange = ACCEL_KXSD9_RANGE; + list[Accelerometer].resolution = ACCEL_KXSD9_RESOLUTION; + list[Accelerometer].power = ACCEL_KXSD9_POWER; + break; + + case ACCEL_ID_KXTF9: + list[Accelerometer].maxRange = ACCEL_KXTF9_RANGE; + list[Accelerometer].resolution = ACCEL_KXTF9_RESOLUTION; + list[Accelerometer].power = ACCEL_KXTF9_POWER; + break; + + case ACCEL_ID_BMA150: + list[Accelerometer].maxRange = ACCEL_BMA150_RANGE; + list[Accelerometer].resolution = ACCEL_BMA150_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA150_POWER; + break; + + case ACCEL_ID_BMA222: + list[Accelerometer].maxRange = ACCEL_BMA222_RANGE; + list[Accelerometer].resolution = ACCEL_BMA222_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA222_POWER; + break; + + case ACCEL_ID_BMA250: + list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; + list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA250_POWER; + break; + + case ACCEL_ID_ADXL34X: + list[Accelerometer].maxRange = ACCEL_ADXL34X_RANGE; + list[Accelerometer].resolution = ACCEL_ADXL34X_RESOLUTION; + list[Accelerometer].power = ACCEL_ADXL34X_POWER; + break; + + case ACCEL_ID_MMA8450: + list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; + list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; + list[Accelerometer].maxRange = ACCEL_MMA8450_RANGE; + break; + + case ACCEL_ID_MMA845X: + list[Accelerometer].maxRange = ACCEL_MMA845X_RANGE; + list[Accelerometer].resolution = ACCEL_MMA845X_RESOLUTION; + list[Accelerometer].power = ACCEL_MMA845X_POWER; + break; + + case ACCEL_ID_MPU6050: + list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; + list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU6050_POWER; + break; + default: + ALOGE("unknown accel id -- accel params will be wrong."); + break; + } +} + +void MPLSensor::fillCompass(unsigned char compass, struct sensor_t *list) +{ + switch (compass) { + case COMPASS_ID_AK8975: + list[MagneticField].maxRange = COMPASS_AKM8975_RANGE; + list[MagneticField].resolution = COMPASS_AKM8975_RESOLUTION; + list[MagneticField].power = COMPASS_AKM8975_POWER; + break; + case COMPASS_ID_AMI30X: + list[MagneticField].maxRange = COMPASS_AMI30X_RANGE; + list[MagneticField].resolution = COMPASS_AMI30X_RESOLUTION; + list[MagneticField].power = COMPASS_AMI30X_POWER; + break; + case COMPASS_ID_AMI306: + list[MagneticField].maxRange = COMPASS_AMI306_RANGE; + list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; + list[MagneticField].power = COMPASS_AMI306_POWER; + break; + case COMPASS_ID_YAS529: + list[MagneticField].maxRange = COMPASS_YAS529_RANGE; + list[MagneticField].resolution = COMPASS_AMI306_RESOLUTION; + list[MagneticField].power = COMPASS_AMI306_POWER; + break; + case COMPASS_ID_YAS530: + list[MagneticField].maxRange = COMPASS_YAS530_RANGE; + list[MagneticField].resolution = COMPASS_YAS530_RESOLUTION; + list[MagneticField].power = COMPASS_YAS530_POWER; + break; + case COMPASS_ID_HMC5883: + list[MagneticField].maxRange = COMPASS_HMC5883_RANGE; + list[MagneticField].resolution = COMPASS_HMC5883_RESOLUTION; + list[MagneticField].power = COMPASS_HMC5883_POWER; + break; + case COMPASS_ID_MMC314X: + list[MagneticField].maxRange = COMPASS_MMC314X_RANGE; + list[MagneticField].resolution = COMPASS_MMC314X_RESOLUTION; + list[MagneticField].power = COMPASS_MMC314X_POWER; + break; + case COMPASS_ID_HSCDTD002B: + list[MagneticField].maxRange = COMPASS_HSCDTD002B_RANGE; + list[MagneticField].resolution = COMPASS_HSCDTD002B_RESOLUTION; + list[MagneticField].power = COMPASS_HSCDTD002B_POWER; + break; + case COMPASS_ID_HSCDTD004A: + list[MagneticField].maxRange = COMPASS_HSCDTD004A_RANGE; + list[MagneticField].resolution = COMPASS_HSCDTD004A_RESOLUTION; + list[MagneticField].power = COMPASS_HSCDTD004A_POWER; + break; + default: + ALOGE("unknown compass id -- compass parameters will be wrong"); + } +} + +void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) +{ + if ((gyro != NULL) && (strcmp(gyro, "mpu3050") == 0)) { + list[Gyro].maxRange = GYRO_MPU3050_RANGE; + list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; + list[Gyro].power = GYRO_MPU3050_POWER; + } else { + list[Gyro].maxRange = GYRO_MPU6050_RANGE; + list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; + list[Gyro].power = GYRO_MPU6050_POWER; + } + return; +} + + +/* fillRV depends on values of accel and compass in the list */ +void MPLSensor::fillRV(struct sensor_t *list) +{ + /* compute power on the fly */ + list[RotationVector].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[RotationVector].resolution = .00001; + list[RotationVector].maxRange = 1.0; + return; +} + +void MPLSensor::fillOrientation(struct sensor_t *list) +{ + list[Orientation].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[Orientation].resolution = .00001; + list[Orientation].maxRange = 360.0; + return; +} + +void MPLSensor::fillGravity( struct sensor_t *list) +{ + list[Gravity].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[Gravity].resolution = .00001; + list[Gravity].maxRange = 9.81; + return; +} + +void MPLSensor::fillLinearAccel(struct sensor_t *list) +{ + list[Gravity].power = list[Gyro].power + list[Accelerometer].power + + list[MagneticField].power; + list[Gravity].resolution = list[Accelerometer].resolution; + list[Gravity].maxRange = list[Accelerometer].maxRange; + return; +} diff --git a/60xx/libsensors/MPLSensor.h b/60xx/libsensors/MPLSensor.h new file mode 100644 index 0000000..f580732 --- /dev/null +++ b/60xx/libsensors/MPLSensor.h @@ -0,0 +1,142 @@ +/* + * Copyright (C) 2011 Invensense, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +/*************Removed the gesture related info for Google check in : Meenakshi Ramamoorthi: May 31st *********/ + +#ifndef ANDROID_MPL_SENSOR_H +#define ANDROID_MPL_SENSOR_H + +#include +#include +#include +#include +#include +#include +#include +#include "sensors.h" +#include "SensorBase.h" + +/*****************************************************************************/ +/** MPLSensor implementation which fits into the HAL example for crespo provided + * * by Google. + * * WARNING: there may only be one instance of MPLSensor, ever. + */ + +class MPLSensor: public SensorBase +{ + typedef void (MPLSensor::*hfunc_t)(sensors_event_t*, uint32_t*, int); + +public: + MPLSensor(); + virtual ~MPLSensor(); + + enum + { + Gyro=0, + Accelerometer, + MagneticField, + Orientation, + RotationVector, + LinearAccel, + Gravity, + numSensors + }; + + virtual int setDelay(int32_t handle, int64_t ns); + virtual int enable(int32_t handle, int enabled); + virtual int readEvents(sensors_event_t *data, int count); + virtual int getFd() const; + virtual int getAccelFd() const; + virtual int getTimerFd() const; + virtual int getPowerFd() const; + virtual int getPollTime(); + virtual bool hasPendingEvents() const; + virtual void handlePowerEvent(); + virtual void sleepEvent(); + virtual void wakeEvent(); + int populateSensorList(struct sensor_t *list, size_t len); + void cbOnMotion(uint16_t); + void cbProcData(); + +protected: + + void clearIrqData(bool* irq_set); + void setPowerStates(int enabledsensor); + void initMPL(); + void setupFIFO(); + void setupCallbacks(); + void gyroHandler(sensors_event_t *data, uint32_t *pendmask, int index); + void accelHandler(sensors_event_t *data, uint32_t *pendmask, int index); + void compassHandler(sensors_event_t *data, uint32_t *pendmask, int index); + void rvHandler(sensors_event_t *data, uint32_t *pendmask, int index); + void laHandler(sensors_event_t *data, uint32_t *pendmask, int index); + void gravHandler(sensors_event_t *data, uint32_t *pendmask, int index); + void orienHandler(sensors_event_t *data, uint32_t *pendmask, int index); + void calcOrientationSensor(float *Rx, float *Val); + int estimateCompassAccuracy(); + + int mNewData; //flag indicating that the MPL calculated new output values + int mDmpStarted; + long mMasterSensorMask; + long mLocalSensorMask; + int mPollTime; + int mCurFifoRate; //current fifo rate + bool mHaveGoodMpuCal; //flag indicating that the cal file can be written + bool mHaveGoodCompassCal; + bool mUseTimerIrqAccel; + bool mUsetimerIrqCompass; + bool mUseTimerirq; + struct pollfd mPollFds[4]; + int mSampleCount; + pthread_mutex_t mMplMutex; + int64_t now_ns(); + int64_t select_ns(unsigned long long time_set[]); + + enum FILEHANDLES + { + MPUIRQ_FD, ACCELIRQ_FD, COMPASSIRQ_FD, TIMERIRQ_FD, + }; + +private: + + int update_delay(); + int accel_fd; + int timer_fd; + + uint32_t mEnabled; + uint32_t mPendingMask; + sensors_event_t mPendingEvents[numSensors]; + uint64_t mDelays[numSensors]; + hfunc_t mHandlers[numSensors]; + bool mForceSleep; + long int mOldEnabledMask; + android::KeyedVector mIrqFds; + + /* added for dynamic get sensor list */ + bool mNineAxisEnabled; + void fillAccel(unsigned char accel, struct sensor_t *list); + void fillCompass(unsigned char compass, struct sensor_t *list); + void fillGyro(const char* gyro, struct sensor_t *list); + void fillRV(struct sensor_t *list); + void fillOrientation(struct sensor_t *list); + void fillGravity(struct sensor_t *list); + void fillLinearAccel(struct sensor_t *list); +}; + +void setCallbackObject(MPLSensor*); + +/*****************************************************************************/ + +#endif // ANDROID_MPL_SENSOR_H diff --git a/60xx/libsensors/SensorBase.cpp b/60xx/libsensors/SensorBase.cpp new file mode 100644 index 0000000..9cc1ee8 --- /dev/null +++ b/60xx/libsensors/SensorBase.cpp @@ -0,0 +1,129 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include "SensorBase.h" + +/*****************************************************************************/ + +SensorBase::SensorBase( + const char* dev_name, + const char* data_name) + : dev_name(dev_name), data_name(data_name), + dev_fd(-1), data_fd(-1) +{ + if (data_name) { + data_fd = openInput(data_name); + } +} + +SensorBase::~SensorBase() { + if (data_fd >= 0) { + close(data_fd); + } + if (dev_fd >= 0) { + close(dev_fd); + } +} + +int SensorBase::open_device() { + if (dev_fd<0 && dev_name) { + dev_fd = open(dev_name, O_RDONLY); + ALOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno)); + } + return 0; +} + +int SensorBase::close_device() { + if (dev_fd >= 0) { + close(dev_fd); + dev_fd = -1; + } + return 0; +} + +int SensorBase::getFd() const { + if (!data_name) { + return dev_fd; + } + return data_fd; +} + +int SensorBase::setDelay(int32_t handle, int64_t ns) { + return 0; +} + +bool SensorBase::hasPendingEvents() const { + return false; +} + +int64_t SensorBase::getTimestamp() { + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return int64_t(t.tv_sec)*1000000000LL + t.tv_nsec; +} + +int SensorBase::openInput(const char* inputName) { + int fd = -1; + const char *dirname = "/dev/input"; + char devname[PATH_MAX]; + char *filename; + DIR *dir; + struct dirent *de; + dir = opendir(dirname); + if(dir == NULL) + return -1; + strcpy(devname, dirname); + filename = devname + strlen(devname); + *filename++ = '/'; + while((de = readdir(dir))) { + if(de->d_name[0] == '.' && + (de->d_name[1] == '\0' || + (de->d_name[1] == '.' && de->d_name[2] == '\0'))) + continue; + strcpy(filename, de->d_name); + fd = open(devname, O_RDONLY); + if (fd>=0) { + char name[80]; + if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) { + name[0] = '\0'; + } + if (!strcmp(name, inputName)) { + strcpy(input_name, filename); + break; + } else { + close(fd); + fd = -1; + } + } + } + closedir(dir); + ALOGE_IF(fd<0, "couldn't find '%s' input device", inputName); + return fd; +} diff --git a/60xx/libsensors/SensorBase.h b/60xx/libsensors/SensorBase.h new file mode 100644 index 0000000..bb4d055 --- /dev/null +++ b/60xx/libsensors/SensorBase.h @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SENSOR_BASE_H +#define ANDROID_SENSOR_BASE_H + +#include +#include +#include +#include + + +/*****************************************************************************/ + +struct sensors_event_t; + +class SensorBase { +protected: + const char* dev_name; + const char* data_name; + char input_name[PATH_MAX]; + int dev_fd; + int data_fd; + + int openInput(const char* inputName); + static int64_t getTimestamp(); + + + static int64_t timevalToNano(timeval const& t) { + return t.tv_sec*1000000000LL + t.tv_usec*1000; + } + + int open_device(); + int close_device(); + +public: + SensorBase( + const char* dev_name, + const char* data_name); + + virtual ~SensorBase(); + + virtual int readEvents(sensors_event_t* data, int count) = 0; + virtual bool hasPendingEvents() const; + virtual int getFd() const; + virtual int setDelay(int32_t handle, int64_t ns); + virtual int enable(int32_t handle, int enabled) = 0; +}; + +/*****************************************************************************/ + +#endif // ANDROID_SENSOR_BASE_H diff --git a/60xx/libsensors/sensor_params.h b/60xx/libsensors/sensor_params.h new file mode 100644 index 0000000..4925ac4 --- /dev/null +++ b/60xx/libsensors/sensor_params.h @@ -0,0 +1,134 @@ +/* + * Copyright (C) 2011 Invensense, Inc. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef INV_SENSOR_PARAMS_H +#define INV_SENSOR_PARAMS_H + +/* Physical parameters of the sensors supported by Invensense MPL */ +#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV) +#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA) +#define SENSORS_GRAVITY_HANDLE (ID_GR) +#define SENSORS_GYROSCOPE_HANDLE (ID_GY) +#define SENSORS_ACCELERATION_HANDLE (ID_A) +#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M) +#define SENSORS_ORIENTATION_HANDLE (ID_O) +/******************************************/ +//COMPASS_ID_AKM +#define COMPASS_AKM8975_RANGE (9830.0f) +#define COMPASS_AKM8975_RESOLUTION (0.285f) +#define COMPASS_AKM8975_POWER (10.0f) +//COMPASS_ID_AMI30X +#define COMPASS_AMI30X_RANGE (5461.0f) +#define COMPASS_AMI30X_RESOLUTION (0.9f) +#define COMPASS_AMI30X_POWER (0.15f) +//COMPASS_ID_AMI306 +#define COMPASS_AMI306_RANGE (5461.0f) +#define COMPASS_AMI306_RESOLUTION (0.9f) +#define COMPASS_AMI306_POWER (0.15f) +//COMPASS_ID_YAS529 +#define COMPASS_YAS529_RANGE (19660.0f) +#define COMPASS_YAS529_RESOLUTION (0.012f) +#define COMPASS_YAS529_POWER (4.0f) +//COMPASS_ID_YAS530 +#define COMPASS_YAS530_RANGE (8001.0f) +#define COMPASS_YAS530_RESOLUTION (0.012f) +#define COMPASS_YAS530_POWER (4.0f) +//COMPASS_ID_HMC5883 +#define COMPASS_HMC5883_RANGE (10673.0f) +#define COMPASS_HMC5883_RESOLUTION (10.0f) +#define COMPASS_HMC5883_POWER (0.24f) +//COMPASS_ID_LSM303DLH +#define COMPASS_LSM303DLH_RANGE (10240.0f) +#define COMPASS_LSM303DLH_RESOLUTION (1.0f) +#define COMPASS_LSM303DLH_POWER (1.0f) +//COMPASS_ID_LSM303DLM +#define COMPASS_LSM303DLM_RANGE (10240.0f) +#define COMPASS_LSM303DLM_RESOLUTION (1.0f) +#define COMPASS_LSM303DLM_POWER (1.0f) +//COMPASS_ID_MMC314X +#define COMPASS_MMC314X_RANGE (400.0f) +#define COMPASS_MMC314X_RESOLUTION (2.0f) +#define COMPASS_MMC314X_POWER (0.55f) +//COMPASS_ID_HSCDTD002B +#define COMPASS_HSCDTD002B_RANGE (9830.0f) +#define COMPASS_HSCDTD002B_RESOLUTION (1.0f) +#define COMPASS_HSCDTD002B_POWER (1.0f) +//COMPASS_ID_HSCDTD004A +#define COMPASS_HSCDTD004A_RANGE (9830.0f) +#define COMPASS_HSCDTD004A_RESOLUTION (1.0f) +#define COMPASS_HSCDTD004A_POWER (1.0f) +/*******************************************/ +//ACCEL_ID_LIS331 +#define ACCEL_LIS331_RANGE (2.480f*GRAVITY_EARTH) +#define ACCEL_LIS331_RESOLUTION (.001f*GRAVITY_EARTH) +#define ACCEL_LIS331_POWER (1.0f) +//ACCEL_ID_LSM303DLX +#define ACCEL_LSM303DLX_RANGE (2.480f*GRAVITY_EARTH) +#define ACCEL_LSM303DLX_RESOLUTION (0.001f*GRAVITY_EARTH) +#define ACCEL_LSM303DLX_POWER (1.0f) +//ACCEL_ID_LIS3DH +#define ACCEL_LIS3DH_RANGE (2.480f*GRAVITY_EARTH) +#define ACCEL_LIS3DH_RESOLUTION (0.001f*GRAVITY_EARTH) +#define ACCEL_LIS3DH_POWER (1.0f) +//ACCEL_ID_KXSD9 +#define ACCEL_KXSD9_RANGE (2.5006f*GRAVITY_EARTH) +#define ACCEL_KXSD9_RESOLUTION (0.001f*GRAVITY_EARTH) +#define ACCEL_KXSD9_POWER (1.0f) +//ACCEL_ID_KXTF9 +#define ACCEL_KXTF9_RANGE (1.0f*GRAVITY_EARTH) +#define ACCEL_KXTF9_RESOLUTION (0.033f*GRAVITY_EARTH) +#define ACCEL_KXTF9_POWER (0.35f) +//ACCEL_ID_BMA150 +#define ACCEL_BMA150_RANGE (2.0f*GRAVITY_EARTH) +#define ACCEL_BMA150_RESOLUTION (0.004f*GRAVITY_EARTH) +#define ACCEL_BMA150_POWER (0.2f) +//ACCEL_ID_BMA222 +#define ACCEL_BMA222_RANGE (2.0f*GRAVITY_EARTH) +#define ACCEL_BMA222_RESOLUTION (0.001f*GRAVITY_EARTH) +#define ACCEL_BMA222_POWER (0.1f) +//ACCEL_ID_BMA250 +#define ACCEL_BMA250_RANGE (2.0f*GRAVITY_EARTH) +#define ACCEL_BMA250_RESOLUTION (0.00391f*GRAVITY_EARTH) +#define ACCEL_BMA250_POWER (0.139f) +//ACCEL_ID_ADXL34X +#define ACCEL_ADXL34X_RANGE (2.0f*GRAVITY_EARTH) +#define ACCEL_ADXL34X_RESOLUTION (0.001f*GRAVITY_EARTH) +#define ACCEL_ADXL34X_POWER (1.0f) +//ACCEL_ID_MMA8450 +#define ACCEL_MMA8450_RANGE (2.0f*GRAVITY_EARTH) +#define ACCEL_MMA8450_RESOLUTION (0.001f*GRAVITY_EARTH) +#define ACCEL_MMA8450_POWER (1.0f) +//ACCEL_ID_MMA845X +#define ACCEL_MMA845X_RANGE (2.0f*GRAVITY_EARTH) +#define ACCEL_MMA845X_RESOLUTION (0.001f*GRAVITY_EARTH) +#define ACCEL_MMA845X_POWER (1.0f) +//ACCEL_ID_MPU6050 +#define ACCEL_MPU6050_RANGE (2.0f*GRAVITY_EARTH) +#define ACCEL_MPU6050_RESOLUTION (0.004f*GRAVITY_EARTH) +#define ACCEL_MPU6050_POWER (0.0f) +/******************************************/ +//GYRO MPU3050 +#define RAD_P_DEG (3.14159f/180.0f) +#define GYRO_MPU3050_RANGE (2000.0f*RAD_P_DEG) +#define GYRO_MPU3050_RESOLUTION (32.8f*RAD_P_DEG) +#define GYRO_MPU3050_POWER (6.1f) +//GYRO MPU6050 +#define GYRO_MPU6050_RANGE (2000.0f*RAD_P_DEG) +#define GYRO_MPU6050_RESOLUTION (16.4f*RAD_P_DEG) +#define GYRO_MPU6050_POWER (5.5f) + +#endif + diff --git a/60xx/libsensors/sensors.h b/60xx/libsensors/sensors.h new file mode 100644 index 0000000..5c56da0 --- /dev/null +++ b/60xx/libsensors/sensors.h @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2008 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ANDROID_SENSORS_H +#define ANDROID_SENSORS_H + +#include +#include +#include +#include + +#include + +#include +#include + +__BEGIN_DECLS + +/*****************************************************************************/ + +#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) + +#define ID_MPL_BASE (0) +#define ID_GY (ID_MPL_BASE) +#define ID_A (ID_GY + 1) +#define ID_M (ID_A + 1) +#define ID_O (ID_M + 1) +#define ID_RV (ID_O + 1) +#define ID_LA (ID_RV + 1) +#define ID_GR (ID_LA + 1) + +/*****************************************************************************/ + +/* + * The SENSORS Module + */ + +__END_DECLS + +#endif // ANDROID_SENSORS_H diff --git a/60xx/libsensors_iio/Android.mk b/60xx/libsensors_iio/Android.mk new file mode 100644 index 0000000..fd8e472 --- /dev/null +++ b/60xx/libsensors_iio/Android.mk @@ -0,0 +1,118 @@ +# Copyright (C) 2008 The Android Open Source Project +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# Modified 2011 by InvenSense, Inc + +LOCAL_PATH := $(call my-dir) + +# InvenSense fragment of the HAL +include $(CLEAR_VARS) + +LOCAL_MODULE := libinvensense_hal + +LOCAL_MODULE_TAGS := optional +LOCAL_MODULE_OWNER := invensense + +LOCAL_CFLAGS := -DLOG_TAG=\"Sensors\" -Werror -Wall +# Comment out for ICS. Affects Android LOG macros. +LOCAL_CFLAGS += -DANDROID_JELLYBEAN + +ifeq ($(ENG_BUILD),1) +ifeq ($(COMPILE_INVENSENSE_COMPASS_CAL),1) +LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL +endif +ifeq ($(COMPILE_THIRD_PARTY_ACCEL),1) +LOCAL_CFLAGS += -DTHIRD_PARTY_ACCEL +endif +ifeq ($(COMPILE_COMPASS_YAS53x),1) +LOCAL_CFLAGS += -DCOMPASS_YAS53x +endif +ifeq ($(COMPILE_COMPASS_AK8975),1) +LOCAL_CFLAGS += -DCOMPASS_AK8975 +endif +ifeq ($(COMPILE_COMPASS_AMI306),1) +LOCAL_CFLAGS += -DCOMPASS_AMI306 +endif +else # release builds, default +LOCAL_CFLAGS += -DINVENSENSE_COMPASS_CAL +endif + +LOCAL_SRC_FILES := SensorBase.cpp +LOCAL_SRC_FILES += MPLSensor.cpp +LOCAL_SRC_FILES += MPLSupport.cpp +LOCAL_SRC_FILES += InputEventReader.cpp +LOCAL_SRC_FILES += CompassSensor.IIO.9150.cpp + +LOCAL_C_INCLUDES += $(LOCAL_PATH) +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux + +LOCAL_SHARED_LIBRARIES := liblog +LOCAL_SHARED_LIBRARIES += libcutils +LOCAL_SHARED_LIBRARIES += libutils +LOCAL_SHARED_LIBRARIES += libdl +LOCAL_SHARED_LIBRARIES += libmllite + +# Additions for SysPed +LOCAL_SHARED_LIBRARIES += libmplmpu +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mpl +LOCAL_CPPFLAGS += -DLINUX=1 +# Experimental +ifeq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true) +LOCAL_CPPFLAGS += -DLIBMLLITE_FROM_SOURCE +endif + +include $(BUILD_SHARED_LIBRARY) + +include $(CLEAR_VARS) +LOCAL_MODULE := libmplmpu +LOCAL_SRC_FILES := libmplmpu.so +LOCAL_MODULE_TAGS := optional +LOCAL_MODULE_OWNER := invensense +LOCAL_MODULE_SUFFIX := .so +LOCAL_MODULE_CLASS := SHARED_LIBRARIES +LOCAL_MODULE_PATH := $(TARGET_OUT)/lib +OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) +LOCAL_STRIP_MODULE := true +include $(BUILD_PREBUILT) + +# Experimental +ifneq ($(BOARD_INV_LIBMLLITE_FROM_SOURCE),true) +include $(CLEAR_VARS) +LOCAL_MODULE := libmllite +LOCAL_SRC_FILES := libmllite.so +LOCAL_MODULE_TAGS := optional +LOCAL_MODULE_OWNER := invensense +LOCAL_MODULE_SUFFIX := .so +LOCAL_MODULE_CLASS := SHARED_LIBRARIES +LOCAL_MODULE_PATH := $(TARGET_OUT)/lib +OVERRIDE_BUILT_MODULE_PATH := $(TARGET_OUT_INTERMEDIATE_LIBRARIES) +LOCAL_STRIP_MODULE := true +include $(BUILD_PREBUILT) +else +include $(CLEAR_VARS) +LOCAL_CFLAGS += -DANDROID_JELLYBEAN +LOCAL_CFLAGS += -DLINUX=1 +LOCAL_MODULE := libmllite +LOCAL_SRC_FILES := $(call all-c-files-under, software/core/mllite) +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/mllite/linux +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include +LOCAL_C_INCLUDES += $(LOCAL_PATH)/software/core/driver/include/linux +LOCAL_MODULE_OWNER := invensense +LOCAL_MODULE_PATH := $(TARGET_OUT)/lib +LOCAL_SHARED_LIBRARIES := liblog +include $(BUILD_SHARED_LIBRARY) +endif diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp new file mode 100644 index 0000000..be8c912 --- /dev/null +++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.cpp @@ -0,0 +1,390 @@ +/* + * Copyright (C) 2012 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#define LOG_NDEBUG 0 + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "CompassSensor.IIO.9150.h" +#include "sensors.h" +#include "MPLSupport.h" +#include "sensor_params.h" +#include "ml_sysfs_helper.h" +#include "local_log_def.h" + +#define COMPASS_MAX_SYSFS_ATTRB sizeof(compassSysFs) / sizeof(char*) + +#if defined COMPASS_YAS53x +# pragma message "Invensense compass cal with YAS53x IIO on secondary bus" +# define USE_MPL_COMPASS_HAL (1) +# define COMPASS_NAME "INV_YAS530" +#elif defined COMPASS_AK8975 +# pragma message "Invensense compass cal with AK8975 on primary bus" +# define USE_MPL_COMPASS_HAL (1) +# define COMPASS_NAME "INV_AK8975" +#elif defined INVENSENSE_COMPASS_CAL +# pragma message "Invensense compass cal with compass IIO on secondary bus" +# define USE_MPL_COMPASS_HAL (1) +# define COMPASS_NAME "INV_COMPASS" +#else +# pragma message "third party compass cal HAL" +# define USE_MPL_COMPASS_HAL (0) +// TODO: change to vendor's name +# define COMPASS_NAME "AKM8975" +#endif + + +/*****************************************************************************/ + +CompassSensor::CompassSensor() + : SensorBase(NULL, NULL), + compass_fd(-1), + mCompassTimestamp(0), + mCompassInputReader(8) +{ + VFUNC_LOG; + + if(inv_init_sysfs_attributes()) { + LOGE("Error Instantiating Compass\n"); + return; + } + + if (!strcmp(COMPASS_NAME, "INV_COMPASS")) { + mI2CBus = COMPASS_BUS_SECONDARY; + } else { + mI2CBus = COMPASS_BUS_PRIMARY; + } + + memset(mCachedCompassData, 0, sizeof(mCachedCompassData)); + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", + compassSysFs.compass_orient, getTimestamp()); + FILE *fptr; + fptr = fopen(compassSysFs.compass_orient, "r"); + if (fptr != NULL) { + int om[9]; + fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]); + fclose(fptr); + + LOGV_IF(EXTRA_VERBOSE, + "HAL:compass mounting matrix: " + "%+d %+d %+d %+d %+d %+d %+d %+d %+d", + om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); + + mCompassOrientation[0] = om[0]; + mCompassOrientation[1] = om[1]; + mCompassOrientation[2] = om[2]; + mCompassOrientation[3] = om[3]; + mCompassOrientation[4] = om[4]; + mCompassOrientation[5] = om[5]; + mCompassOrientation[6] = om[6]; + mCompassOrientation[7] = om[7]; + mCompassOrientation[8] = om[8]; + } else { + LOGE("HAL:Couldn't read compass mounting matrix"); + } + + if (!isIntegrated()) { + enable(ID_M, 0); + } +} + +CompassSensor::~CompassSensor() +{ + VFUNC_LOG; + + free(pathP); + if( compass_fd > 0) + close(compass_fd); +} + +int CompassSensor::getFd() const +{ + VHANDLER_LOG; + return compass_fd; +} + +/** + * @brief This function will enable/disable sensor. + * @param[in] handle + * which sensor to enable/disable. + * @param[in] en + * en=1, enable; + * en=0, disable + * @return if the operation is successful. + */ +int CompassSensor::enable(int32_t /*handle*/, int en) +{ + VFUNC_LOG; + + mEnable = en; + int res; + + res = write_sysfs_int(compassSysFs.compass_enable, en); + LOGE_IF(res < 0, "HAL:enable compass failed"); + + if (en) { + res = write_sysfs_int(compassSysFs.compass_x_fifo_enable, en); + res = write_sysfs_int(compassSysFs.compass_y_fifo_enable, en); + res = write_sysfs_int(compassSysFs.compass_z_fifo_enable, en); + } + + return res; +} + +int CompassSensor::setDelay(int32_t /*handle*/, int64_t ns) +{ + VFUNC_LOG; + int tempFd; + int res; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / ns, compassSysFs.compass_rate, getTimestamp()); + mDelay = ns; + if (ns == 0) + return -1; + tempFd = open(compassSysFs.compass_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / ns); + if(res < 0) { + LOGE("HAL:Compass update delay error"); + } + return res; +} + + +/** + @brief This function will return the state of the sensor. + @return 1=enabled; 0=disabled +**/ +int CompassSensor::getEnable(int32_t /*handle*/) +{ + VFUNC_LOG; + return mEnable; +} + +/* use for Invensense compass calibration */ +#define COMPASS_EVENT_DEBUG (0) +void CompassSensor::processCompassEvent(const input_event *event) +{ + VHANDLER_LOG; + + switch (event->code) { + case EVENT_TYPE_ICOMPASS_X: + LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_X\n"); + mCachedCompassData[0] = event->value; + break; + case EVENT_TYPE_ICOMPASS_Y: + LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Y\n"); + mCachedCompassData[1] = event->value; + break; + case EVENT_TYPE_ICOMPASS_Z: + LOGV_IF(COMPASS_EVENT_DEBUG, "EVENT_TYPE_ICOMPASS_Z\n"); + mCachedCompassData[2] = event->value; + break; + } + + mCompassTimestamp = + (int64_t)event->time.tv_sec * 1000000000L + event->time.tv_usec * 1000L; +} + +void CompassSensor::getOrientationMatrix(signed char *orient) +{ + VFUNC_LOG; + memcpy(orient, mCompassOrientation, sizeof(mCompassOrientation)); +} + +long CompassSensor::getSensitivity() +{ + VFUNC_LOG; + + long sensitivity; + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", + compassSysFs.compass_scale, getTimestamp()); + inv_read_data(compassSysFs.compass_scale, &sensitivity); + return sensitivity; +} + +/** + @brief This function is called by sensors_mpl.cpp + to read sensor data from the driver. + @param[out] data sensor data is stored in this variable. Scaled such that + 1 uT = 2^16 + @para[in] timestamp data's timestamp + @return 1, if 1 sample read, 0, if not, negative if error + */ +int CompassSensor::readSample(long *data, int64_t *timestamp) +{ + VHANDLER_LOG; + + int done = 0; + + ssize_t n = mCompassInputReader.fill(compass_fd); + if (n < 0) { + LOGE("HAL:no compass events read"); + return n; + } + + input_event const* event; + + while (done == 0 && mCompassInputReader.readEvent(&event)) { + int type = event->type; + if (type == EV_REL) { + processCompassEvent(event); + } else if (type == EV_SYN) { + *timestamp = mCompassTimestamp; + memcpy(data, mCachedCompassData, sizeof(mCachedCompassData)); + done = 1; + } else { + LOGE("HAL:Compass Sensor: unknown event (type=%d, code=%d)", + type, event->code); + } + mCompassInputReader.next(); + } + + return done; +} + +/** + * @brief This function will return the current delay for this sensor. + * @return delay in nanoseconds. + */ +int64_t CompassSensor::getDelay(int32_t /*handle*/) +{ + VFUNC_LOG; + return mDelay; +} + +void CompassSensor::fillList(struct sensor_t *list) +{ + VFUNC_LOG; + + const char *compass = COMPASS_NAME; + + if (compass) { + if(!strcmp(compass, "INV_COMPASS")) { + list->maxRange = COMPASS_MPU9150_RANGE; + /* since target platform would use AK8963 instead of AK8975, + need to adopt AK8963's resolution here */ + list->resolution = COMPASS_AKM8963_RESOLUTION; + list->power = COMPASS_MPU9150_POWER; + list->minDelay = COMPASS_MPU9150_MINDELAY; + return; + } + if(!strcmp(compass, "compass") + || !strcmp(compass, "INV_AK8975") ) { + list->maxRange = COMPASS_AKM8975_RANGE; + list->resolution = COMPASS_AKM8975_RESOLUTION; + list->power = COMPASS_AKM8975_POWER; + list->minDelay = COMPASS_AKM8975_MINDELAY; + return; + } + if(!strcmp(compass, "INV_YAS530")) { + list->maxRange = COMPASS_YAS53x_RANGE; + list->resolution = COMPASS_YAS53x_RESOLUTION; + list->power = COMPASS_YAS53x_POWER; + list->minDelay = COMPASS_YAS53x_MINDELAY; + return; + } + if(!strcmp(compass, "INV_AMI306")) { + list->maxRange = COMPASS_AMI306_RANGE; + list->resolution = COMPASS_AMI306_RESOLUTION; + list->power = COMPASS_AMI306_POWER; + list->minDelay = COMPASS_AMI306_MINDELAY; + return; + } + } + + LOGE("HAL:unknown compass id %s -- " + "params default to ak8975 and might be wrong.", + compass); + list->maxRange = COMPASS_AKM8975_RANGE; + list->resolution = COMPASS_AKM8975_RESOLUTION; + list->power = COMPASS_AKM8975_POWER; + list->minDelay = COMPASS_AKM8975_MINDELAY; +} + +int CompassSensor::inv_init_sysfs_attributes(void) +{ + VFUNC_LOG; + + unsigned char i = 0; + char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN]; + char *sptr; + char **dptr; + + pathP = (char*)malloc( + sizeof(char[COMPASS_MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); + sptr = pathP; + dptr = (char**)&compassSysFs; + if (sptr == NULL) + return -1; + + do { + *dptr++ = sptr; + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < COMPASS_MAX_SYSFS_ATTRB); + + // get proper (in absolute/relative) IIO path & build MPU's sysfs paths + // inv_get_sysfs_abs_path(sysfs_path); + if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) { + ALOGE("CompassSensor failed get sysfs path"); + return -1; + } + + inv_get_iio_trigger_path(iio_trigger_path); + +#if defined COMPASS_AK8975 + char tbuf[2]; + int num; + inv_get_input_number(COMPASS_NAME, &num); + tbuf[0] = num + 0x30; + tbuf[1] = 0; + sprintf(sysfs_path, "%s%s", "sys/class/input/input", tbuf); + strcat(sysfs_path, "/ak8975"); + + sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/enable"); + sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/rate"); + sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/scale"); + sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); +#else + sprintf(compassSysFs.compass_enable, "%s%s", sysfs_path, "/compass_enable"); + sprintf(compassSysFs.compass_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_x_en"); + sprintf(compassSysFs.compass_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_y_en"); + sprintf(compassSysFs.compass_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_magn_z_en"); + sprintf(compassSysFs.compass_rate, "%s%s", sysfs_path, "/sampling_frequency"); + sprintf(compassSysFs.compass_scale, "%s%s", sysfs_path, "/in_magn_scale"); + sprintf(compassSysFs.compass_orient, "%s%s", sysfs_path, "/compass_matrix"); +#endif + +#if SYSFS_VERBOSE + // test print sysfs paths + dptr = (char**)&compassSysFs; + for (i = 0; i < COMPASS_MAX_SYSFS_ATTRB; i++) { + LOGE("HAL:sysfs path: %s", *dptr++); + } +#endif + return 0; +} diff --git a/60xx/libsensors_iio/CompassSensor.IIO.9150.h b/60xx/libsensors_iio/CompassSensor.IIO.9150.h new file mode 100644 index 0000000..6e9bf03 --- /dev/null +++ b/60xx/libsensors_iio/CompassSensor.IIO.9150.h @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2012 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef COMPASS_SENSOR_H +#define COMPASS_SENSOR_H + +#include +#include +#include +#include + +// TODO fixme, need input_event +#include +#include +#include +#include +#include +#include +#include + +#include "sensors.h" +#include "SensorBase.h" +#include "InputEventReader.h" + +class CompassSensor : public SensorBase { + +public: + CompassSensor(); + virtual ~CompassSensor(); + + virtual int getFd() const; + virtual int enable(int32_t handle, int enabled); + virtual int setDelay(int32_t handle, int64_t ns); + virtual int getEnable(int32_t handle); + virtual int64_t getDelay(int32_t handle); + + // unnecessary for MPL + virtual int readEvents(sensors_event_t* /*data*/, int /*count*/) { return 0; } + + int readSample(long *data, int64_t *timestamp); + int providesCalibration() { return 0; } + void getOrientationMatrix(signed char *orient); + long getSensitivity(); + int getAccuracy() { return 0; } + void fillList(struct sensor_t *list); + int isIntegrated() { return (mI2CBus == COMPASS_BUS_SECONDARY); } + +private: + enum CompassBus { + COMPASS_BUS_PRIMARY = 0, + COMPASS_BUS_SECONDARY = 1 + } mI2CBus; + + struct sysfs_attrbs { + char *compass_enable; + char *compass_x_fifo_enable; + char *compass_y_fifo_enable; + char *compass_z_fifo_enable; + char *compass_rate; + char *compass_scale; + char *compass_orient; + } compassSysFs; + + // implementation specific + signed char mCompassOrientation[9]; + long mCachedCompassData[3]; + int compass_fd; + int64_t mCompassTimestamp; + InputEventCircularReader mCompassInputReader; + int64_t mDelay; + int mEnable; + char *pathP; + + void processCompassEvent(const input_event *event); + int inv_init_sysfs_attributes(void); +}; + +/*****************************************************************************/ + +#endif // COMPASS_SENSOR_H diff --git a/60xx/libsensors_iio/InputEventReader.cpp b/60xx/libsensors_iio/InputEventReader.cpp new file mode 100644 index 0000000..fc48892 --- /dev/null +++ b/60xx/libsensors_iio/InputEventReader.cpp @@ -0,0 +1,106 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#define LOG_NDEBUG 0 + +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include + +#include "InputEventReader.h" +#include "local_log_def.h" + +/*****************************************************************************/ + +struct input_event; + +InputEventCircularReader::InputEventCircularReader(size_t numEvents) + : mBuffer(new input_event[numEvents * 2]), + mBufferEnd(mBuffer + numEvents), + mHead(mBuffer), + mCurr(mBuffer), + mFreeSpace(numEvents) +{ +} + +InputEventCircularReader::~InputEventCircularReader() +{ + delete [] mBuffer; +} + +#define INPUT_EVENT_DEBUG (0) +ssize_t InputEventCircularReader::fill(int fd) +{ + size_t numEventsRead = 0; + LOGV_IF(INPUT_EVENT_DEBUG, + "DEBUG:%s enter, fd=%d\n", __PRETTY_FUNCTION__, fd); + if (mFreeSpace) { + const ssize_t nread = read(fd, mHead, mFreeSpace * sizeof(input_event)); + if (nread < 0 || nread % sizeof(input_event)) { + //LOGE("Partial event received nread=%d, required=%d", + // nread, sizeof(input_event)); + //LOGE("FD trying to read is: %d"); + // we got a partial event!! + if (INPUT_EVENT_DEBUG) { + LOGV_IF(nread < 0, "DEBUG:%s exit nread < 0\n", + __PRETTY_FUNCTION__); + LOGV_IF(nread % sizeof(input_event), + "DEBUG:%s exit nread %% sizeof(input_event)\n", + __PRETTY_FUNCTION__); + } + return (nread < 0 ? -errno : -EINVAL); + } + + numEventsRead = nread / sizeof(input_event); + if (numEventsRead) { + mHead += numEventsRead; + mFreeSpace -= numEventsRead; + if (mHead > mBufferEnd) { + size_t s = mHead - mBufferEnd; + memcpy(mBuffer, mBufferEnd, s * sizeof(input_event)); + mHead = mBuffer + s; + } + } + } + + LOGV_IF(INPUT_EVENT_DEBUG, "DEBUG:%s exit\n", __PRETTY_FUNCTION__); + return numEventsRead; +} + +ssize_t InputEventCircularReader::readEvent(input_event const** events) +{ + *events = mCurr; + ssize_t available = (mBufferEnd - mBuffer) - mFreeSpace; + return available ? 1 : 0; +} + +void InputEventCircularReader::next() +{ + mCurr++; + mFreeSpace++; + if (mCurr >= mBufferEnd) { + mCurr = mBuffer; + } +} diff --git a/60xx/libsensors_iio/InputEventReader.h b/60xx/libsensors_iio/InputEventReader.h new file mode 100644 index 0000000..5c752db --- /dev/null +++ b/60xx/libsensors_iio/InputEventReader.h @@ -0,0 +1,49 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#ifndef ANDROID_INPUT_EVENT_READER_H +#define ANDROID_INPUT_EVENT_READER_H + +#include +#include +#include +#include + +#include "SensorBase.h" + +/*****************************************************************************/ + +struct input_event; + +class InputEventCircularReader +{ + struct input_event* const mBuffer; + struct input_event* const mBufferEnd; + struct input_event* mHead; + struct input_event* mCurr; + ssize_t mFreeSpace; + +public: + InputEventCircularReader(size_t numEvents); + ~InputEventCircularReader(); + ssize_t fill(int fd); + ssize_t readEvent(input_event const** events); + void next(); +}; + +/*****************************************************************************/ + +#endif // ANDROID_INPUT_EVENT_READER_H diff --git a/60xx/libsensors_iio/License.txt b/60xx/libsensors_iio/License.txt new file mode 100644 index 0000000..930f931 --- /dev/null +++ b/60xx/libsensors_iio/License.txt @@ -0,0 +1,217 @@ +SOFTWARE LICENSE AGREEMENT + +Unless you and InvenSense Corporation ("InvenSense") execute a separate written +software license agreement governing use of the accompanying software, this +software is licensed to you under the terms of this Software License +Agreement ("Agreement"). + +ANY USE, REPRODUCTION OR DISTRIBUTION OF THE SOFTWARE CONSTITUTES YOUR +ACCEPTANCE OF THIS AGREEMENT. + +1. DEFINITIONS. + +1.1. "InvenSense Product" means any of the proprietary integrated circuit +product(s) sold by InvenSense with which the Software was designed to be used, +or their successors. + +1.2. "Licensee" means you or if you are accepting on behalf of an entity +then the entity and its affiliates exercising rights under, and complying +with all of the terms of this Agreement. + +1.3. "Software" shall mean that software made available by InvenSense to +Licensee in binary code form with this Agreement. + +2. LICENSE GRANT; OWNERSHIP + +2.1. License Grants. Subject to the terms and conditions of this Agreement, +InvenSense hereby grants to Licensee a non-exclusive, non-transferable, +royalty-free license (i) to use and integrate the Software in conjunction +with any other software; and (ii) to reproduce and distribute the Software +complete, unmodified and only for use with a InvenSense Product. + +2.2. Restriction on Modification. If and to the extent that the Software is +designed to be compliant with any published communications standard +(including, without limitation, DOCSIS, HomePNA, IEEE, and ITU standards), +Licensee may not make any modifications to the Software that would cause the +Software or the accompanying InvenSense Products to be incompatible with such +standard. + +2.3. Restriction on Distribution. Licensee shall only distribute the +Software (a) under the terms of this Agreement and a copy of this Agreement +accompanies such distribution, and (b) agrees to defend and indemnify +InvenSense and its licensors from and against any damages, costs, liabilities, +settlement amounts and/or expenses (including attorneys' fees) incurred in +connection with any claim, lawsuit or action by any third party that arises +or results from the use or distribution of any and all Software by the +Licensee except as contemplated herein. + +2.4. Proprietary Notices. Licensee shall not remove, efface or obscure any +copyright or trademark notices from the Software. Licensee shall include +reproductions of the InvenSense copyright notice with each copy of the +Software, except where such Software is embedded in a manner not readily +accessible to the end user. Licensee acknowledges that any symbols, +trademarks, tradenames, and service marks adopted by InvenSense to identify the +Software belong to InvenSense and that Licensee shall have no rights therein. + +2.5. Ownership. InvenSense shall retain all right, title and interest, +including all intellectual property rights, in and to the Software. Licensee +hereby covenants that it will not assert any claim that the Software created +by or for InvenSense infringe any intellectual property right owned or +controlled by Licensee. + +2.6. No Other Rights Granted; Restrictions. Apart from the license rights +expressly set forth in this Agreement, InvenSense does not grant and Licensee +does not receive any ownership right, title or interest nor any security +interest or other interest in any intellectual property rights relating to +the Software, nor in any copy of any part of the foregoing. No license is +granted to Licensee in any human readable code of the Software (source code). +Licensee shall not (i) use, license, sell or otherwise distribute the +Software except as provided in this Agreement, (ii) attempt to reverse +engineer, decompile or disassemble any portion of the Software; or (iii) use +the Software or other material in violation of any applicable law or +regulation, including but not limited to any regulatory agency, such as FCC, +rules. + +3. NO WARRANTY OR SUPPORT + +3.1. No Warranty. THE SOFTWARE IS OFFERED "AS IS," AND INVENSENSE GRANTS AND +LICENSEE RECEIVES NO WARRANTIES OF ANY KIND, EXPRESS OR IMPLIED, BY STATUTE, +COMMUNICATION OR CONDUCT WITH LICENSEE, OR OTHERWISE. INVENSENSE SPECIFICALLY +DISCLAIMS ANY IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A SPECIFIC +PURPOSE OR NONINFRINGEMENT CONCERNING THE SOFTWARE OR ANY UPGRADES TO OR +DOCUMENTATION FOR THE SOFTWARE. WITHOUT LIMITATION OF THE ABOVE, INVENSENSE +GRANTS NO WARRANTY THAT THE SOFTWARE IS ERROR-FREE OR WILL OPERATE WITHOUT +INTERRUPTION, AND GRANTS NO WARRANTY REGARDING ITS USE OR THE RESULTS +THEREFROM INCLUDING, WITHOUT LIMITATION, ITS CORRECTNESS, ACCURACY OR +RELIABILITY. + +3.2. No Support. Nothing in this agreement shall obligate InvenSense to +provide any support for the Software. InvenSense may, but shall be under no +obligation to, correct any defects in the Software and/or provide updates to +licensees of the Software. Licensee shall make reasonable efforts to +promptly report to InvenSense any defects it finds in the Software, as an aid +to creating improved revisions of the Software. + +3.3. Dangerous Applications. The Software is not designed, intended, or +certified for use in components of systems intended for the operation of +weapons, weapons systems, nuclear installations, means of mass +transportation, aviation, life-support computers or equipment (including +resuscitation equipment and surgical implants), pollution control, hazardous +substances management, or for any other dangerous application in which the +failure of the Software could create a situation where personal injury or +death may occur. Licensee understands that use of the Software in such +applications is fully at the risk of Licensee. + +4. TERM AND TERMINATION + +4.1. Termination. This Agreement will automatically terminate if Licensee +fails to comply with any of the terms and conditions hereof. In such event, +Licensee must destroy all copies of the Software and all of its component +parts. + +4.2. Effect Of Termination. Upon any termination of this Agreement, the +rights and licenses granted to Licensee under this Agreement shall +immediately terminate. + +4.3. Survival. The rights and obligations under this Agreement which by +their nature should survive termination will remain in effect after +expiration or termination of this Agreement. + +5. CONFIDENTIALITY + +5.1. Obligations. Licensee acknowledges and agrees that any documentation +relating to the Software, and any other information (if such other +information is identified as confidential or should be recognized as +confidential under the circumstances) provided to Licensee by InvenSense +hereunder (collectively, "Confidential Information") constitute the +confidential and proprietary information of InvenSense, and that Licensee's +protection thereof is an essential condition to Licensee's use and possession +of the Software. Licensee shall retain all Confidential Information in +strict confidence and not disclose it to any third party or use it in any way +except under a written agreement with terms and conditions at least as +protective as the terms of this Section. Licensee will exercise at least the +same amount of diligence in preserving the secrecy of the Confidential +Information as it uses in preserving the secrecy of its own most valuable +confidential information, but in no event less than reasonable diligence. +Information shall not be considered Confidential Information if and to the +extent that it: (i) was in the public domain at the time it was disclosed or +has entered the public domain through no fault of Licensee; (ii) was known to +Licensee, without restriction, at the time of disclosure as proven by the +files of Licensee in existence at the time of disclosure; or (iii) becomes +known to Licensee, without restriction, from a source other than InvenSense +without breach of this Agreement by Licensee and otherwise not in violation +of InvenSense's rights. + +5.2. Return of Confidential Information. Notwithstanding the foregoing, all +documents and other tangible objects containing or representing InvenSense +Confidential Information and all copies thereof which are in the possession +of Licensee shall be and remain the property of InvenSense, and shall be +promptly returned to InvenSense upon written request by InvenSense or upon +termination of this Agreement. + +6. LIMITATION OF LIABILITY +TO THE MAXIMUM EXTENT PERMITTED BY LAW, IN NO EVENT SHALL INVENSENSE OR ANY OF +INVENSENSE'S LICENSORS HAVE ANY LIABILITY FOR ANY INDIRECT, INCIDENTAL, +SPECIAL, OR CONSEQUENTIAL DAMAGES, HOWEVER CAUSED AND ON ANY THEORY OF +LIABILITY, WHETHER FOR BREACH OF CONTRACT, TORT (INCLUDING NEGLIGENCE) OR +OTHERWISE, ARISING OUT OF THIS AGREEMENT, INCLUDING BUT NOT LIMITED TO LOSS +OF PROFITS, EVEN IF SUCH PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. IN NO EVENT WILL INVENSENSE'S LIABILITY WHETHER IN CONTRACT, TORT +(INCLUDING NEGLIGENCE), OR OTHERWISE, EXCEED THE AMOUNT PAID BY LICENSEE FOR +SOFTWARE UNDER THIS AGREEMENT. THESE LIMITATIONS SHALL APPLY NOTWITHSTANDING +ANY FAILURE OF ESSENTIAL PURPOSE OF ANY LIMITED REMEDY. + +7. MISCELLANEOUS + +7.1. Export Regulations. YOU UNDERSTAND AND AGREE THAT THE SOFTWARE IS +SUBJECT TO UNITED STATES AND OTHER APPLICABLE EXPORT-RELATED LAWS AND +REGULATIONS AND THAT YOU MAY NOT EXPORT, RE-EXPORT OR TRANSFER THE SOFTWARE +OR ANY DIRECT PRODUCT OF THE SOFTWARE EXCEPT AS PERMITTED UNDER THOSE LAWS. +WITHOUT LIMITING THE FOREGOING, EXPORT, RE-EXPORT OR TRANSFER OF THE SOFTWARE +TO CUBA, IRAN, NORTH KOREA, SUDAN AND SYRIA IS PROHIBITED. + +7.2 Assignment. This Agreement shall be binding upon and inure to the +benefit of the parties and their respective successors and assigns, provided, +however that Licensee may not assign this Agreement or any rights or +obligation hereunder, directly or indirectly, by operation of law or +otherwise, without the prior written consent of InvenSense, and any such +attempted assignment shall be void. Notwithstanding the foregoing, Licensee +may assign this Agreement to a successor to all or substantially all of its +business or assets to which this Agreement relates that is not a competitor +of InvenSense. + +7.3. Governing Law; Venue. This Agreement shall be governed by the laws of +California without regard to any conflict-of-laws rules, and the United +Nations Convention on Contracts for the International Sale of Goods is hereby +excluded. The sole jurisdiction and venue for actions related to the subject +matter hereof shall be the state and federal courts located in the County of +Orange, California, and both parties hereby consent to such jurisdiction and +venue. + +7.4. Severability. All terms and provisions of this Agreement shall, if +possible, be construed in a manner which makes them valid, but in the event +any term or provision of this Agreement is found by a court of competent +jurisdiction to be illegal or unenforceable, the validity or enforceability +of the remainder of this Agreement shall not be affected if the illegal or +unenforceable provision does not materially affect the intent of this +Agreement. If the illegal or unenforceable provision materially affects the +intent of the parties to this Agreement, this Agreement shall become +terminated. + +7.5. Equitable Relief. Licensee hereby acknowledges that its breach of this +Agreement would cause irreparable harm and significant injury to InvenSense +that may be difficult to ascertain and that a remedy at law would be +inadequate. Accordingly, Licensee agrees that InvenSense shall have the right +to seek and obtain immediate injunctive relief to enforce obligations under +the Agreement in addition to any other rights and remedies it may have. + +7.6. Waiver. The waiver of, or failure to enforce, any breach or default +hereunder shall not constitute the waiver of any other or subsequent breach +or default. + +7.7. Entire Agreement. This Agreement sets forth the entire Agreement +between the parties and supersedes any and all prior proposals, agreements +and representations between them, whether written or oral concerning the +Software. This Agreement may be changed only by mutual agreement of the +parties in writing. + diff --git a/60xx/libsensors_iio/MPLSensor.cpp b/60xx/libsensors_iio/MPLSensor.cpp new file mode 100644 index 0000000..031ae6e --- /dev/null +++ b/60xx/libsensors_iio/MPLSensor.cpp @@ -0,0 +1,2764 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#define LOG_NDEBUG 0 +//see also the EXTRA_VERBOSE define in the MPLSensor.h header file + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MPLSensor.h" +#include "MPLSupport.h" +#include "sensor_params.h" +#include "local_log_def.h" + +#include "invensense.h" +#include "invensense_adv.h" +#include "ml_stored_data.h" +#include "ml_load_dmp.h" +#include "ml_sysfs_helper.h" + +// #define TESTING + +#ifdef THIRD_PARTY_ACCEL +# warning "Third party accel" +# define USE_THIRD_PARTY_ACCEL (1) +#else +# define USE_THIRD_PARTY_ACCEL (0) +#endif + +#define MAX_SYSFS_ATTRB (sizeof(struct sysfs_attrbs) / sizeof(char*)) + +/******************************************************************************/ +/* MPL interface misc. */ +/******************************************************************************/ +static int hertz_request = 200; + +#define DEFAULT_MPL_GYRO_RATE (20000L) //us +#define DEFAULT_MPL_COMPASS_RATE (20000L) //us + +#define DEFAULT_HW_GYRO_RATE (100) //Hz +#define DEFAULT_HW_ACCEL_RATE (20) //ms +#define DEFAULT_HW_COMPASS_RATE (20000000L) //ns +#define DEFAULT_HW_AKMD_COMPASS_RATE (200000000L) //ns + +/* convert ns to hardware units */ +#define HW_GYRO_RATE_NS (1000000000LL / rate_request) // to Hz +#define HW_ACCEL_RATE_NS (rate_request / (1000000L)) // to ms +#define HW_COMPASS_RATE_NS (rate_request) // to ns + +/* convert Hz to hardware units */ +#define HW_GYRO_RATE_HZ (hertz_request) +#define HW_ACCEL_RATE_HZ (1000 / hertz_request) +#define HW_COMPASS_RATE_HZ (1000000000LL / hertz_request) + +#define RATE_200HZ 5000000LL +#define RATE_15HZ 66667000LL +#define RATE_5HZ 200000000LL + +static struct sensor_t sSensorList[] = +{ + {"MPL Gyroscope", "Invensense", 1, + SENSORS_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, + {"MPL Raw Gyroscope", "Invensense", 1, + SENSORS_RAW_GYROSCOPE_HANDLE, + SENSOR_TYPE_GYROSCOPE, 2000.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, + {"MPL Accelerometer", "Invensense", 1, + SENSORS_ACCELERATION_HANDLE, + SENSOR_TYPE_ACCELEROMETER, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, + {"MPL Magnetic Field", "Invensense", 1, + SENSORS_MAGNETIC_FIELD_HANDLE, + SENSOR_TYPE_MAGNETIC_FIELD, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, + {"MPL Orientation", "Invensense", 1, + SENSORS_ORIENTATION_HANDLE, + SENSOR_TYPE_ORIENTATION, 360.0f, 1.0f, 9.7f, 10000, 0, 0, 0, 0, 0, 0, {}}, + {"MPL Rotation Vector", "Invensense", 1, + SENSORS_ROTATION_VECTOR_HANDLE, + SENSOR_TYPE_ROTATION_VECTOR, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, + {"MPL Linear Acceleration", "Invensense", 1, + SENSORS_LINEAR_ACCEL_HANDLE, + SENSOR_TYPE_LINEAR_ACCELERATION, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, + {"MPL Gravity", "Invensense", 1, + SENSORS_GRAVITY_HANDLE, + SENSOR_TYPE_GRAVITY, 10240.0f, 1.0f, 0.5f, 10000, 0, 0, 0, 0, 0, 0, {}}, + +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION + {"MPL Screen Orientation", "Invensense ", 1, + SENSORS_SCREEN_ORIENTATION_HANDLE, + SENSOR_TYPE_SCREEN_ORIENTATION, 100.0f, 1.0f, 1.1f, 0, 0, 0, 0, 0, 0, 0, {}}, +#endif +}; + +MPLSensor *MPLSensor::gMPLSensor = NULL; + +extern "C" { +void procData_cb_wrapper() +{ + if(MPLSensor::gMPLSensor) { + MPLSensor::gMPLSensor->cbProcData(); + } +} + +void setCallbackObject(MPLSensor* gbpt) +{ + MPLSensor::gMPLSensor = gbpt; +} + +MPLSensor* getCallbackObject() { + return MPLSensor::gMPLSensor; +} + +} // end of extern C + +#ifdef INV_PLAYBACK_DBG +static FILE *logfile = NULL; +#endif + +pthread_mutex_t GlobalHalMutex = PTHREAD_MUTEX_INITIALIZER; + +/******************************************************************************* + * MPLSensor class implementation + ******************************************************************************/ + +MPLSensor::MPLSensor(CompassSensor *compass, int (*m_pt2AccelCalLoadFunc)(long *)) + : SensorBase(NULL, NULL), + mNewData(0), + mMasterSensorMask(INV_ALL_SENSORS), + mLocalSensorMask(0), + mPollTime(-1), + mHaveGoodMpuCal(0), + mGyroAccuracy(0), + mAccelAccuracy(0), + mCompassAccuracy(0), + mSampleCount(0), + dmp_orient_fd(-1), + mDmpOrientationEnabled(0), + mEnabled(0), + mOldEnabledMask(0), + mAccelInputReader(4), + mGyroInputReader(32), + mTempScale(0), + mTempOffset(0), + mTempCurrentTime(0), + mAccelScale(2), + mPendingMask(0), + mSensorMask(0), + mFeatureActiveMask(0) { + VFUNC_LOG; + + inv_error_t rv; + int fd; + char *ver_str; + + mCompassSensor = compass; + + LOGV_IF(EXTRA_VERBOSE, + "HAL:MPLSensor constructor : numSensors = %d", numSensors); + + pthread_mutex_init(&mMplMutex, NULL); + pthread_mutex_init(&mHALMutex, NULL); + memset(mGyroOrientation, 0, sizeof(mGyroOrientation)); + memset(mAccelOrientation, 0, sizeof(mAccelOrientation)); + +#ifdef INV_PLAYBACK_DBG + LOGV_IF(PROCESS_VERBOSE, "HAL:inv_turn_on_data_logging"); + logfile = fopen("/data/playback.bin", "wb"); + if (logfile) + inv_turn_on_data_logging(logfile); +#endif + + /* setup sysfs paths */ + inv_init_sysfs_attributes(); + + /* get chip name */ + if (inv_get_chip_name(chip_ID) != INV_SUCCESS) { + LOGE("HAL:ERR- Failed to get chip ID\n"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Chip ID= %s\n", chip_ID); + } + + enable_iio_sysfs(); + + /* turn on power state */ + onPower(1); + + /* reset driver master enable */ + masterEnable(0); + + if (isLowPowerQuatEnabled() || isDmpDisplayOrientationOn()) { + /* Load DMP image if capable, ie. MPU6xxx/9xxx */ + loadDMP(); + } + + /* open temperature fd for temp comp */ + LOGV_IF(EXTRA_VERBOSE, "HAL:gyro temperature path: %s", mpu.temperature); + gyro_temperature_fd = open(mpu.temperature, O_RDONLY); + if (gyro_temperature_fd == -1) { + LOGE("HAL:could not open temperature node"); + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:temperature_fd opened: %s", mpu.temperature); + } + + /* read accel FSR to calcuate accel scale later */ + if (!USE_THIRD_PARTY_ACCEL) { + char buf[3]; + int count = 0; + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.accel_fsr, getTimestamp()); + + fd = open(mpu.accel_fsr, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening accel FSR"); + } else { + memset(buf, 0, sizeof(buf)); + count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 1) { + LOGE("HAL:Error reading accel FSR"); + } else { + count = sscanf(buf, "%d", &mAccelScale); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Accel FSR used %d", mAccelScale); + } + close(fd); + } + } + + /* initialize sensor data */ + memset(mPendingEvents, 0, sizeof(mPendingEvents)); + + mPendingEvents[RotationVector].version = sizeof(sensors_event_t); + mPendingEvents[RotationVector].sensor = ID_RV; + mPendingEvents[RotationVector].type = SENSOR_TYPE_ROTATION_VECTOR; + + mPendingEvents[LinearAccel].version = sizeof(sensors_event_t); + mPendingEvents[LinearAccel].sensor = ID_LA; + mPendingEvents[LinearAccel].type = SENSOR_TYPE_LINEAR_ACCELERATION; + + mPendingEvents[Gravity].version = sizeof(sensors_event_t); + mPendingEvents[Gravity].sensor = ID_GR; + mPendingEvents[Gravity].type = SENSOR_TYPE_GRAVITY; + + mPendingEvents[Gyro].version = sizeof(sensors_event_t); + mPendingEvents[Gyro].sensor = ID_GY; + mPendingEvents[Gyro].type = SENSOR_TYPE_GYROSCOPE; + + mPendingEvents[RawGyro].version = sizeof(sensors_event_t); + mPendingEvents[RawGyro].sensor = ID_RG; + mPendingEvents[RawGyro].type = SENSOR_TYPE_GYROSCOPE; + + mPendingEvents[Accelerometer].version = sizeof(sensors_event_t); + mPendingEvents[Accelerometer].sensor = ID_A; + mPendingEvents[Accelerometer].type = SENSOR_TYPE_ACCELEROMETER; + + /* Invensense compass calibration */ + mPendingEvents[MagneticField].version = sizeof(sensors_event_t); + mPendingEvents[MagneticField].sensor = ID_M; + mPendingEvents[MagneticField].type = SENSOR_TYPE_MAGNETIC_FIELD; + mPendingEvents[MagneticField].magnetic.status = + SENSOR_STATUS_ACCURACY_HIGH; + + mPendingEvents[Orientation].version = sizeof(sensors_event_t); + mPendingEvents[Orientation].sensor = ID_O; + mPendingEvents[Orientation].type = SENSOR_TYPE_ORIENTATION; + mPendingEvents[Orientation].orientation.status + = SENSOR_STATUS_ACCURACY_HIGH; + + mHandlers[RotationVector] = &MPLSensor::rvHandler; + mHandlers[LinearAccel] = &MPLSensor::laHandler; + mHandlers[Gravity] = &MPLSensor::gravHandler; + mHandlers[Gyro] = &MPLSensor::gyroHandler; + mHandlers[RawGyro] = &MPLSensor::rawGyroHandler; + mHandlers[Accelerometer] = &MPLSensor::accelHandler; + mHandlers[MagneticField] = &MPLSensor::compassHandler; + mHandlers[Orientation] = &MPLSensor::orienHandler; + + for (int i = 0; i < numSensors; i++) { + mDelays[i] = 0; + } + + (void)inv_get_version(&ver_str); + LOGV_IF(PROCESS_VERBOSE, "%s\n", ver_str); + + /* setup MPL */ + inv_constructor_init(); + + /* load calibration file from /data/inv_cal_data.bin */ + rv = inv_load_calibration(); + if(rv == INV_SUCCESS) + LOGV_IF(PROCESS_VERBOSE, "HAL:Calibration file successfully loaded"); + else + LOGE("HAL:Could not open or load MPL calibration file (%d)", rv); + + /* Takes external Accel Calibration Load Method */ + if( m_pt2AccelCalLoadFunc != NULL) + { + long accel_offset[3]; + long tmp_offset[3]; + int result = m_pt2AccelCalLoadFunc(accel_offset); + if(result) + LOGW("HAL:Vendor accelerometer calibration file load failed %d\n", result); + else + { + LOGW("HAL:Vendor accelerometer calibration file successfully loaded"); + inv_get_accel_bias(tmp_offset, NULL); + LOGV_IF(PROCESS_VERBOSE, "HAL:Original accel offset, %ld, %ld, %ld\n", + tmp_offset[0], tmp_offset[1], tmp_offset[2]); + inv_set_accel_bias(accel_offset, mAccelAccuracy); + inv_get_accel_bias(tmp_offset, NULL); + LOGV_IF(PROCESS_VERBOSE, "HAL:Set accel offset, %ld, %ld, %ld\n", + tmp_offset[0], tmp_offset[1], tmp_offset[2]); + } + } + /* End of Accel Calibration Load Method */ + + inv_set_device_properties(); + + /* disable driver master enable the first sensor goes on */ + masterEnable(0); + enableGyro(0); + enableAccel(0); + enableCompass(0); + + if (isLowPowerQuatEnabled()) { + enableLPQuaternion(0); + } + + onPower(0); + + if (isDmpDisplayOrientationOn()) { + enableDmpOrientation(!isDmpScreenAutoRotationEnabled()); + } + +} + +void MPLSensor::enable_iio_sysfs() +{ + VFUNC_LOG; + + char iio_trigger_name[MAX_CHIP_ID_LEN], iio_device_node[MAX_CHIP_ID_LEN]; + FILE *tempFp = NULL; + + /* ignore failures */ + write_sysfs_int(mpu.in_timestamp_en, 1); + + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", + mpu.trigger_name, getTimestamp()); + tempFp = fopen(mpu.trigger_name, "r"); + if (tempFp == NULL) { + LOGE("HAL:could not open trigger name"); + } else { + if (fscanf(tempFp, "%s", iio_trigger_name) < 0) { + LOGE("HAL:could not read trigger name"); + } + fclose(tempFp); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %s > %s (%lld)", + iio_trigger_name, mpu.current_trigger, getTimestamp()); + tempFp = fopen(mpu.current_trigger, "w"); + if (tempFp == NULL) { + LOGE("HAL:could not open current trigger"); + } else { + if (fprintf(tempFp, "%s", iio_trigger_name) < 0 || fclose(tempFp) < 0) { + LOGE("HAL:could not write current trigger %s err=%d", iio_trigger_name, errno); + } + } + + write_sysfs_int(mpu.buffer_length, IIO_BUFFER_LENGTH); + + if (inv_get_iio_device_node(iio_device_node) < 0) { + LOGE("HAL:could retrive the iio device node"); + } + iio_fd = open(iio_device_node, O_RDONLY); + if (iio_fd < 0) { + LOGE("HAL:could not open iio device node"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:iio iio_fd (%s) opened: %d", iio_device_node, iio_fd); + } +} + +int MPLSensor::inv_constructor_init() +{ + VFUNC_LOG; + + inv_error_t result = inv_init_mpl(); + if (result) { + LOGE("HAL:inv_init_mpl() failed"); + return result; + } + result = inv_constructor_default_enable(); + result = inv_start_mpl(); + if (result) { + LOGE("HAL:inv_start_mpl() failed"); + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +int MPLSensor::inv_constructor_default_enable() +{ + VFUNC_LOG; + + inv_error_t result; + + result = inv_enable_quaternion(); + if (result) { + LOGE("HAL:Cannot enable quaternion\n"); + return result; + } + + result = inv_enable_in_use_auto_calibration(); + if (result) { + return result; + } + + // result = inv_enable_motion_no_motion(); + result = inv_enable_fast_nomot(); + if (result) { + return result; + } + + result = inv_enable_gyro_tc(); + if (result) { + return result; + } + + result = inv_enable_hal_outputs(); + if (result) { + return result; + } + + if (!mCompassSensor->providesCalibration()) { + /* Invensense compass calibration */ + LOGV_IF(PROCESS_VERBOSE, "HAL:Invensense vector compass cal enabled"); + result = inv_enable_vector_compass_cal(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } else { + mFeatureActiveMask |= INV_COMPASS_CAL; + } + + // specify MPL's trust weight, used by compass algorithms + inv_vector_compass_cal_sensitivity(3); + + result = inv_enable_compass_bias_w_gyro(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_enable_heading_from_gyro(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_enable_magnetic_disturbance(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + result = inv_enable_9x_sensor_fusion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } else { + // 9x sensor fusion enables Compass fit + mFeatureActiveMask |= INV_COMPASS_FIT; + } + + result = inv_enable_no_gyro_fusion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_enable_quat_accuracy_monitor(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/* TODO: create function pointers to calculate scale */ +void MPLSensor::inv_set_device_properties() +{ + VFUNC_LOG; + + unsigned short orient; + + inv_get_sensors_orientation(); + + inv_set_gyro_sample_rate(DEFAULT_MPL_GYRO_RATE); + inv_set_compass_sample_rate(DEFAULT_MPL_COMPASS_RATE); + + /* gyro setup */ + orient = inv_orientation_matrix_to_scalar(mGyroOrientation); + inv_set_gyro_orientation_and_scale(orient, 2000L << 15); + + /* accel setup */ + orient = inv_orientation_matrix_to_scalar(mAccelOrientation); + /* use for third party accel input subsystem driver + inv_set_accel_orientation_and_scale(orient, 1LL << 22); + */ + inv_set_accel_orientation_and_scale(orient, mAccelScale << 15); + + /* compass setup */ + signed char orientMtx[9]; + mCompassSensor->getOrientationMatrix(orientMtx); + orient = + inv_orientation_matrix_to_scalar(orientMtx); + long sensitivity; + sensitivity = mCompassSensor->getSensitivity(); + inv_set_compass_orientation_and_scale(orient, sensitivity); +} + +void MPLSensor::loadDMP() +{ + int res, fd; + FILE *fptr; + + if (isMpu3050()) { + //DMP support only for MPU6xxx/9xxx currently + return; + } + + /* load DMP firmware */ + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.firmware_loaded, getTimestamp()); + fd = open(mpu.firmware_loaded, O_RDONLY); + if(fd < 0) { + LOGE("HAL:could not open dmp state"); + return; + } + if(inv_read_dmp_state(fd)) { + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP is already loaded"); + return; + } + + LOGV_IF(EXTRA_VERBOSE, "HAL:load dmp: %s", mpu.dmp_firmware); + fptr = fopen(mpu.dmp_firmware, "w"); + if(!fptr) { + LOGE("HAL:could open %s for write. %s", mpu.dmp_firmware, strerror(errno)); + return; + } + res = inv_load_dmp(fptr); + if(res < 0) { + LOGE("HAL:load DMP failed"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP loaded"); + } + fclose(fptr); +} + +void MPLSensor::inv_get_sensors_orientation() +{ + FILE *fptr; + + // get gyro orientation + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.gyro_orient, getTimestamp()); + fptr = fopen(mpu.gyro_orient, "r"); + if (fptr != NULL) { + int om[9]; + fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]); + fclose(fptr); + + LOGV_IF(EXTRA_VERBOSE, + "HAL:gyro mounting matrix: " + "%+d %+d %+d %+d %+d %+d %+d %+d %+d", + om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); + + mGyroOrientation[0] = om[0]; + mGyroOrientation[1] = om[1]; + mGyroOrientation[2] = om[2]; + mGyroOrientation[3] = om[3]; + mGyroOrientation[4] = om[4]; + mGyroOrientation[5] = om[5]; + mGyroOrientation[6] = om[6]; + mGyroOrientation[7] = om[7]; + mGyroOrientation[8] = om[8]; + } else { + LOGE("HAL:Couldn't read gyro mounting matrix"); + } + + // get accel orientation + LOGV_IF(SYSFS_VERBOSE, + "HAL:sysfs:cat %s (%lld)", mpu.accel_orient, getTimestamp()); + fptr = fopen(mpu.accel_orient, "r"); + if (fptr != NULL) { + int om[9]; + fscanf(fptr, "%d,%d,%d,%d,%d,%d,%d,%d,%d", + &om[0], &om[1], &om[2], &om[3], &om[4], &om[5], + &om[6], &om[7], &om[8]); + fclose(fptr); + + LOGV_IF(EXTRA_VERBOSE, + "HAL:accel mounting matrix: " + "%+d %+d %+d %+d %+d %+d %+d %+d %+d", + om[0], om[1], om[2], om[3], om[4], om[5], om[6], om[7], om[8]); + + mAccelOrientation[0] = om[0]; + mAccelOrientation[1] = om[1]; + mAccelOrientation[2] = om[2]; + mAccelOrientation[3] = om[3]; + mAccelOrientation[4] = om[4]; + mAccelOrientation[5] = om[5]; + mAccelOrientation[6] = om[6]; + mAccelOrientation[7] = om[7]; + mAccelOrientation[8] = om[8]; + } else { + LOGE("HAL:Couldn't read accel mounting matrix"); + } +} + +MPLSensor::~MPLSensor() +{ + VFUNC_LOG; + + mCompassSensor = NULL; + + /* Close open fds */ + if (iio_fd > 0) + close(iio_fd); + if( accel_fd > 0 ) + close(accel_fd ); + if (gyro_temperature_fd > 0) + close(gyro_temperature_fd); + if (sysfs_names_ptr) + free(sysfs_names_ptr); + + if (isDmpDisplayOrientationOn()) { + closeDmpOrientFd(); + } + + /* Turn off Gyro master enable */ + /* A workaround until driver handles it */ + /* TODO: Turn off and close all sensors */ + if(write_sysfs_int(mpu.chip_enable, 0) < 0) { + LOGE("HAL:could not disable gyro master enable"); + } + +#ifdef INV_PLAYBACK_DBG + inv_turn_off_data_logging(); + fclose(logfile); +#endif +} + +#define GY_ENABLED (((1 << ID_GY) | (1 << ID_RG)) & enabled_sensors) +#define A_ENABLED ((1 << ID_A) & enabled_sensors) +#define M_ENABLED ((1 << ID_M) & enabled_sensors) +#define O_ENABLED ((1 << ID_O) & enabled_sensors) +#define LA_ENABLED ((1 << ID_LA) & enabled_sensors) +#define GR_ENABLED ((1 << ID_GR) & enabled_sensors) +#define RV_ENABLED ((1 << ID_RV) & enabled_sensors) + +/* TODO: this step is optional, remove? */ +int MPLSensor::setGyroInitialState() +{ + VFUNC_LOG; + + int res = 0; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + HW_GYRO_RATE_HZ, mpu.gyro_fifo_rate, getTimestamp()); + int fd = open(mpu.gyro_fifo_rate, O_RDWR); + res = errno; + if(fd < 0) { + LOGE("HAL:open of %s failed with '%s' (%d)", + mpu.gyro_fifo_rate, strerror(res), res); + return res; + } + res = write_attribute_sensor(fd, HW_GYRO_RATE_HZ); + if(res < 0) { + LOGE("HAL:write_attribute_sensor : error writing %s with %d", + mpu.gyro_fifo_rate, HW_GYRO_RATE_HZ); + return res; + } + + // Setting LPF is deprecated + + return 0; +} + +/* this applies to BMA250 Input Subsystem Driver only */ +int MPLSensor::setAccelInitialState() +{ + VFUNC_LOG; + + struct input_absinfo absinfo_x; + struct input_absinfo absinfo_y; + struct input_absinfo absinfo_z; + float value; + if (!ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_X), &absinfo_x) && + !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Y), &absinfo_y) && + !ioctl(accel_fd, EVIOCGABS(EVENT_TYPE_ACCEL_Z), &absinfo_z)) { + value = absinfo_x.value; + mPendingEvents[Accelerometer].data[0] = value * CONVERT_A_X; + value = absinfo_y.value; + mPendingEvents[Accelerometer].data[1] = value * CONVERT_A_Y; + value = absinfo_z.value; + mPendingEvents[Accelerometer].data[2] = value * CONVERT_A_Z; + } + return 0; +} + +int MPLSensor::onPower(int en) +{ + VFUNC_LOG; + + int res; + + int curr_power_state; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + en, mpu.power_state, getTimestamp()); + res = read_sysfs_int(mpu.power_state, &curr_power_state); + if (res < 0) { + LOGE("HAL:Error reading power state"); + // will set power_state anyway + curr_power_state = -1; + } + if (en != curr_power_state) { + if((res = write_sysfs_int(mpu.power_state, en)) < 0) { + LOGE("HAL:Couldn't write power state"); + } + } else { + LOGV_IF(EXTRA_VERBOSE, + "HAL:Power state already enable/disable curr=%d new=%d", + curr_power_state, en); + } + return res; +} + +int MPLSensor::onDMP(int en) +{ + VFUNC_LOG; + + int res = -1; + int status; + + //Sequence to enable DMP + //1. Turn On power if not already on + //2. Load DMP image if not already loaded + //3. Either Gyro or Accel must be enabled/configured before next step + //4. Enable DMP + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", + mpu.firmware_loaded, getTimestamp()); + res = read_sysfs_int(mpu.firmware_loaded, &status); + if (res < 0){ + LOGE("HAL:ERR can't get firmware_loaded status"); + return res; + } + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs: %s status=%d", mpu.firmware_loaded, status); + + if (status) { + //Write only if curr DMP state <> request + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:cat %s (%lld)", + mpu.dmp_on, getTimestamp()); + if (read_sysfs_int(mpu.dmp_on, &status) < 0) { + LOGE("HAL:ERR can't read DMP state"); + } else if (status != en) { + res = write_sysfs_int(mpu.dmp_on, en); + //Enable DMP interrupt + if (write_sysfs_int(mpu.dmp_int_on, en) < 0) { + LOGE("HAL:ERR can't en/dis DMP interrupt"); + } + } else { + res = 0; //DMP already set as requested + } + } else { + LOGE("HAL:ERR No DMP image"); + } + return res; +} + +int MPLSensor::checkLPQuaternion(void) +{ + VFUNC_LOG; + + return ((mFeatureActiveMask & INV_DMP_QUATERNION)? 1:0); +} + +int MPLSensor::enableLPQuaternion(int en) +{ + VFUNC_LOG; + + if (!en) { + enableQuaternionData(0); + onDMP(0); + mFeatureActiveMask &= ~INV_DMP_QUATERNION; + LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat disabled"); + } else { + if (enableQuaternionData(1) < 0 || onDMP(1) < 0) { + LOGE("HAL:ERR can't enable LP Quaternion"); + } else { + mFeatureActiveMask |= INV_DMP_QUATERNION; + LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat enabled"); + } + } + return 0; +} + +int MPLSensor::enableQuaternionData(int en) +{ + int res = 0; + VFUNC_LOG; + + // Enable DMP quaternion + res = write_sysfs_int(mpu.quaternion_on, en); + + if (!en) { + LOGV_IF(PROCESS_VERBOSE, "HAL:Disabling quat scan elems"); + } else { + + LOGV_IF(PROCESS_VERBOSE, "HAL:Enabling quat scan elems"); + } + write_sysfs_int(mpu.in_quat_r_en, en); + write_sysfs_int(mpu.in_quat_x_en, en); + write_sysfs_int(mpu.in_quat_y_en, en); + write_sysfs_int(mpu.in_quat_z_en, en); + + LOGV_IF(EXTRA_VERBOSE, "HAL:DMP quaternion data was turned off"); + + if (!en) { + inv_quaternion_sensor_was_turned_off(); + } + + return res; +} + +int MPLSensor::enableTap(int /*en*/) +{ + VFUNC_LOG; + + return 0; +} + +int MPLSensor::enableFlick(int /*en*/) +{ + VFUNC_LOG; + + return 0; +} + +int MPLSensor::enablePedometer(int /*en*/) +{ + VFUNC_LOG; + + return 0; +} + +int MPLSensor::masterEnable(int en) +{ + VFUNC_LOG; + return write_sysfs_int(mpu.chip_enable, en); +} + +int MPLSensor::enableGyro(int en) +{ + VFUNC_LOG; + + /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ + int res; + + /* need to also turn on/off the master enable */ + res = write_sysfs_int(mpu.gyro_enable, en); + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_gyro_was_turned_off"); + inv_gyro_was_turned_off(); + } else { + write_sysfs_int(mpu.gyro_x_fifo_enable, en); + write_sysfs_int(mpu.gyro_y_fifo_enable, en); + res = write_sysfs_int(mpu.gyro_z_fifo_enable, en); + } + + return res; +} + +int MPLSensor::enableAccel(int en) +{ + VFUNC_LOG; + + /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ + int res; + + /* need to also turn on/off the master enable */ + res = write_sysfs_int(mpu.accel_enable, en); + + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_accel_was_turned_off"); + inv_accel_was_turned_off(); + } else { + write_sysfs_int(mpu.accel_x_fifo_enable, en); + write_sysfs_int(mpu.accel_y_fifo_enable, en); + res = write_sysfs_int(mpu.accel_z_fifo_enable, en); + } + + return res; +} + +int MPLSensor::enableCompass(int en) +{ + VFUNC_LOG; + + int res = mCompassSensor->enable(ID_M, en); + if (!en) { + LOGV_IF(EXTRA_VERBOSE, "HAL:MPL:inv_compass_was_turned_off"); + inv_compass_was_turned_off(); + } + return res; +} + +void MPLSensor::computeLocalSensorMask(int enabled_sensors) +{ + VFUNC_LOG; + + do { + if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { + LOGV_IF(ENG_VERBOSE, "FUSION ENABLED"); + mLocalSensorMask = ALL_MPL_SENSORS_NP; + break; + } + + if(!A_ENABLED && !M_ENABLED && !GY_ENABLED) { + /* Invensense compass cal */ + LOGV_IF(ENG_VERBOSE, "ALL DISABLED"); + mLocalSensorMask = 0; + break; + } + + if (GY_ENABLED) { + LOGV_IF(ENG_VERBOSE, "G ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_GYRO; + } else { + LOGV_IF(ENG_VERBOSE, "G DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_GYRO; + } + + if (A_ENABLED) { + LOGV_IF(ENG_VERBOSE, "A ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_ACCEL; + } else { + LOGV_IF(ENG_VERBOSE, "A DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_ACCEL; + } + + /* Invensense compass calibration */ + if (M_ENABLED) { + LOGV_IF(ENG_VERBOSE, "M ENABLED"); + mLocalSensorMask |= INV_THREE_AXIS_COMPASS; + } else { + LOGV_IF(ENG_VERBOSE, "M DISABLED"); + mLocalSensorMask &= ~INV_THREE_AXIS_COMPASS; + } + } while (0); +} + +int MPLSensor::enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)) { + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - %s %s", en ? "enabled" : "disable", name); + return (this->*enabler)(en); +} + +int MPLSensor::enableSensors(unsigned long sensors, int /*en*/, uint32_t changed) { + VFUNC_LOG; + + inv_error_t res = -1; + bool store_cal = false; + bool ext_compass_changed = false; + + // Sequence to enable or disable a sensor + // 1. enable Power state + // 2. reset master enable (=0) + // 3. enable or disable a sensor + // 4. set master enable (=1) + + pthread_mutex_lock(&GlobalHalMutex); + + uint32_t all_changeables = (1 << Gyro) | (1 << RawGyro) | (1 << Accelerometer) + | (1 << MagneticField); + uint32_t all_integrated_changeables = all_changeables; + + if (!mCompassSensor->isIntegrated()) { + ext_compass_changed = changed & (1 << MagneticField); + all_integrated_changeables = all_changeables & ~(1 << MagneticField); + } + + if (isLowPowerQuatEnabled() || (changed & all_integrated_changeables)) { + /* ensure power state is on */ + onPower(1); + + /* reset master enable */ + res = masterEnable(0); + if(res < 0) { + goto unlock_res; + } + } + + LOGV_IF(PROCESS_VERBOSE, "HAL:enableSensors - sensors: 0x%0x", (unsigned int)sensors); + + if (changed & ((1 << Gyro) | (1 << RawGyro))) { + res = enableOneSensor(sensors & INV_THREE_AXIS_GYRO, "gyro", &MPLSensor::enableGyro); + if(res < 0) { + goto unlock_res; + } + } + + if (changed & (1 << Accelerometer)) { + res = enableOneSensor(sensors & INV_THREE_AXIS_ACCEL, "accel", &MPLSensor::enableAccel); + if(res < 0) { + goto unlock_res; + } + } + + if (changed & (1 << MagneticField)) { + /* Invensense compass calibration */ + res = enableOneSensor(sensors & INV_THREE_AXIS_COMPASS, "compass", &MPLSensor::enableCompass); + if(res < 0) { + goto unlock_res; + } + } + + if ( isLowPowerQuatEnabled() ) { + // Enable LP Quat + if ((mEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity)))) { + if (!(changed & all_integrated_changeables)) { + /* ensure power state is on */ + onPower(1); + /* reset master enable */ + res = masterEnable(0); + if(res < 0) { + goto unlock_res; + } + } + if (!checkLPQuaternion()) { + enableLPQuaternion(1); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:LP Quat already enabled"); + } + } else if (checkLPQuaternion()) { + enableLPQuaternion(0); + } + } + + if (changed & all_integrated_changeables) { + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { + if ( isLowPowerQuatEnabled() || + (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) ) { + // disable DMP event interrupt only (w/ data interrupt) + if (write_sysfs_int(mpu.dmp_event_int_on, 0) < 0) { + res = -1; + LOGE("HAL:ERR can't disable DMP event interrupt"); + goto unlock_res; + } + } + + if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { + // enable DMP + onDMP(1); + + res = enableAccel(1); + if(res < 0) { + goto unlock_res; + } + + if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { + res = turnOffAccelFifo(); + } + if(res < 0) { + goto unlock_res; + } + } + + res = masterEnable(1); + if(res < 0) { + goto unlock_res; + } + } else { // all sensors idle -> reduce power + if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { + // enable DMP + onDMP(1); + // enable DMP event interrupt only (no data interrupt) + if (write_sysfs_int(mpu.dmp_event_int_on, 1) < 0) { + res = -1; + LOGE("HAL:ERR can't enable DMP event interrupt"); + } + res = enableAccel(1); + if(res < 0) { + goto unlock_res; + } + if ((sensors & INV_THREE_AXIS_ACCEL) == 0) { + res = turnOffAccelFifo(); + } + if(res < 0) { + goto unlock_res; + } + res = masterEnable(1); + if(res < 0) { + goto unlock_res; + } + } + else { + res = onPower(0); + if(res < 0) { + goto unlock_res; + } + } + store_cal = true; + } + } else if (ext_compass_changed && + !(sensors & (INV_THREE_AXIS_GYRO | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * (!mCompassSensor->isIntegrated()))))) { + store_cal = true; + } + + if (store_cal || ((changed & all_changeables) != all_changeables)) { + storeCalibration(); + } + +unlock_res: + pthread_mutex_unlock(&GlobalHalMutex); + return res; +} + +/* Store calibration file */ +void MPLSensor::storeCalibration() +{ + if(mHaveGoodMpuCal || mAccelAccuracy >= 2 || mCompassAccuracy >= 3) { + int res = inv_store_calibration(); + if (res) { + LOGE("HAL:Cannot store calibration on file"); + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:Cal file updated"); + } + } +} + +void MPLSensor::cbProcData() +{ + mNewData = 1; + mSampleCount++; + LOGV_IF(EXTRA_VERBOSE, "HAL:new data"); +} + +/* these handlers transform mpl data into one of the Android sensor types */ +int MPLSensor::gyroHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_gyroscope(s->gyro.v, &status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:gyro data : %+f %+f %+f -- %lld - %d", + s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::rawGyroHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_gyroscope_raw(s->gyro.v, &status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:raw gyro data : %+f %+f %+f -- %lld - %d", + s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::accelHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_accelerometer( + s->acceleration.v, &status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:accel data : %+f %+f %+f -- %lld - %d", + s->acceleration.v[0], s->acceleration.v[1], s->acceleration.v[2], + s->timestamp, update); + mAccelAccuracy = status; + return update; +} + +int MPLSensor::compassHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_magnetic_field( + s->magnetic.v, &s->magnetic.status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:compass data: %+f %+f %+f -- %lld - %d", + s->magnetic.v[0], s->magnetic.v[1], s->magnetic.v[2], s->timestamp, update); + mCompassAccuracy = s->magnetic.status; + return update; +} + +int MPLSensor::rvHandler(sensors_event_t* s) +{ + // rotation vector does not have an accuracy or status + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_rotation_vector(s->data, &status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:rv data: %+f %+f %+f %+f - %+lld - %d", + s->data[0], s->data[1], s->data[2], s->data[3], s->timestamp, update); + return update; +} + +int MPLSensor::laHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_linear_acceleration( + s->gyro.v, &status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:la data: %+f %+f %+f - %lld - %d", + s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::gravHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int8_t status; + int update; + update = inv_get_sensor_type_gravity(s->gyro.v, &status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:gr data: %+f %+f %+f - %lld - %d", + s->gyro.v[0], s->gyro.v[1], s->gyro.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::orienHandler(sensors_event_t* s) +{ + VHANDLER_LOG; + int update; + update = inv_get_sensor_type_orientation( + s->orientation.v, &s->orientation.status, &s->timestamp); + LOGV_IF(HANDLER_DATA, "HAL:or data: %f %f %f - %lld - %d", + s->orientation.v[0], s->orientation.v[1], s->orientation.v[2], s->timestamp, update); + return update; +} + +int MPLSensor::enable(int32_t handle, int en) +{ + VFUNC_LOG; + + android::String8 sname; + int what = -1, err = 0; + + switch (handle) { + case ID_SO: + sname = "Screen Orientation"; + LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle, + (mDmpOrientationEnabled? "en": "dis"), + (en? "en" : "dis")); + enableDmpOrientation(en && isDmpDisplayOrientationOn()); + /* TODO: stop manually testing/using 0 and 1 instead of + * false and true, but just use 0 and non-0. + * This allows passing 0 and non-0 ints around instead of + * having to convert to 1 and test against 1. + */ + mDmpOrientationEnabled = en; + return 0; + case ID_A: + what = Accelerometer; + sname = "Accelerometer"; + break; + case ID_M: + what = MagneticField; + sname = "MagneticField"; + break; + case ID_O: + what = Orientation; + sname = "Orientation"; + break; + case ID_GY: + what = Gyro; + sname = "Gyro"; + break; + case ID_RG: + what = RawGyro; + sname = "RawGyro"; + break; + case ID_GR: + what = Gravity; + sname = "Gravity"; + break; + case ID_RV: + what = RotationVector; + sname = "RotationVector"; + break; + case ID_LA: + what = LinearAccel; + sname = "LinearAccel"; + break; + default: //this takes care of all the gestures + what = handle; + sname = "Others"; + break; + } + + if (uint32_t(what) >= numSensors) + return -EINVAL; + + int newState = en ? 1 : 0; + unsigned long sen_mask; + + LOGV_IF(PROCESS_VERBOSE, "HAL:enable - sensor %s (handle %d) %s -> %s", sname.string(), handle, + ((mEnabled & (1 << what)) ? "en" : "dis"), + ((uint32_t(newState) << what) ? "en" : "dis")); + LOGV_IF(PROCESS_VERBOSE, + "HAL:%s sensor state change what=%d", sname.string(), what); + + if ((uint32_t(newState) << what) != (mEnabled & (1 << what))) { + short flags = newState; + uint32_t lastEnabled = mEnabled, changed = 0; + + mEnabled &= ~(1 << what); + mEnabled |= (uint32_t(flags) << what); + + LOGV_IF(PROCESS_VERBOSE, "HAL:handle = %d", handle); + LOGV_IF(PROCESS_VERBOSE, "HAL:flags = %d", flags); + computeLocalSensorMask(mEnabled); + LOGV_IF(PROCESS_VERBOSE, "HAL:enable : mEnabled = %d", mEnabled); + sen_mask = mLocalSensorMask & mMasterSensorMask; + mSensorMask = sen_mask; + LOGV_IF(PROCESS_VERBOSE, "HAL:sen_mask= 0x%0lx", sen_mask); + + switch (what) { + case Gyro: + case RawGyro: + case Accelerometer: + case MagneticField: + if (!(mEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity))) && + ((lastEnabled & (1 << what)) != (mEnabled & (1 << what)))) { + changed |= (1 << what); + } + break; + + case Orientation: + case RotationVector: + case LinearAccel: + case Gravity: + if ((en && !(lastEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity)))) || + (!en && !(mEnabled & ((1 << Orientation) | (1 << RotationVector) | + (1 << LinearAccel) | (1 << Gravity))))) { + for (int i = Gyro; i <= MagneticField; i++) { + if (!(mEnabled & (1 << i))) { + changed |= (1 << i); + } + } + } + break; + } + LOGV_IF(PROCESS_VERBOSE, "HAL:changed = %d", changed); + enableSensors(sen_mask, flags, changed); + } + +#ifdef INV_PLAYBACK_DBG + /* apparently the logging needs to be go through this sequence + to properly flush the log file */ + inv_turn_off_data_logging(); + fclose(logfile); + logfile = fopen("/data/playback.bin", "ab"); + if (logfile) + inv_turn_on_data_logging(logfile); +#endif + + return err; +} + +int MPLSensor::setDelay(int32_t handle, int64_t ns) +{ + VFUNC_LOG; + + android::String8 sname; + int what = -1; + + switch (handle) { + case ID_SO: + return update_delay(); + case ID_A: + what = Accelerometer; + sname = "Accelerometer"; + break; + case ID_M: + what = MagneticField; + sname = "MagneticField"; + break; + case ID_O: + what = Orientation; + sname = "Orientation"; + break; + case ID_GY: + what = Gyro; + sname = "Gyro"; + break; + case ID_RG: + what = RawGyro; + sname = "RawGyro"; + break; + case ID_GR: + what = Gravity; + sname = "Gravity"; + break; + case ID_RV: + what = RotationVector; + sname = "RotationVector"; + break; + case ID_LA: + what = LinearAccel; + sname = "LinearAccel"; + break; + default: // this takes care of all the gestures + what = handle; + sname = "Others"; + break; + } + +#if 0 + // skip the 1st call for enalbing sensors called by ICS/JB sensor service + static int counter_delay = 0; + if (!(mEnabled & (1 << what))) { + counter_delay = 0; + } else { + if (++counter_delay == 1) { + return 0; + } + else { + counter_delay = 0; + } + } +#endif + + if (uint32_t(what) >= numSensors) + return -EINVAL; + + if (ns < 0) + return -EINVAL; + + LOGV_IF(PROCESS_VERBOSE, "setDelay : %llu ns, (%.2f Hz)", ns, 1000000000.f / ns); + + // limit all rates to reasonable ones */ + if (ns < 5000000LL) { + ns = 5000000LL; + } + + /* store request rate to mDelays array for each sensor */ + mDelays[what] = ns; + + switch (what) { + case Gyro: + case RawGyro: + case Accelerometer: + for (int i = Gyro; i <= Accelerometer + mCompassSensor->isIntegrated(); + i++) { + if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { + LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); + return 0; + } + } + break; + + case MagneticField: + if (mCompassSensor->isIntegrated() && + (((mEnabled & (1 << Gyro)) && ns > mDelays[Gyro]) || + ((mEnabled & (1 << RawGyro)) && ns > mDelays[RawGyro]) || + ((mEnabled & (1 << Accelerometer)) && ns > mDelays[Accelerometer]))) { + LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to gyro/accel"); + return 0; + } + break; + + case Orientation: + case RotationVector: + case LinearAccel: + case Gravity: + if (isLowPowerQuatEnabled()) { + LOGV_IF(PROCESS_VERBOSE, "HAL:need to update delay due to LPQ"); + break; + } + + for (int i = 0; i < numSensors; i++) { + if (i != what && (mEnabled & (1 << i)) && ns > mDelays[i]) { + LOGV_IF(PROCESS_VERBOSE, "HAL:ignore delay set due to sensor %d", i); + return 0; + } + } + break; + } + + int res = update_delay(); + return res; +} + +int MPLSensor::update_delay() { + VHANDLER_LOG; + + int res = 0; + int64_t got; + + pthread_mutex_lock(&GlobalHalMutex); + if (mEnabled) { + int64_t wanted = 1000000000; + int64_t wanted_3rd_party_sensor = 1000000000; + + // Sequence to change sensor's FIFO rate + // 1. enable Power state + // 2. reset master enable + // 3. Update delay + // 4. set master enable + + // ensure power on + onPower(1); + + // reset master enable + masterEnable(0); + + /* search the minimum delay requested across all enabled sensors */ + for (int i = 0; i < numSensors; i++) { + if (mEnabled & (1 << i)) { + int64_t ns = mDelays[i]; + wanted = wanted < ns ? wanted : ns; + } + } + + // same delay for 3rd party Accel or Compass + wanted_3rd_party_sensor = wanted; + + /* mpl rate in us in future maybe different for + gyro vs compass vs accel */ + int rateInus = (int)wanted / 1000LL; + int mplGyroRate = rateInus; + int mplAccelRate = rateInus; + int mplCompassRate = rateInus; + + LOGV_IF(PROCESS_VERBOSE, "HAL:wanted rate for all sensors : " + "%llu ns, mpl rate: %d us, (%.2f Hz)", + wanted, rateInus, 1000000000.f / wanted); + + /* set rate in MPL */ + /* compass can only do 100Hz max */ + inv_set_gyro_sample_rate(mplGyroRate); + inv_set_accel_sample_rate(mplAccelRate); + inv_set_compass_sample_rate(mplCompassRate); +#ifdef LIBMLLITE_FROM_SOURCE + inv_set_linear_acceleration_sample_rate(rateInus); + inv_set_gravity_sample_rate(rateInus); +#endif + + /* TODO: Test 200Hz */ + // inv_set_gyro_sample_rate(5000); + LOGV_IF(PROCESS_VERBOSE, "HAL:MPL gyro sample rate: %d", mplGyroRate); + LOGV_IF(PROCESS_VERBOSE, "HAL:MPL accel sample rate: %d", mplAccelRate); + LOGV_IF(PROCESS_VERBOSE, "HAL:MPL compass sample rate: %d", mplCompassRate); + + int enabled_sensors = mEnabled; + int tempFd = -1; + if (LA_ENABLED || GR_ENABLED || RV_ENABLED || O_ENABLED) { + if (isLowPowerQuatEnabled() || + (isDmpDisplayOrientationOn() && mDmpOrientationEnabled)) { + bool setDMPrate= 0; + // Set LP Quaternion sample rate if enabled + if (checkLPQuaternion()) { + if (wanted < RATE_200HZ) { + enableLPQuaternion(0); + } else { + inv_set_quat_sample_rate(rateInus); + setDMPrate= 1; + } + } + + if (checkDMPOrientation() || setDMPrate==1) { + getDmpRate(&wanted); + } + } + + int64_t tempRate = wanted; + LOGV_IF(EXTRA_VERBOSE, "HAL:setDelay - Fusion"); + //nsToHz + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / tempRate, mpu.gyro_fifo_rate, + getTimestamp()); + tempFd = open(mpu.gyro_fifo_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / tempRate); + if(res < 0) { + LOGE("HAL:GYRO update delay error"); + } + + //nsToHz (BMA250) + if(USE_THIRD_PARTY_ACCEL) { + LOGV_IF(SYSFS_VERBOSE, "echo %lld > %s (%lld)", + wanted_3rd_party_sensor / 1000000L, mpu.accel_fifo_rate, + getTimestamp()); + tempFd = open(mpu.accel_fifo_rate, O_RDWR); + res = write_attribute_sensor(tempFd, wanted_3rd_party_sensor / 1000000L); + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + } + + if (!mCompassSensor->isIntegrated()) { + LOGV_IF(PROCESS_VERBOSE, "HAL:Ext compass rate %.2f Hz", 1000000000.f / wanted_3rd_party_sensor); + mCompassSensor->setDelay(ID_M, wanted_3rd_party_sensor); + got = mCompassSensor->getDelay(ID_M); + inv_set_compass_sample_rate(got / 1000); + } + + } else { + + if (GY_ENABLED) { + wanted = (mDelays[Gyro] <= mDelays[RawGyro]? + (mEnabled & (1 << Gyro)? mDelays[Gyro]: mDelays[RawGyro]): + (mEnabled & (1 << RawGyro)? mDelays[RawGyro]: mDelays[Gyro])); + + if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { + getDmpRate(&wanted); + } + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / wanted, mpu.gyro_fifo_rate, getTimestamp()); + tempFd = open(mpu.gyro_fifo_rate, O_RDWR); + res = write_attribute_sensor(tempFd, 1000000000.f / wanted); + LOGE_IF(res < 0, "HAL:GYRO update delay error"); + } + + if (A_ENABLED) { /* else if because there is only 1 fifo rate for MPUxxxx */ + if (GY_ENABLED && mDelays[Gyro] < mDelays[Accelerometer]) { + wanted = mDelays[Gyro]; + } + else if (GY_ENABLED && mDelays[RawGyro] < mDelays[Accelerometer]) { + wanted = mDelays[RawGyro]; + + } else { + wanted = mDelays[Accelerometer]; + } + + if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { + getDmpRate(&wanted); + } + + /* TODO: use function pointers to calculate delay value specific to vendor */ + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %.0f > %s (%lld)", + 1000000000.f / wanted, mpu.accel_fifo_rate, getTimestamp()); + tempFd = open(mpu.accel_fifo_rate, O_RDWR); + if(USE_THIRD_PARTY_ACCEL) { + //BMA250 in ms + res = write_attribute_sensor(tempFd, wanted / 1000000L); + } + else { + //MPUxxxx in hz + res = write_attribute_sensor(tempFd, 1000000000.f/wanted); + } + LOGE_IF(res < 0, "HAL:ACCEL update delay error"); + } + + /* Invensense compass calibration */ + if (M_ENABLED) { + if (!mCompassSensor->isIntegrated()) { + wanted = mDelays[MagneticField]; + } else { + if (GY_ENABLED && mDelays[Gyro] < mDelays[MagneticField]) { + wanted = mDelays[Gyro]; + } + else if (GY_ENABLED && mDelays[RawGyro] < mDelays[MagneticField]) { + wanted = mDelays[RawGyro]; + } else if (A_ENABLED && mDelays[Accelerometer] < mDelays[MagneticField]) { + wanted = mDelays[Accelerometer]; + } else { + wanted = mDelays[MagneticField]; + } + + if (isDmpDisplayOrientationOn() && mDmpOrientationEnabled) { + getDmpRate(&wanted); + } + } + + mCompassSensor->setDelay(ID_M, wanted); + got = mCompassSensor->getDelay(ID_M); + inv_set_compass_sample_rate(got / 1000); + } + + } + + unsigned long sensors = mLocalSensorMask & mMasterSensorMask; + if (sensors & + (INV_THREE_AXIS_GYRO + | INV_THREE_AXIS_ACCEL + | (INV_THREE_AXIS_COMPASS * mCompassSensor->isIntegrated()))) { + res = masterEnable(1); + } else { // all sensors idle -> reduce power + res = onPower(0); + } + } + pthread_mutex_unlock(&GlobalHalMutex); + return res; +} + +/* For Third Party Accel Input Subsystem Drivers only */ +/* TODO: FIX! data is not used and count not decremented, results is hardcoded to 0 */ +int MPLSensor::readAccelEvents(sensors_event_t* /*data*/, int count) +{ + VHANDLER_LOG; + + if (count < 1) + return -EINVAL; + + ssize_t n = mAccelInputReader.fill(accel_fd); + if (n < 0) { + LOGE("HAL:missed accel events, exit"); + return n; + } + + int numEventReceived = 0; + input_event const* event; + int done = 0; + + while (!done && count && mAccelInputReader.readEvent(&event)) { + int type = event->type; + if (type == EV_ABS) { + if (event->code == EVENT_TYPE_ACCEL_X) { + mPendingMask |= 1 << Accelerometer; + mCachedAccelData[0] = event->value; + } else if (event->code == EVENT_TYPE_ACCEL_Y) { + mPendingMask |= 1 << Accelerometer; + mCachedAccelData[1] = event->value; + } else if (event->code == EVENT_TYPE_ACCEL_Z) { + mPendingMask |= 1 << Accelerometer; + mCachedAccelData[2] =event-> value; + } + } else if (type == EV_SYN) { + done = 1; + if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { + inv_build_accel(mCachedAccelData, 0, getTimestamp()); + } + } else { + LOGE("HAL:AccelSensor: unknown event (type=%d, code=%d)", + type, event->code); + } + mAccelInputReader.next(); + } + + return numEventReceived; +} + +/** + * Should be called after reading at least one of gyro + * compass or accel data. (Also okay for handling all of them). + * @returns 0, if successful, error number if not. + */ +/* TODO: This should probably be called "int readEvents(...)" + * and readEvents() called "void cacheData(void)". + */ +int MPLSensor::executeOnData(sensors_event_t* data, int count) +{ + VFUNC_LOG; + + inv_execute_on_data(); + + int numEventReceived = 0; + + long msg; + msg = inv_get_message_level_0(1); + if (msg) { + if (msg & INV_MSG_MOTION_EVENT) { + LOGV_IF(PROCESS_VERBOSE, "HAL:**** Motion ****\n"); + } + if (msg & INV_MSG_NO_MOTION_EVENT) { + LOGV_IF(PROCESS_VERBOSE, "HAL:***** No Motion *****\n"); + /* after the first no motion, the gyro should be + calibrated well */ + mGyroAccuracy = SENSOR_STATUS_ACCURACY_HIGH; + /* if gyros are on and we got a no motion, set a flag + indicating that the cal file can be written. */ + mHaveGoodMpuCal = true; + } + } + + // load up virtual sensors + for (int i = 0; i < numSensors; i++) { + int update; + if (mEnabled & (1 << i)) { + update = CALL_MEMBER_FN(this, mHandlers[i])(mPendingEvents + i); + mPendingMask |= (1 << i); + + if (update && (count > 0)) { + *data++ = mPendingEvents[i]; + count--; + numEventReceived++; + } + } + } + + return numEventReceived; +} + +// collect data for MPL (but NOT sensor service currently), from driver layer +/* TODO: FIX! data and count are not used, results is hardcoded to 0 */ +/* TODO: This should probably be called "void cacheEvents(void)" + * And executeOnData() should be int readEvents(data,count) + */ +int MPLSensor::readEvents(sensors_event_t* /*data*/, int /*count*/) { + + + int lp_quaternion_on = 0, nbyte; + int i, mask = 0, numEventReceived = 0, + sensors = ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 : 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 : 0) + + (((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated())? 1 : 0); + char *rdata = mIIOBuffer; + + nbyte= (8 * sensors + 8) * 1; + + if (isLowPowerQuatEnabled()) { + lp_quaternion_on = checkLPQuaternion(); + if (lp_quaternion_on) { + nbyte += sizeof(mCachedQuaternionData); //currently 16 bytes for Q data + } + } + + ssize_t rsize = read(iio_fd, rdata, nbyte); + if (sensors == 0) { + rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); + } + +#ifdef TESTING + LOGI("get one sample of IIO data with size: %d", rsize); + LOGI("sensors: %d", sensors); + + LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_GYRO, "gyro x/y/z: %d/%d/%d", + *((short *) (rdata + 0)), *((short *) (rdata + 2)), + *((short *) (rdata + 4))); + LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_ACCEL, "accel x/y/z: %d/%d/%d", + *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), + *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))), + *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); + + LOGI_IF(mLocalSensorMask & INV_THREE_AXIS_COMPASS & + mCompassSensor->isIntegrated(), "compass x/y/z: %d/%d/%d", + *((short *) (rdata + 0 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), + *((short *) (rdata + 2 + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))), + *((short *) (rdata + 4) + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 6: 0))); +#endif + + if (rsize != nbyte) { + LOGE("HAL:ERR Full data packet was not read. rsize=%zd nbyte=%d sensors=%d errno=%d(%s)", + rsize, nbyte, sensors, errno, strerror(errno)); + rsize = read(iio_fd, rdata, sizeof(mIIOBuffer)); + return -1; + } + + if (isLowPowerQuatEnabled() && lp_quaternion_on) { + + for (i=0; i< 4; i++) { + mCachedQuaternionData[i]= *(long*)rdata; + rdata += sizeof(long); + } + } + + for (i = 0; i < 3; i++) { + if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { + mCachedGyroData[i] = *((short *) (rdata + i * 2)); + } + if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { + mCachedAccelData[i] = *((short *) (rdata + i * 2 + + ((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 6: 0))); + } + if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated()) { + mCachedCompassData[i] = *((short *) (rdata + i * 2 + 6 * (sensors - 1))); + } + } + + mask |= (((mLocalSensorMask & INV_THREE_AXIS_GYRO)? 1 << Gyro: 0) + + ((mLocalSensorMask & INV_THREE_AXIS_ACCEL)? 1 << Accelerometer: 0)); + if ((mLocalSensorMask & INV_THREE_AXIS_COMPASS) && mCompassSensor->isIntegrated() && + (mCachedCompassData[0] != 0 || mCachedCompassData[1] != 0 || mCachedCompassData[0] != 0)) { + mask |= 1 << MagneticField; + } + + mSensorTimestamp = *((long long *) (rdata + 8 * sensors)); + if (mCompassSensor->isIntegrated()) { + mCompassTimestamp = mSensorTimestamp; + } + + if (mask & (1 << Gyro)) { + // send down temperature every 0.5 seconds + // with timestamp measured in "driver" layer + if(mSensorTimestamp - mTempCurrentTime >= 500000000LL) { + mTempCurrentTime = mSensorTimestamp; + long long temperature[2]; + if(inv_read_temperature(temperature) == 0) { + LOGV_IF(INPUT_DATA, + "HAL:inv_read_temperature = %lld, timestamp= %lld", + temperature[0], temperature[1]); + inv_build_temp(temperature[0], temperature[1]); + } +#ifdef TESTING + long bias[3], temp, temp_slope[3]; + inv_get_gyro_bias(bias, &temp); + inv_get_gyro_ts(temp_slope); + + LOGI("T: %.3f " + "GB: %+13f %+13f %+13f " + "TS: %+13f %+13f %+13f " + "\n", + (float)temperature[0] / 65536.f, + (float)bias[0] / 65536.f / 16.384f, + (float)bias[1] / 65536.f / 16.384f, + (float)bias[2] / 65536.f / 16.384f, + temp_slope[0] / 65536.f, + temp_slope[1] / 65536.f, + temp_slope[2] / 65536.f); +#endif + } + + mPendingMask |= 1 << Gyro; + mPendingMask |= 1 << RawGyro; + + if (mLocalSensorMask & INV_THREE_AXIS_GYRO) { + inv_build_gyro(mCachedGyroData, mSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:inv_build_gyro: %+8d %+8d %+8d - %lld", + mCachedGyroData[0], mCachedGyroData[1], + mCachedGyroData[2], mSensorTimestamp); + } + } + + if (mask & (1 << Accelerometer)) { + mPendingMask |= 1 << Accelerometer; + if (mLocalSensorMask & INV_THREE_AXIS_ACCEL) { + inv_build_accel(mCachedAccelData, 0, mSensorTimestamp); + LOGV_IF(INPUT_DATA, + "HAL:inv_build_accel: %+8ld %+8ld %+8ld - %lld", + mCachedAccelData[0], mCachedAccelData[1], + mCachedAccelData[2], mSensorTimestamp); + } + } + + if ((mask & (1 << MagneticField)) && mCompassSensor->isIntegrated()) { + int status = 0; + if (mCompassSensor->providesCalibration()) { + status = mCompassSensor->getAccuracy(); + status |= INV_CALIBRATED; + } + if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { + inv_build_compass(mCachedCompassData, status, + mCompassTimestamp); + LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], + mCachedCompassData[2], mCompassTimestamp); + } + } + + if (isLowPowerQuatEnabled() && lp_quaternion_on) { + + inv_build_quat(mCachedQuaternionData, 32 /*default 32 for now (16/32bits)*/, mSensorTimestamp); + LOGV_IF(INPUT_DATA, "HAL:inv_build_quat: %+8ld %+8ld %+8ld %+8ld - %lld", + mCachedQuaternionData[0], mCachedQuaternionData[1], + mCachedQuaternionData[2], mCachedQuaternionData[3], mSensorTimestamp); + } + + return numEventReceived; +} + +/* use for both MPUxxxx and third party compass */ +int MPLSensor::readCompassEvents(sensors_event_t* /*data*/, int count) +{ + VHANDLER_LOG; + + if (count < 1) + return -EINVAL; + + int numEventReceived = 0; + int done = 0; + + done = mCompassSensor->readSample(mCachedCompassData, &mCompassTimestamp); +#ifdef COMPASS_YAS53x + if (mCompassSensor->checkCoilsReset()) { + //Reset relevant compass settings + resetCompass(); + } +#endif + if (done > 0) { + int status = 0; + if (mCompassSensor->providesCalibration()) { + status = mCompassSensor->getAccuracy(); + status |= INV_CALIBRATED; + } + if (mLocalSensorMask & INV_THREE_AXIS_COMPASS) { + inv_build_compass(mCachedCompassData, status, + mCompassTimestamp); + LOGV_IF(INPUT_DATA, "HAL:inv_build_compass: %+8ld %+8ld %+8ld - %lld", + mCachedCompassData[0], mCachedCompassData[1], + mCachedCompassData[2], mCompassTimestamp); + } + } + + return numEventReceived; +} + +#ifdef COMPASS_YAS53x +int MPLSensor::resetCompass() +{ + VFUNC_LOG; + + //Reset compass cal if enabled + if (mFeatureActiveMask & INV_COMPASS_CAL) { + LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass cal"); + inv_init_vector_compass_cal(); + } + + //Reset compass fit if enabled + if (mFeatureActiveMask & INV_COMPASS_FIT) { + LOGV_IF(EXTRA_VERBOSE, "HAL:Reset compass fit"); + inv_init_compass_fit(); + } + + return 0; +} +#endif + +int MPLSensor::getFd() const +{ + VFUNC_LOG; + LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getFd returning %d", iio_fd); + return iio_fd; +} + +int MPLSensor::getAccelFd() const +{ + VFUNC_LOG; + LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getAccelFd returning %d", accel_fd); + return accel_fd; +} + +int MPLSensor::getCompassFd() const +{ + VFUNC_LOG; + int fd = mCompassSensor->getFd(); + LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getCompassFd returning %d", fd); + return fd; +} + +int MPLSensor::turnOffAccelFifo() { + int i, res; + char *accel_fifo_enable[3] = {mpu.accel_x_fifo_enable, + mpu.accel_y_fifo_enable, mpu.accel_z_fifo_enable}; + + for (i = 0; i < 3; i++) { + res = write_sysfs_int(accel_fifo_enable[i], 0); + if (res < 0) { + return res; + } + } + return 0; +} + +int MPLSensor::enableDmpOrientation(int en) +{ + VFUNC_LOG; + /* TODO: FIX error handling. Handle or ignore it appropriately for hw. */ + int res = 0; + int enabled_sensors = mEnabled; + + if (isMpu3050()) + return res; + + pthread_mutex_lock(&GlobalHalMutex); + + // on power if not already On + res = onPower(1); + // reset master enable + res = masterEnable(0); + + if (en) { + //Enable DMP orientation + if (write_sysfs_int(mpu.display_orientation_on, en) < 0) { + LOGE("HAL:ERR can't enable Android orientation"); + res = -1; // indicate an err + } + + // open DMP Orient Fd + res = openDmpOrientFd(); + + // enable DMP + res = onDMP(1); + + // default DMP output rate to FIFO + if (write_sysfs_int(mpu.dmp_output_rate, 5) < 0) { + LOGE("HAL:ERR can't default DMP output rate"); + } + + // set DMP rate to 200Hz + if (write_sysfs_int(mpu.accel_fifo_rate, 200) < 0) { + res = -1; + LOGE("HAL:ERR can't set DMP rate to 200Hz"); + } + + // enable accel engine + res = enableAccel(1); + + // disable accel FIFO + if (!A_ENABLED) { + res = turnOffAccelFifo(); + } + + mFeatureActiveMask |= INV_DMP_DISPL_ORIENTATION; + } else { + // disable DMP + res = onDMP(0); + + // disable accel engine + if (!A_ENABLED) { + res = enableAccel(0); + } + } + + res = masterEnable(1); + pthread_mutex_unlock(&GlobalHalMutex); + return res; +} + +int MPLSensor::openDmpOrientFd() +{ + VFUNC_LOG; + + if (!isDmpDisplayOrientationOn() || dmp_orient_fd >= 0) { + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP display orientation disabled or file desc opened"); + return -1; + } + + dmp_orient_fd = open(mpu.event_display_orientation, O_RDONLY| O_NONBLOCK); + if (dmp_orient_fd < 0) { + LOGE("HAL:ERR couldn't open dmpOrient node"); + return -1; + } else { + LOGV_IF(PROCESS_VERBOSE, "HAL:dmp_orient_fd opened : %d", dmp_orient_fd); + return 0; + } +} + +int MPLSensor::closeDmpOrientFd() +{ + VFUNC_LOG; + if (dmp_orient_fd >= 0) + close(dmp_orient_fd); + return 0; +} + +int MPLSensor::dmpOrientHandler(int orient) +{ + VFUNC_LOG; + + LOGV_IF(PROCESS_VERBOSE, "HAL:orient %x", orient); + return 0; +} + +int MPLSensor::readDmpOrientEvents(sensors_event_t* data, int count) { + VFUNC_LOG; + + char dummy[4]; + int screen_orientation = 0; + FILE *fp; + + fp = fopen(mpu.event_display_orientation, "r"); + if (fp == NULL) { + LOGE("HAL:cannot open event_display_orientation"); + return 0; + } + fscanf(fp, "%d\n", &screen_orientation); + fclose(fp); + + int numEventReceived = 0; + + if (mDmpOrientationEnabled && count > 0) { + sensors_event_t temp; + + bzero(&temp, sizeof(temp)); /* Let's hope that SENSOR_TYPE_NONE is 0 */ + temp.version = sizeof(sensors_event_t); + temp.sensor = ID_SO; +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION + temp.type = SENSOR_TYPE_SCREEN_ORIENTATION; + temp.screen_orientation = screen_orientation; +#endif + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + temp.timestamp = (int64_t) ts.tv_sec * 1000000000 + ts.tv_nsec; + + *data++ = temp; + count--; + numEventReceived++; + } + + // read dummy data per driver's request + dmpOrientHandler(screen_orientation); + read(dmp_orient_fd, dummy, 4); + + return numEventReceived; +} + +int MPLSensor::getDmpOrientFd() +{ + VFUNC_LOG; + + LOGV_IF(EXTRA_VERBOSE, "MPLSensor::getDmpOrientFd returning %d", dmp_orient_fd); + return dmp_orient_fd; + +} + +int MPLSensor::checkDMPOrientation() +{ + VFUNC_LOG; + return ((mFeatureActiveMask & INV_DMP_DISPL_ORIENTATION) ? 1 : 0); +} + +int MPLSensor::getDmpRate(int64_t *wanted) +{ + if (checkDMPOrientation() || checkLPQuaternion()) { + // set DMP output rate to FIFO + write_sysfs_int(mpu.dmp_output_rate, 1000000000.f / *wanted); + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP FIFO rate %.2f Hz", 1000000000.f / *wanted); + + //DMP running rate must be @ 200Hz + *wanted= RATE_200HZ; + LOGV_IF(PROCESS_VERBOSE, "HAL:DMP rate= %.2f Hz", 1000000000.f / *wanted); + } + return 0; +} + +int MPLSensor::getPollTime() +{ + VHANDLER_LOG; + return mPollTime; +} + +bool MPLSensor::hasPendingEvents() const +{ + VHANDLER_LOG; + // if we are using the polling workaround, force the main + // loop to check for data every time + return (mPollTime != -1); +} + +/* TODO: support resume suspend when we gain more info about them*/ +void MPLSensor::sleepEvent() +{ + VFUNC_LOG; +} + +void MPLSensor::wakeEvent() +{ + VFUNC_LOG; +} + +int MPLSensor::inv_float_to_q16(float *fdata, long *ldata) +{ + VHANDLER_LOG; + + if (!fdata || !ldata) + return -1; + ldata[0] = (long)(fdata[0] * 65536.f); + ldata[1] = (long)(fdata[1] * 65536.f); + ldata[2] = (long)(fdata[2] * 65536.f); + return 0; +} + +int MPLSensor::inv_long_to_q16(long *fdata, long *ldata) +{ + VHANDLER_LOG; + + if (!fdata || !ldata) + return -1; + ldata[0] = (fdata[1] * 65536.f); + ldata[1] = (fdata[2] * 65536.f); + ldata[2] = (fdata[3] * 65536.f); + return 0; +} + +int MPLSensor::inv_float_to_round(float *fdata, long *ldata) +{ + VHANDLER_LOG; + + if (!fdata || !ldata) + return -1; + ldata[0] = (long)fdata[0]; + ldata[1] = (long)fdata[1]; + ldata[2] = (long)fdata[2]; + return 0; +} + +int MPLSensor::inv_float_to_round2(float *fdata, short *ldata) +{ + VHANDLER_LOG; + + if (!fdata || !ldata) + return -1; + ldata[0] = (short)fdata[0]; + ldata[1] = (short)fdata[1]; + ldata[2] = (short)fdata[2]; + return 0; +} + +int MPLSensor::inv_read_temperature(long long *data) +{ + VHANDLER_LOG; + + int count = 0; + char raw_buf[40]; + long raw = 0; + + long long timestamp = 0; + + memset(raw_buf, 0, sizeof(raw_buf)); + count = read_attribute_sensor(gyro_temperature_fd, raw_buf, + sizeof(raw_buf)); + if(count < 1) { + LOGE("HAL:error reading gyro temperature"); + return -1; + } + + count = sscanf(raw_buf, "%ld%lld", &raw, ×tamp); + + if(count < 0) { + return -1; + } + + LOGV_IF(ENG_VERBOSE, + "HAL:temperature raw = %ld, timestamp = %lld, count = %d", + raw, timestamp, count); + data[0] = raw; + data[1] = timestamp; + + return 0; +} + +int MPLSensor::inv_read_dmp_state(int fd) +{ + VFUNC_LOG; + + if(fd < 0) + return -1; + + int count = 0; + char raw_buf[10]; + short raw = 0; + + memset(raw_buf, 0, sizeof(raw_buf)); + count = read_attribute_sensor(fd, raw_buf, sizeof(raw_buf)); + if(count < 1) { + LOGE("HAL:error reading dmp state"); + close(fd); + return -1; + } + count = sscanf(raw_buf, "%hd", &raw); + if(count < 0) { + LOGE("HAL:dmp state data is invalid"); + close(fd); + return -1; + } + LOGV_IF(EXTRA_VERBOSE, "HAL:dmp state = %d, count = %d", raw, count); + close(fd); + return (int)raw; +} + +int MPLSensor::inv_read_sensor_bias(int fd, long *data) +{ + VFUNC_LOG; + + if(fd == -1) { + return -1; + } + + char buf[50]; + char x[15], y[15], z[15]; + + memset(buf, 0, sizeof(buf)); + int count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 1) { + LOGE("HAL:Error reading gyro bias"); + return -1; + } + count = sscanf(buf, "%[^','],%[^','],%[^',']", x, y, z); + if(count) { + /* scale appropriately for MPL */ + LOGV_IF(ENG_VERBOSE, + "HAL:pre-scaled bias: X:Y:Z (%ld, %ld, %ld)", + atol(x), atol(y), atol(z)); + + data[0] = (long)(atol(x) / 10000 * (1L << 16)); + data[1] = (long)(atol(y) / 10000 * (1L << 16)); + data[2] = (long)(atol(z) / 10000 * (1L << 16)); + + LOGV_IF(ENG_VERBOSE, + "HAL:scaled bias: X:Y:Z (%ld, %ld, %ld)", + data[0], data[1], data[2]); + } + return 0; +} + +/** fill in the sensor list based on which sensors are configured. + * return the number of configured sensors. + * parameter list must point to a memory region of at least 7*sizeof(sensor_t) + * parameter len gives the length of the buffer pointed to by list + */ +int MPLSensor::populateSensorList(struct sensor_t *list, int len) +{ + VFUNC_LOG; + + int numsensors; + + if(len < (int)((sizeof(sSensorList) / sizeof(sensor_t)) * sizeof(sensor_t))) { + LOGE("HAL:sensor list too small, not populating."); + return -(sizeof(sSensorList) / sizeof(sensor_t)); + } + + /* fill in the base values */ + memcpy(list, sSensorList, sizeof (struct sensor_t) * (sizeof(sSensorList) / sizeof(sensor_t))); + + /* first add gyro, accel and compass to the list */ + + /* fill in gyro/accel values */ + fillGyro(chip_ID, list); + fillAccel(chip_ID, list); + + // TODO: need fixes for unified HAL and 3rd-party solution + mCompassSensor->fillList(&list[MagneticField]); + + if(1) { + numsensors = (sizeof(sSensorList) / sizeof(sensor_t)); + /* all sensors will be added to the list + fill in orientation values */ + fillOrientation(list); + /* fill in rotation vector values */ + fillRV(list); + /* fill in gravity values */ + fillGravity(list); + /* fill in Linear accel values */ + fillLinearAccel(list); + } else { + /* no 9-axis sensors, zero fill that part of the list */ + numsensors = 3; + memset(list + 3, 0, 4 * sizeof(struct sensor_t)); + } + + return numsensors; +} + +void MPLSensor::fillAccel(const char* accel, struct sensor_t *list) +{ + VFUNC_LOG; + + if (accel) { + if(accel != NULL && strcmp(accel, "BMA250") == 0) { + list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; + list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA250_POWER; + list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; + return; + } else if (accel != NULL && strcmp(accel, "MPU6050") == 0) { + list[Accelerometer].maxRange = ACCEL_MPU6050_RANGE; + list[Accelerometer].resolution = ACCEL_MPU6050_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU6050_POWER; + list[Accelerometer].minDelay = ACCEL_MPU6050_MINDELAY; + return; + } else if (accel != NULL && strcmp(accel, "MPU6500") == 0) { + list[Accelerometer].maxRange = ACCEL_MPU6500_RANGE; + list[Accelerometer].resolution = ACCEL_MPU6500_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU6500_POWER; + list[Accelerometer].minDelay = ACCEL_MPU6500_MINDELAY; + + return; + } else if (accel != NULL && strcmp(accel, "MPU9150") == 0) { + list[Accelerometer].maxRange = ACCEL_MPU9150_RANGE; + list[Accelerometer].resolution = ACCEL_MPU9150_RESOLUTION; + list[Accelerometer].power = ACCEL_MPU9150_POWER; + list[Accelerometer].minDelay = ACCEL_MPU9150_MINDELAY; + return; + } else if (accel != NULL && strcmp(accel, "MPU3050") == 0) { + list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; + list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA250_POWER; + list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; + return; + } + } + + LOGE("HAL:unknown accel id %s -- " + "params default to bma250 and might be wrong.", + accel); + list[Accelerometer].maxRange = ACCEL_BMA250_RANGE; + list[Accelerometer].resolution = ACCEL_BMA250_RESOLUTION; + list[Accelerometer].power = ACCEL_BMA250_POWER; + list[Accelerometer].minDelay = ACCEL_BMA250_MINDELAY; +} + +void MPLSensor::fillGyro(const char* gyro, struct sensor_t *list) +{ + VFUNC_LOG; + + if ( gyro != NULL && strcmp(gyro, "MPU3050") == 0) { + list[Gyro].maxRange = GYRO_MPU3050_RANGE; + list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; + list[Gyro].power = GYRO_MPU3050_POWER; + list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; + } else if( gyro != NULL && strcmp(gyro, "MPU6050") == 0) { + list[Gyro].maxRange = GYRO_MPU6050_RANGE; + list[Gyro].resolution = GYRO_MPU6050_RESOLUTION; + list[Gyro].power = GYRO_MPU6050_POWER; + list[Gyro].minDelay = GYRO_MPU6050_MINDELAY; + } else if( gyro != NULL && strcmp(gyro, "MPU6500") == 0) { + list[Gyro].maxRange = GYRO_MPU6500_RANGE; + list[Gyro].resolution = GYRO_MPU6500_RESOLUTION; + list[Gyro].power = GYRO_MPU6500_POWER; + list[Gyro].minDelay = GYRO_MPU6500_MINDELAY; + } else if( gyro != NULL && strcmp(gyro, "MPU9150") == 0) { + list[Gyro].maxRange = GYRO_MPU9150_RANGE; + list[Gyro].resolution = GYRO_MPU9150_RESOLUTION; + list[Gyro].power = GYRO_MPU9150_POWER; + list[Gyro].minDelay = GYRO_MPU9150_MINDELAY; + } else { + LOGE("HAL:unknown gyro id -- gyro params will be wrong."); + LOGE("HAL:default to use mpu3050 params"); + list[Gyro].maxRange = GYRO_MPU3050_RANGE; + list[Gyro].resolution = GYRO_MPU3050_RESOLUTION; + list[Gyro].power = GYRO_MPU3050_POWER; + list[Gyro].minDelay = GYRO_MPU3050_MINDELAY; + } + + list[RawGyro].maxRange = list[Gyro].maxRange; + list[RawGyro].resolution = list[Gyro].resolution; + list[RawGyro].power = list[Gyro].power; + list[RawGyro].minDelay = list[Gyro].minDelay; + + return; +} + +/* fillRV depends on values of accel and compass in the list */ +void MPLSensor::fillRV(struct sensor_t *list) +{ + VFUNC_LOG; + + /* compute power on the fly */ + list[RotationVector].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[RotationVector].resolution = .00001; + list[RotationVector].maxRange = 1.0; + list[RotationVector].minDelay = 5000; + + return; +} + +void MPLSensor::fillOrientation(struct sensor_t *list) +{ + VFUNC_LOG; + + list[Orientation].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[Orientation].resolution = .00001; + list[Orientation].maxRange = 360.0; + list[Orientation].minDelay = 5000; + + return; +} + +void MPLSensor::fillGravity( struct sensor_t *list) +{ + VFUNC_LOG; + + list[Gravity].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[Gravity].resolution = .00001; + list[Gravity].maxRange = 9.81; + list[Gravity].minDelay = 5000; + + return; +} + +void MPLSensor::fillLinearAccel(struct sensor_t *list) +{ + VFUNC_LOG; + + list[LinearAccel].power = list[Gyro].power + + list[Accelerometer].power + + list[MagneticField].power; + list[LinearAccel].resolution = list[Accelerometer].resolution; + list[LinearAccel].maxRange = list[Accelerometer].maxRange; + list[LinearAccel].minDelay = 5000; + + return; +} + +int MPLSensor::inv_init_sysfs_attributes(void) +{ + VFUNC_LOG; + + unsigned char i; + char sysfs_path[MAX_SYSFS_NAME_LEN], iio_trigger_path[MAX_SYSFS_NAME_LEN]; + char *sptr; + char **dptr; + + sysfs_names_ptr = + (char*)calloc(1, sizeof(char[MAX_SYSFS_ATTRB][MAX_SYSFS_NAME_LEN])); + sptr = sysfs_names_ptr; + if (sptr == NULL) { + LOGE("HAL:couldn't alloc mem for sysfs paths"); + return -1; + } + + dptr = (char**)&mpu; + i = 0; + do { + *dptr++ = sptr; + sptr += sizeof(char[MAX_SYSFS_NAME_LEN]); + } while (++i < MAX_SYSFS_ATTRB); + + // get proper (in absolute/relative) IIO path & build MPU's sysfs paths + // inv_get_sysfs_abs_path(sysfs_path); + if(INV_SUCCESS != inv_get_sysfs_path(sysfs_path)) { + ALOGE("MPLSensor failed get sysfs path"); + return -1; + } + + if(INV_SUCCESS != inv_get_iio_trigger_path(iio_trigger_path)) { + ALOGE("MPLSensor failed get iio trigger path"); + return -1; + } + + sprintf(mpu.key, "%s%s", sysfs_path, "/key"); + sprintf(mpu.chip_enable, "%s%s", sysfs_path, "/buffer/enable"); + sprintf(mpu.buffer_length, "%s%s", sysfs_path, "/buffer/length"); + sprintf(mpu.power_state, "%s%s", sysfs_path, "/power_state"); + sprintf(mpu.in_timestamp_en, "%s%s", sysfs_path, "/scan_elements/in_timestamp_en"); + sprintf(mpu.trigger_name, "%s%s", iio_trigger_path, "/name"); + sprintf(mpu.current_trigger, "%s%s", sysfs_path, "/trigger/current_trigger"); + + sprintf(mpu.dmp_firmware, "%s%s", sysfs_path,"/dmp_firmware"); + sprintf(mpu.firmware_loaded,"%s%s", sysfs_path, "/firmware_loaded"); + sprintf(mpu.dmp_on,"%s%s", sysfs_path, "/dmp_on"); + sprintf(mpu.dmp_int_on,"%s%s", sysfs_path, "/dmp_int_on"); + sprintf(mpu.dmp_event_int_on,"%s%s", sysfs_path, "/dmp_event_int_on"); + sprintf(mpu.dmp_output_rate,"%s%s", sysfs_path, "/dmp_output_rate"); + sprintf(mpu.tap_on, "%s%s", sysfs_path, "/tap_on"); + + // TODO: for self test + sprintf(mpu.self_test, "%s%s", sysfs_path, "/self_test"); + + sprintf(mpu.temperature, "%s%s", sysfs_path, "/temperature"); + sprintf(mpu.gyro_enable, "%s%s", sysfs_path, "/gyro_enable"); + sprintf(mpu.gyro_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); + sprintf(mpu.gyro_orient, "%s%s", sysfs_path, "/gyro_matrix"); + sprintf(mpu.gyro_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_x_en"); + sprintf(mpu.gyro_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_y_en"); + sprintf(mpu.gyro_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_anglvel_z_en"); + + sprintf(mpu.accel_enable, "%s%s", sysfs_path, "/accl_enable"); + sprintf(mpu.accel_fifo_rate, "%s%s", sysfs_path, "/sampling_frequency"); + sprintf(mpu.accel_orient, "%s%s", sysfs_path, "/accl_matrix"); + + +#ifndef THIRD_PARTY_ACCEL //MPUxxxx + sprintf(mpu.accel_fsr, "%s%s", sysfs_path, "/in_accel_scale"); + // TODO: for bias settings + sprintf(mpu.accel_bias, "%s%s", sysfs_path, "/accl_bias"); +#endif + + sprintf(mpu.accel_x_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_x_en"); + sprintf(mpu.accel_y_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_y_en"); + sprintf(mpu.accel_z_fifo_enable, "%s%s", sysfs_path, "/scan_elements/in_accel_z_en"); + + sprintf(mpu.quaternion_on, "%s%s", sysfs_path, "/quaternion_on"); + sprintf(mpu.in_quat_r_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_r_en"); + sprintf(mpu.in_quat_x_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_x_en"); + sprintf(mpu.in_quat_y_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_y_en"); + sprintf(mpu.in_quat_z_en, "%s%s", sysfs_path, "/scan_elements/in_quaternion_z_en"); + + sprintf(mpu.display_orientation_on, "%s%s", sysfs_path, "/display_orientation_on"); + sprintf(mpu.event_display_orientation, "%s%s", sysfs_path, "/event_display_orientation"); + +#if SYSFS_VERBOSE + // test print sysfs paths + dptr = (char**)&mpu; + for (i = 0; i < MAX_SYSFS_ATTRB; i++) { + LOGE("HAL:sysfs path: %s", *dptr++); + } +#endif + return 0; +} + +/* TODO: stop manually testing/using 0 and 1 instead of + * false and true, but just use 0 and non-0. + * This allows passing 0 and non-0 ints around instead of + * having to convert to 1 and test against 1. + */ +bool MPLSensor::isMpu3050() +{ + return !strcmp(chip_ID, "mpu3050") || !strcmp(chip_ID, "MPU3050"); +} + +int MPLSensor::isLowPowerQuatEnabled() +{ +#ifdef ENABLE_LP_QUAT_FEAT + return !isMpu3050(); +#else + return 0; +#endif +} + +int MPLSensor::isDmpDisplayOrientationOn() +{ +#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT + return !isMpu3050(); +#else + return 0; +#endif +} diff --git a/60xx/libsensors_iio/MPLSensor.h b/60xx/libsensors_iio/MPLSensor.h new file mode 100644 index 0000000..4698f33 --- /dev/null +++ b/60xx/libsensors_iio/MPLSensor.h @@ -0,0 +1,346 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#ifndef ANDROID_MPL_SENSOR_H +#define ANDROID_MPL_SENSOR_H + +#include +#include +#include +#include +#include +#include +#include +#include "sensors.h" +#include "SensorBase.h" +#include "InputEventReader.h" + +#ifdef INVENSENSE_COMPASS_CAL + +#ifdef COMPASS_YAS53x +#pragma message "unified HAL for YAS53x" +#include "CompassSensor.IIO.primary.h" +#elif COMPASS_AMI306 +#pragma message "unified HAL for AMI306" +#include "CompassSensor.IIO.primary.h" +#else +#pragma message "unified HAL for MPU9150" +#include "CompassSensor.IIO.9150.h" +#endif +#else +#pragma message "unified HAL for AKM" +#include "CompassSensor.AKM.h" +#endif + +/*****************************************************************************/ +/* Sensors Enable/Disable Mask + *****************************************************************************/ +#define MAX_CHIP_ID_LEN (20) + +#define INV_THREE_AXIS_GYRO (0x000F) +#define INV_THREE_AXIS_ACCEL (0x0070) +#define INV_THREE_AXIS_COMPASS (0x0380) +#define INV_ALL_SENSORS (0x7FFF) + +#ifdef INVENSENSE_COMPASS_CAL +#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \ + | INV_THREE_AXIS_COMPASS \ + | INV_THREE_AXIS_GYRO) +#else +#define ALL_MPL_SENSORS_NP (INV_THREE_AXIS_ACCEL \ + | INV_THREE_AXIS_COMPASS \ + | INV_THREE_AXIS_GYRO) +#endif + +// bit mask of current active features (mFeatureActiveMask) +#define INV_COMPASS_CAL 0x01 +#define INV_COMPASS_FIT 0x02 +#define INV_DMP_QUATERNION 0x04 +#define INV_DMP_DISPL_ORIENTATION 0x08 + +/* Uncomment to enable Low Power Quaternion */ +#define ENABLE_LP_QUAT_FEAT + +/* Uncomment to enable DMP display orientation + (within the HAL, see below for Java framework) */ +// #define ENABLE_DMP_DISPL_ORIENT_FEAT + +#ifdef ENABLE_DMP_DISPL_ORIENT_FEAT +/* Uncomment following to expose the SENSOR_TYPE_SCREEN_ORIENTATION + sensor type (DMP screen orientation) to the Java framework. + NOTE: + need Invensense customized + 'hardware/libhardware/include/hardware/sensors.h' to compile correctly. + NOTE: + need Invensense java framework changes to: + 'frameworks/base/core/java/android/view/WindowOrientationListener.java' + 'frameworks/base/core/java/android/hardware/Sensor.java' + 'frameworks/base/core/java/android/hardware/SensorEvent.java' + for the 'Auto-rotate screen' to use this feature. +*/ +#define ENABLE_DMP_SCREEN_AUTO_ROTATION +#warning "ENABLE_DMP_DISPL_ORIENT_FEAT is defined, framework changes are necessary for HAL to work properly" +#endif + +int isDmpScreenAutoRotationEnabled() +{ +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION + return 1; +#else + return 0; +#endif +} + +int (*m_pt2AccelCalLoadFunc)(long *bias) = NULL; +/*****************************************************************************/ +/** MPLSensor implementation which fits into the HAL example for crespo provided + * by Google. + * WARNING: there may only be one instance of MPLSensor, ever. + */ + +class MPLSensor: public SensorBase +{ + typedef int (MPLSensor::*hfunc_t)(sensors_event_t*); + +public: + + enum { + Gyro = 0, + RawGyro, + Accelerometer, + MagneticField, + Orientation, + RotationVector, + LinearAccel, + Gravity, + numSensors + }; + + MPLSensor(CompassSensor *, int (*m_pt2AccelCalLoadFunc)(long*) = 0); + virtual ~MPLSensor(); + + virtual int setDelay(int32_t handle, int64_t ns); + virtual int enable(int32_t handle, int enabled); + int32_t getEnableMask() { return mEnabled; } + + virtual int readEvents(sensors_event_t *data, int count); + virtual int getFd() const; + virtual int getAccelFd() const; + virtual int getCompassFd() const; + virtual int getPollTime(); + virtual bool hasPendingEvents() const; + virtual void sleepEvent(); + virtual void wakeEvent(); + int populateSensorList(struct sensor_t *list, int len); + void cbProcData(); + + //static pointer to the object that will handle callbacks + static MPLSensor* gMPLSensor; + + //AKM HAL Integration + //void set_compass(long ready, long x, long y, long z, long accuracy); + int executeOnData(sensors_event_t* data, int count); + int readAccelEvents(sensors_event_t* data, int count); + int readCompassEvents(sensors_event_t* data, int count); + + int turnOffAccelFifo(); + int enableDmpOrientation(int); + int dmpOrientHandler(int); + int readDmpOrientEvents(sensors_event_t* data, int count); + int getDmpOrientFd(); + int openDmpOrientFd(); + int closeDmpOrientFd(); + + int getDmpRate(int64_t *); + int checkDMPOrientation(); + +protected: + CompassSensor *mCompassSensor; + + int gyroHandler(sensors_event_t *data); + int rawGyroHandler(sensors_event_t *data); + int accelHandler(sensors_event_t *data); + int compassHandler(sensors_event_t *data); + int rvHandler(sensors_event_t *data); + int laHandler(sensors_event_t *data); + int gravHandler(sensors_event_t *data); + int orienHandler(sensors_event_t *data); + void calcOrientationSensor(float *Rx, float *Val); + virtual int update_delay(); + + void inv_set_device_properties(); + int inv_constructor_init(); + int inv_constructor_default_enable(); + int setGyroInitialState(); + int setAccelInitialState(); + int masterEnable(int en); + int onPower(int en); + int enableLPQuaternion(int); + int enableQuaternionData(int); + int onDMP(int); + int enableOneSensor(int en, const char *name, int (MPLSensor::*enabler)(int)); + int enableGyro(int en); + int enableAccel(int en); + int enableCompass(int en); + void computeLocalSensorMask(int enabled_sensors); + int enableSensors(unsigned long sensors, int en, uint32_t changed); + int inv_read_gyro_buffer(int fd, short *data, long long *timestamp); + int inv_float_to_q16(float *fdata, long *ldata); + int inv_long_to_q16(long *fdata, long *ldata); + int inv_float_to_round(float *fdata, long *ldata); + int inv_float_to_round2(float *fdata, short *sdata); + int inv_read_temperature(long long *data); + int inv_read_dmp_state(int fd); + int inv_read_sensor_bias(int fd, long *data); + void inv_get_sensors_orientation(void); + int inv_init_sysfs_attributes(void); +#ifdef COMPASS_YAS53x + int resetCompass(void); +#endif + void setCompassDelay(int64_t ns); + void enable_iio_sysfs(void); + int enableTap(int); + int enableFlick(int); + int enablePedometer(int); + int checkLPQuaternion(); + + int mNewData; // flag indicating that the MPL calculated new output values + int mDmpStarted; + long mMasterSensorMask; + long mLocalSensorMask; + int mPollTime; + bool mHaveGoodMpuCal; // flag indicating that the cal file can be written + int mGyroAccuracy; // value indicating the quality of the gyro calibr. + int mAccelAccuracy; // value indicating the quality of the accel calibr. + int mCompassAccuracy; // value indicating the quality of the compass calibr. + struct pollfd mPollFds[5]; + int mSampleCount; + pthread_mutex_t mMplMutex; + pthread_mutex_t mHALMutex; + + char mIIOBuffer[(16 + 8 * 3 + 8) * IIO_BUFFER_LENGTH]; + + int iio_fd; + int accel_fd; + int mpufifo_fd; + int gyro_temperature_fd; + int dmp_orient_fd; + + int mDmpOrientationEnabled; + + + uint32_t mEnabled; + uint32_t mOldEnabledMask; + sensors_event_t mPendingEvents[numSensors]; + int64_t mDelays[numSensors]; + hfunc_t mHandlers[numSensors]; + short mCachedGyroData[3]; + long mCachedAccelData[3]; + long mCachedCompassData[3]; + long mCachedQuaternionData[4]; + android::KeyedVector mIrqFds; + + InputEventCircularReader mAccelInputReader; + InputEventCircularReader mGyroInputReader; + + bool mFirstRead; + short mTempScale; + short mTempOffset; + int64_t mTempCurrentTime; + int mAccelScale; + + uint32_t mPendingMask; + unsigned long mSensorMask; + + char chip_ID[MAX_CHIP_ID_LEN]; + + signed char mGyroOrientation[9]; + signed char mAccelOrientation[9]; + + int64_t mSensorTimestamp; + int64_t mCompassTimestamp; + + struct sysfs_attrbs { + char *chip_enable; + char *power_state; + char *dmp_firmware; + char *firmware_loaded; + char *dmp_on; + char *dmp_int_on; + char *dmp_event_int_on; + char *dmp_output_rate; + char *tap_on; + char *key; + char *self_test; + char *temperature; + + char *gyro_enable; + char *gyro_fifo_rate; + char *gyro_orient; + char *gyro_x_fifo_enable; + char *gyro_y_fifo_enable; + char *gyro_z_fifo_enable; + + char *accel_enable; + char *accel_fifo_rate; + char *accel_fsr; + char *accel_bias; + char *accel_orient; + char *accel_x_fifo_enable; + char *accel_y_fifo_enable; + char *accel_z_fifo_enable; + + char *quaternion_on; + char *in_quat_r_en; + char *in_quat_x_en; + char *in_quat_y_en; + char *in_quat_z_en; + + char *in_timestamp_en; + char *trigger_name; + char *current_trigger; + char *buffer_length; + + char *display_orientation_on; + char *event_display_orientation; + } mpu; + + char *sysfs_names_ptr; + int mFeatureActiveMask; + +private: + /* added for dynamic get sensor list */ + void fillAccel(const char* accel, struct sensor_t *list); + void fillGyro(const char* gyro, struct sensor_t *list); + void fillRV(struct sensor_t *list); + void fillOrientation(struct sensor_t *list); + void fillGravity(struct sensor_t *list); + void fillLinearAccel(struct sensor_t *list); + void storeCalibration(); + void loadDMP(); + bool isMpu3050(); + int isLowPowerQuatEnabled(); + int isDmpDisplayOrientationOn(); + + +}; + +extern "C" { + void setCallbackObject(MPLSensor*); + MPLSensor *getCallbackObject(); +} + +#endif // ANDROID_MPL_SENSOR_H diff --git a/60xx/libsensors_iio/MPLSupport.cpp b/60xx/libsensors_iio/MPLSupport.cpp new file mode 100644 index 0000000..9773562 --- /dev/null +++ b/60xx/libsensors_iio/MPLSupport.cpp @@ -0,0 +1,190 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#define LOG_NDEBUG 0 + +#include +#include +#include +#include + +#include "log.h" +#include "SensorBase.h" + +#include "ml_sysfs_helper.h" +#include "local_log_def.h" + +int64_t getTimestamp() +{ + struct timespec t; + t.tv_sec = t.tv_nsec = 0; + clock_gettime(CLOCK_MONOTONIC, &t); + return int64_t(t.tv_sec) * 1000000000LL + t.tv_nsec; +} + +int64_t timevalToNano(timeval const& t) { + return t.tv_sec * 1000000000LL + t.tv_usec * 1000; +} + +int inv_read_data(char *fname, long *data) +{ + VFUNC_LOG; + + char buf[sizeof(long) * 4]; + int count, fd; + + fd = open(fname, O_RDONLY); + if(fd < 0) { + LOGE("HAL:Error opening %s", fname); + return -1; + } + memset(buf, 0, sizeof(buf)); + count = read_attribute_sensor(fd, buf, sizeof(buf)); + if(count < 1) { + close(fd); + return -1; + } else { + count = sscanf(buf, "%ld", data); + if(count) + LOGV_IF(EXTRA_VERBOSE, "HAL:Data= %ld", *data); + } + close(fd); + + return 0; +} + +/* This one DOES NOT close FDs for you */ +int read_attribute_sensor(int fd, char* data, unsigned int size) +{ + VFUNC_LOG; + + int count = 0; + if (fd > 0) { + count = pread(fd, data, size, 0); + if(count < 1) { + LOGE("HAL:read fails with error code=%d", count); + } + } + return count; +} + +/** + * @brief Enable a sensor through the sysfs file descriptor + * provided. + * @note this function one closes FD after the write + * @param fd + * the file descriptor to write into + * @param en + * the value to write, typically 1 or 0 + * @return the errno whenever applicable. + */ +int enable_sysfs_sensor(int fd, int en) +{ + VFUNC_LOG; + + int nb; + int err = 0; + + char c = en ? '1' : '0'; + nb = write(fd, &c, 1); + + if (nb <= 0) { + err = errno; + LOGE("HAL:enable_sysfs_sensor - write %c returned %d (%s / %d)", + c, nb, strerror(err), err); + } + close(fd); + + + return err; +} + +/* This one closes FDs for you */ +int write_attribute_sensor(int fd, long data) +{ + VFUNC_LOG; + + int num_b = 0; + + if (fd >= 0) { + char buf[80]; + sprintf(buf, "%ld", data); + num_b = write(fd, buf, strlen(buf) + 1); + if (num_b <= 0) { + int err = errno; + LOGE("HAL:write fd %d returned '%s' (%d)", fd, strerror(err), err); + } else { + LOGV_IF(EXTRA_VERBOSE, "HAL:fd=%d write attribute to %ld", fd, data); + } + close(fd); + } + + return num_b; +} + +int read_sysfs_int(char *filename, int *var) +{ + int res=0; + FILE *sysfsfp; + + sysfsfp = fopen(filename, "r"); + if (sysfsfp != NULL) { + if (fscanf(sysfsfp, "%d\n", var) < 1) { + LOGE("HAL:ERR failed to read an int from %s.", filename); + res = -EINVAL; + } + fclose(sysfsfp); + } else { + res = -errno; + LOGE("HAL:ERR open file %s to read with error %d", filename, res); + } + return res; +} + +int write_sysfs_int(char *filename, int var) +{ + int res = 0; + FILE *sysfsfp; + + LOGV_IF(SYSFS_VERBOSE, "HAL:sysfs:echo %d > %s (%lld)", + var, filename, getTimestamp()); + sysfsfp = fopen(filename, "w"); + if (sysfsfp == NULL) { + res = -errno; + LOGE("HAL:ERR open file %s to write with error %d", filename, res); + return res; + } + int fpres, fcres = 0; + fpres = fprintf(sysfsfp, "%d\n", var); + /* fprintf() can succeed because it never actually writes to the + * underlying sysfs node. + */ + if (fpres < 0) { + res = -errno; + fclose(sysfsfp); + } else { + fcres = fclose(sysfsfp); + /* Check for errors from: fflush(), write(), and close() */ + if (fcres < 0) { + res = -errno; + } + } + if (fpres < 0 || fcres < 0) { + LOGE("HAL:ERR failed to write %d to %s (err=%d) print/close=%d/%d", + var, filename, res, fpres, fcres); + } + return res; +} diff --git a/60xx/libsensors_iio/MPLSupport.h b/60xx/libsensors_iio/MPLSupport.h new file mode 100644 index 0000000..98e4497 --- /dev/null +++ b/60xx/libsensors_iio/MPLSupport.h @@ -0,0 +1,33 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#ifndef ANDROID_MPL_SUPPORT_H +#define ANDROID_MPL_SUPPORT_H + +#include +#include + +int64_t getTimestamp(); +int64_t timevalToNano(timeval const& t); + +int inv_read_data(char *fname, long *data); +int read_attribute_sensor(int fd, char* data, unsigned int size); +int enable_sysfs_sensor(int fd, int en); +int write_attribute_sensor(int fd, long data); +int read_sysfs_int(char*, int*); +int write_sysfs_int(char*, int); + +#endif // ANDROID_MPL_SUPPORT_H diff --git a/60xx/libsensors_iio/SensorBase.cpp b/60xx/libsensors_iio/SensorBase.cpp new file mode 100644 index 0000000..587aaf5 --- /dev/null +++ b/60xx/libsensors_iio/SensorBase.cpp @@ -0,0 +1,135 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "SensorBase.h" +#include "local_log_def.h" + +/*****************************************************************************/ + +SensorBase::SensorBase(const char* dev_name, + const char* data_name) : dev_name(dev_name), + data_name(data_name), + dev_fd(-1), + data_fd(-1) +{ + if (data_name) { + data_fd = openInput(data_name); + } +} + +SensorBase::~SensorBase() +{ + if (data_fd >= 0) { + close(data_fd); + } + if (dev_fd >= 0) { + close(dev_fd); + } +} + +int SensorBase::open_device() +{ + if (dev_fd<0 && dev_name) { + dev_fd = open(dev_name, O_RDONLY); + LOGE_IF(dev_fd<0, "Couldn't open %s (%s)", dev_name, strerror(errno)); + } + return 0; +} + +int SensorBase::close_device() +{ + if (dev_fd >= 0) { + close(dev_fd); + dev_fd = -1; + } + return 0; +} + +int SensorBase::getFd() const +{ + if (!data_name) { + return dev_fd; + } + return data_fd; +} + +int SensorBase::setDelay(int32_t /*handle*/, int64_t /*ns*/) +{ + return 0; +} + +bool SensorBase::hasPendingEvents() const +{ + return false; +} + +int SensorBase::openInput(const char *inputName) +{ + int fd = -1; + const char *dirname = "/dev/input"; + char devname[PATH_MAX]; + char *filename; + DIR *dir; + struct dirent *de; + dir = opendir(dirname); + if(dir == NULL) + return -1; + strcpy(devname, dirname); + filename = devname + strlen(devname); + *filename++ = '/'; + while((de = readdir(dir))) { + if(de->d_name[0] == '.' && + (de->d_name[1] == '\0' || + (de->d_name[1] == '.' && de->d_name[2] == '\0'))) + continue; + strcpy(filename, de->d_name); + fd = open(devname, O_RDONLY); + LOGV_IF(EXTRA_VERBOSE, "path open %s", devname); + LOGI("path open %s", devname); + if (fd >= 0) { + char name[80]; + if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) { + name[0] = '\0'; + } + LOGV_IF(EXTRA_VERBOSE, "name read %s", name); + if (!strcmp(name, inputName)) { + strcpy(input_name, filename); + break; + } else { + close(fd); + fd = -1; + } + } + } + closedir(dir); + LOGE_IF(fd < 0, "couldn't find '%s' input device", inputName); + return fd; +} + +int SensorBase::enable(int32_t /*handle*/, int /*enabled*/) +{ + return 0; +} diff --git a/60xx/libsensors_iio/SensorBase.h b/60xx/libsensors_iio/SensorBase.h new file mode 100644 index 0000000..fef47c7 --- /dev/null +++ b/60xx/libsensors_iio/SensorBase.h @@ -0,0 +1,61 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#ifndef ANDROID_SENSOR_BASE_H +#define ANDROID_SENSOR_BASE_H + +#include +#include +#include +#include + +#define CALL_MEMBER_FN(pobject, ptrToMember) ((pobject)->*(ptrToMember)) + +#define MAX_SYSFS_NAME_LEN (100) +#define IIO_BUFFER_LENGTH (480) + +/*****************************************************************************/ + +struct sensors_event_t; + +class SensorBase { +protected: + const char *dev_name; + const char *data_name; + char input_name[PATH_MAX]; + int dev_fd; + int data_fd; + + int openInput(const char* inputName); + + int open_device(); + int close_device(); + +public: + SensorBase(const char* dev_name, const char* data_name); + + virtual ~SensorBase(); + + virtual int readEvents(sensors_event_t* data, int count) = 0; + virtual bool hasPendingEvents() const; + virtual int getFd() const; + virtual int setDelay(int32_t handle, int64_t ns); + virtual int enable(int32_t handle, int enabled); +}; + +/*****************************************************************************/ + +#endif // ANDROID_SENSOR_BASE_H diff --git a/60xx/libsensors_iio/libmllite.so b/60xx/libsensors_iio/libmllite.so new file mode 100755 index 0000000..9bdd5bc Binary files /dev/null and b/60xx/libsensors_iio/libmllite.so differ diff --git a/60xx/libsensors_iio/libmplmpu.so b/60xx/libsensors_iio/libmplmpu.so new file mode 100644 index 0000000..205ab9a Binary files /dev/null and b/60xx/libsensors_iio/libmplmpu.so differ diff --git a/60xx/libsensors_iio/local_log_def.h b/60xx/libsensors_iio/local_log_def.h new file mode 100644 index 0000000..9135992 --- /dev/null +++ b/60xx/libsensors_iio/local_log_def.h @@ -0,0 +1,49 @@ +#ifndef LOCAL_LOG_DEF_H +#define LOCAL_LOG_DEF_H + +/* Log enablers, each of these independent */ + +#define PROCESS_VERBOSE (0) /* process log messages */ +#define EXTRA_VERBOSE (0) /* verbose log messages */ +#define SYSFS_VERBOSE (0) /* log sysfs interactions as cat/echo for repro + purpose on a shell */ +#define FUNC_ENTRY (0) /* log entry in all one-time functions */ + +/* Note that enabling this logs may affect performance */ +#define HANDLER_ENTRY (0) /* log entry in all handler functions */ +#define ENG_VERBOSE (0) /* log some a lot more info about the internals */ +#define INPUT_DATA (0) /* log the data input from the events */ +#define HANDLER_DATA (0) /* log the data fetched from the handlers */ + +#if defined ANDROID_JELLYBEAN +#define LOGV ALOGV +#define LOGV_IF ALOGV_IF +#define LOGD ALOGD +#define LOGD_IF ALOGD_IF +#define LOGI ALOGI +#define LOGI_IF ALOGI_IF +#define LOGW ALOGW +#define LOGW_IF ALOGW_IF +#define LOGE ALOGE +#define LOGE_IF ALOGE_IF +#define IF_LOGV IF_ALOGV +#define IF_LOGD IF_ALOGD +#define IF_LOGI IF_ALOGI +#define IF_LOGW IF_ALOGW +#define IF_LOGE IF_ALOGE +#define LOG_ASSERT ALOG_ASSERT +#define LOG ALOG +#define IF_LOG IF_ALOG +#else +#warning "build for ICS or earlier version" +#endif + + +#define FUNC_LOG \ + LOGV("%s", __PRETTY_FUNCTION__) +#define VFUNC_LOG \ + LOGV_IF(FUNC_ENTRY, "Entering function '%s'", __PRETTY_FUNCTION__) +#define VHANDLER_LOG \ + LOGV_IF(HANDLER_ENTRY, "Entering handler '%s'", __PRETTY_FUNCTION__) + +#endif /*ifndef LOCAL_LOG_DEF_H */ diff --git a/60xx/libsensors_iio/sensor_params.h b/60xx/libsensors_iio/sensor_params.h new file mode 100644 index 0000000..eef0b3b --- /dev/null +++ b/60xx/libsensors_iio/sensor_params.h @@ -0,0 +1,194 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#ifndef INV_SENSOR_PARAMS_H +#define INV_SENSOR_PARAMS_H + +/* Physical parameters of the sensors supported by Invensense MPL */ +#define SENSORS_ROTATION_VECTOR_HANDLE (ID_RV) +#define SENSORS_LINEAR_ACCEL_HANDLE (ID_LA) +#define SENSORS_GRAVITY_HANDLE (ID_GR) +#define SENSORS_GYROSCOPE_HANDLE (ID_GY) +#define SENSORS_RAW_GYROSCOPE_HANDLE (ID_RG) +#define SENSORS_ACCELERATION_HANDLE (ID_A) +#define SENSORS_MAGNETIC_FIELD_HANDLE (ID_M) +#define SENSORS_ORIENTATION_HANDLE (ID_O) +#define SENSORS_SCREEN_ORIENTATION_HANDLE (ID_SO) + +/******************************************/ +//MPU9250 INV_COMPASS +#define COMPASS_MPU9250_RANGE (9830.f) +#define COMPASS_MPU9250_RESOLUTION (0.15f) +#define COMPASS_MPU9250_POWER (10.f) +#define COMPASS_MPU9250_MINDELAY (10000) +//MPU9150 INV_COMPASS +#define COMPASS_MPU9150_RANGE (9830.f) +#define COMPASS_MPU9150_RESOLUTION (0.285f) +#define COMPASS_MPU9150_POWER (10.f) +#define COMPASS_MPU9150_MINDELAY (10000) +//COMPASS_ID_AK8975 +#define COMPASS_AKM8975_RANGE (9830.f) +#define COMPASS_AKM8975_RESOLUTION (0.285f) +#define COMPASS_AKM8975_POWER (10.f) +#define COMPASS_AKM8975_MINDELAY (10000) +//COMPASS_ID_AK8963C +#define COMPASS_AKM8963_RANGE (9830.f) +#define COMPASS_AKM8963_RESOLUTION (0.15f) +#define COMPASS_AKM8963_POWER (10.f) +#define COMPASS_AKM8963_MINDELAY (10000) +//COMPASS_ID_AMI30X +#define COMPASS_AMI30X_RANGE (5461.f) +#define COMPASS_AMI30X_RESOLUTION (0.9f) +#define COMPASS_AMI30X_POWER (0.15f) +//COMPASS_ID_AMI306 +#define COMPASS_AMI306_RANGE (5461.f) +#define COMPASS_AMI306_RESOLUTION (0.9f) +#define COMPASS_AMI306_POWER (0.15f) +#define COMPASS_AMI306_MINDELAY (10000) +//COMPASS_ID_YAS529 +#define COMPASS_YAS529_RANGE (19660.f) +#define COMPASS_YAS529_RESOLUTION (0.012f) +#define COMPASS_YAS529_POWER (4.f) +//COMPASS_ID_YAS53x +#define COMPASS_YAS53x_RANGE (8001.f) +#define COMPASS_YAS53x_RESOLUTION (0.012f) +#define COMPASS_YAS53x_POWER (4.f) +#define COMPASS_YAS53x_MINDELAY (10000) +//COMPASS_ID_HMC5883 +#define COMPASS_HMC5883_RANGE (10673.f) +#define COMPASS_HMC5883_RESOLUTION (10.f) +#define COMPASS_HMC5883_POWER (0.24f) +//COMPASS_ID_LSM303DLH +#define COMPASS_LSM303DLH_RANGE (10240.f) +#define COMPASS_LSM303DLH_RESOLUTION (1.f) +#define COMPASS_LSM303DLH_POWER (1.f) +//COMPASS_ID_LSM303DLM +#define COMPASS_LSM303DLM_RANGE (10240.f) +#define COMPASS_LSM303DLM_RESOLUTION (1.f) +#define COMPASS_LSM303DLM_POWER (1.f) +//COMPASS_ID_MMC314X +#define COMPASS_MMC314X_RANGE (400.f) +#define COMPASS_MMC314X_RESOLUTION (2.f) +#define COMPASS_MMC314X_POWER (0.55f) +//COMPASS_ID_HSCDTD002B +#define COMPASS_HSCDTD002B_RANGE (9830.f) +#define COMPASS_HSCDTD002B_RESOLUTION (1.f) +#define COMPASS_HSCDTD002B_POWER (1.f) +//COMPASS_ID_HSCDTD004A +#define COMPASS_HSCDTD004A_RANGE (9830.f) +#define COMPASS_HSCDTD004A_RESOLUTION (1.f) +#define COMPASS_HSCDTD004A_POWER (1.f) +/*******************************************/ +//ACCEL_ID_MPU6500 +#define ACCEL_MPU6500_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MPU6500_RESOLUTION (0.004f * GRAVITY_EARTH) +#define ACCEL_MPU6500_POWER (0.f) +#define ACCEL_MPU6500_MINDELAY (1000) +//ACCEL_ID_MPU9250 +#define ACCEL_MPU9250_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MPU9250_RESOLUTION (0.004f * GRAVITY_EARTH) +#define ACCEL_MPU9250_POWER (0.f) +#define ACCEL_MPU9250_MINDELAY (1000) +//ACCEL_ID_MPU9150 +#define ACCEL_MPU9150_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MPU9150_RESOLUTION (0.004f * GRAVITY_EARTH) +#define ACCEL_MPU9150_POWER (0.f) +#define ACCEL_MPU9150_MINDELAY (1000) +//ACCEL_ID_LIS331 +#define ACCEL_LIS331_RANGE (2.48f * GRAVITY_EARTH) +#define ACCEL_LIS331_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_LIS331_POWER (1.f) +//ACCEL_ID_LSM303DLX +#define ACCEL_LSM303DLX_RANGE (2.48f * GRAVITY_EARTH) +#define ACCEL_LSM303DLX_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_LSM303DLX_POWER (1.f) +//ACCEL_ID_LIS3DH +#define ACCEL_LIS3DH_RANGE (2.48f * GRAVITY_EARTH) +#define ACCEL_LIS3DH_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_LIS3DH_POWER (1.f) +//ACCEL_ID_KXSD9 +#define ACCEL_KXSD9_RANGE (2.5006f * GRAVITY_EARTH) +#define ACCEL_KXSD9_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_KXSD9_POWER (1.f) +//ACCEL_ID_KXTF9 +#define ACCEL_KXTF9_RANGE (1.f * GRAVITY_EARTH) +#define ACCEL_KXTF9_RESOLUTION (0.033f * GRAVITY_EARTH) +#define ACCEL_KXTF9_POWER (0.35f) +//ACCEL_ID_BMA150 +#define ACCEL_BMA150_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_BMA150_RESOLUTION (0.004f * GRAVITY_EARTH) +#define ACCEL_BMA150_POWER (0.2f) +//ACCEL_ID_BMA222 +#define ACCEL_BMA222_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_BMA222_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_BMA222_POWER (0.1f) +//ACCEL_ID_BMA250 +#define ACCEL_BMA250_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_BMA250_RESOLUTION (0.00391f * GRAVITY_EARTH) +#define ACCEL_BMA250_POWER (0.139f) +#define ACCEL_BMA250_MINDELAY (1000) +//ACCEL_ID_ADXL34X +#define ACCEL_ADXL34X_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_ADXL34X_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_ADXL34X_POWER (1.f) +//ACCEL_ID_MMA8450 +#define ACCEL_MMA8450_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MMA8450_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_MMA8450_POWER (1.0f) +//ACCEL_ID_MMA845X +#define ACCEL_MMA845X_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MMA845X_RESOLUTION (0.001f * GRAVITY_EARTH) +#define ACCEL_MMA845X_POWER (1.f) +//ACCEL_ID_MPU6050 +#define ACCEL_MPU6050_RANGE (2.f * GRAVITY_EARTH) +#define ACCEL_MPU6050_RESOLUTION (0.004f * GRAVITY_EARTH) +#define ACCEL_MPU6050_POWER (0.f) +#define ACCEL_MPU6050_MINDELAY (5000) +/******************************************/ +//GYRO MPU3050 +#define RAD_P_DEG (3.14159f / 180.f) +#define GYRO_MPU3050_RANGE (2000.f * RAD_P_DEG) +#define GYRO_MPU3050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) +#define GYRO_MPU3050_POWER (6.1f) +#define GYRO_MPU3050_MINDELAY (1000) +//GYRO MPU6050 +#define GYRO_MPU6050_RANGE (2000.f * RAD_P_DEG) +#define GYRO_MPU6050_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) +#define GYRO_MPU6050_POWER (5.5f) +#define GYRO_MPU6050_MINDELAY (5000) +//GYRO MPU9150 +#define GYRO_MPU9150_RANGE (2000.f * RAD_P_DEG) +#define GYRO_MPU9150_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) +#define GYRO_MPU9150_POWER (5.5f) +#define GYRO_MPU9150_MINDELAY (1000) +//GYRO MPU9250 +#define GYRO_MPU9250_RANGE (2000.f * RAD_P_DEG) +#define GYRO_MPU9250_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) +#define GYRO_MPU9250_POWER (5.5f) +#define GYRO_MPU9250_MINDELAY (1000) +//GYRO MPU6500 +#define GYRO_MPU6500_RANGE (2000.f * RAD_P_DEG) +#define GYRO_MPU6500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) +#define GYRO_MPU6500_POWER (5.5f) +#define GYRO_MPU6500_MINDELAY (1000) +//GYRO ITG3500 +#define GYRO_ITG3500_RANGE (2000.f * RAD_P_DEG) +#define GYRO_ITG3500_RESOLUTION (2000.f / 32768.f * RAD_P_DEG) +#define GYRO_ITG3500_POWER (5.5f) +#define GYRO_ITG3500_MINDELAY (1000) + +#endif /* INV_SENSOR_PARAMS_H */ + diff --git a/60xx/libsensors_iio/sensors.h b/60xx/libsensors_iio/sensors.h new file mode 100644 index 0000000..f698fc5 --- /dev/null +++ b/60xx/libsensors_iio/sensors.h @@ -0,0 +1,97 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#ifndef ANDROID_SENSORS_H +#define ANDROID_SENSORS_H + +#include +#include +#include +#include + +#include + +#include +#include + +__BEGIN_DECLS + +/*****************************************************************************/ + +#ifndef ARRAY_SIZE +#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0])) +#endif + +enum { + ID_GY = 0, + ID_RG, + ID_A, + ID_M, + ID_O, + ID_RV, + ID_LA, + ID_GR, + ID_SO +}; + +/*****************************************************************************/ + +/* + * The SENSORS Module + */ + +/* ITG3500 */ +#define EVENT_TYPE_GYRO_X REL_X +#define EVENT_TYPE_GYRO_Y REL_Y +#define EVENT_TYPE_GYRO_Z REL_Z +/* MPU6050 MPU9150 */ +#define EVENT_TYPE_IACCEL_X REL_RX +#define EVENT_TYPE_IACCEL_Y REL_RY +#define EVENT_TYPE_IACCEL_Z REL_RZ +/* MPU6050 MPU9150 */ +#define EVENT_TYPE_ICOMPASS_X REL_X +#define EVENT_TYPE_ICOMPASS_Y REL_Y +#define EVENT_TYPE_ICOMPASS_Z REL_Z +/* MPUxxxx */ +#define EVENT_TYPE_TIMESTAMP_HI REL_MISC +#define EVENT_TYPE_TIMESTAMP_LO REL_WHEEL + +/* Accel BMA250 */ +#define EVENT_TYPE_ACCEL_X ABS_X +#define EVENT_TYPE_ACCEL_Y ABS_Y +#define EVENT_TYPE_ACCEL_Z ABS_Z +#define LSG (1000.0f) + +// conversion of acceleration data to SI units (m/s^2) +#define RANGE_A (4*GRAVITY_EARTH) +#define RESOLUTION_A (GRAVITY_EARTH / LSG) +#define CONVERT_A (GRAVITY_EARTH / LSG) +#define CONVERT_A_X (CONVERT_A) +#define CONVERT_A_Y (CONVERT_A) +#define CONVERT_A_Z (CONVERT_A) + +/* Compass AKM8975 */ +#define EVENT_TYPE_MAGV_X ABS_RX +#define EVENT_TYPE_MAGV_Y ABS_RY +#define EVENT_TYPE_MAGV_Z ABS_RZ +#define EVENT_TYPE_MAGV_STATUS ABS_RUDDER + +// conversion of magnetic data to uT units +#define CONVERT_M (0.06f) + +__END_DECLS + +#endif // ANDROID_SENSORS_H diff --git a/60xx/libsensors_iio/sensors_mpl.cpp b/60xx/libsensors_iio/sensors_mpl.cpp new file mode 100644 index 0000000..4aef514 --- /dev/null +++ b/60xx/libsensors_iio/sensors_mpl.cpp @@ -0,0 +1,260 @@ +/* +* Copyright (C) 2012 Invensense, Inc. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*/ + +#define FUNC_LOG LOGV("%s", __PRETTY_FUNCTION__) + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "sensors.h" +#include "MPLSensor.h" + +/*****************************************************************************/ +/* The SENSORS Module */ + +#ifdef ENABLE_DMP_SCREEN_AUTO_ROTATION +#define LOCAL_SENSORS (MPLSensor::numSensors + 1) +#else +#define LOCAL_SENSORS MPLSensor::numSensors +#endif + +/* Vendor-defined Accel Load Calibration File Method +* @param[out] Accel bias, length 3. In HW units scaled by 2^16 in body frame +* @return '0' for a successful load, '1' otherwise +* example: int AccelLoadConfig(long* offset); +* End of Vendor-defined Accel Load Cal Method +*/ + +static struct sensor_t sSensorList[LOCAL_SENSORS]; +static int sensors = (sizeof(sSensorList) / sizeof(sensor_t)); + +static int open_sensors(const struct hw_module_t* module, const char* id, + struct hw_device_t** device); + +static int sensors__get_sensors_list(struct sensors_module_t* module, + struct sensor_t const** list) +{ + *list = sSensorList; + return sensors; +} + +static struct hw_module_methods_t sensors_module_methods = { + open: open_sensors +}; + +struct sensors_module_t HAL_MODULE_INFO_SYM = { + common: { + tag: HARDWARE_MODULE_TAG, + version_major: 1, + version_minor: 0, + id: SENSORS_HARDWARE_MODULE_ID, + name: "Invensense module", + author: "Invensense Inc.", + methods: &sensors_module_methods, + }, + get_sensors_list: sensors__get_sensors_list, +}; + +struct sensors_poll_context_t { + struct sensors_poll_device_t device; // must be first + + sensors_poll_context_t(); + ~sensors_poll_context_t(); + int activate(int handle, int enabled); + int setDelay(int handle, int64_t ns); + int pollEvents(sensors_event_t* data, int count); + +private: + enum { + mpl = 0, + compass, + dmpOrient, + numSensorDrivers, // wake pipe goes here + numFds, + }; + + struct pollfd mPollFds[numSensorDrivers]; + SensorBase *mSensor; +}; + +/******************************************************************************/ + +sensors_poll_context_t::sensors_poll_context_t() { + VFUNC_LOG; + + mCompassSensor = new CompassSensor(); + MPLSensor *mplSensor = new MPLSensor(mCompassSensor); + + /* For Vendor-defined Accel Calibration File Load + * Use the Following Constructor and Pass Your Load Cal File Function + * + * MPLSensor *mplSensor = new MPLSensor(mCompassSensor, AccelLoadConfig); + */ + + // setup the callback object for handing mpl callbacks + setCallbackObject(mplSensor); + + // populate the sensor list + sensors = + mplSensor->populateSensorList(sSensorList, sizeof(sSensorList)); + + mSensor = mplSensor; + mPollFds[mpl].fd = mSensor->getFd(); + mPollFds[mpl].events = POLLIN; + mPollFds[mpl].revents = 0; + + mPollFds[compass].fd = mCompassSensor->getFd(); + mPollFds[compass].events = POLLIN; + mPollFds[compass].revents = 0; + + mPollFds[dmpOrient].fd = ((MPLSensor*) mSensor)->getDmpOrientFd(); + mPollFds[dmpOrient].events = POLLPRI; + mPollFds[dmpOrient].revents = 0; +} + +sensors_poll_context_t::~sensors_poll_context_t() { + FUNC_LOG; + delete mSensor; +} + +int sensors_poll_context_t::activate(int handle, int enabled) { + FUNC_LOG; + return mSensor->enable(handle, enabled); +} + +int sensors_poll_context_t::setDelay(int handle, int64_t ns) +{ + FUNC_LOG; + return mSensor->setDelay(handle, ns); +} + +int sensors_poll_context_t::pollEvents(sensors_event_t *data, int count) +{ + VHANDLER_LOG; + + int nbEvents = 0; + int nb, polltime = -1; + + // look for new events + nb = poll(mPollFds, numSensorDrivers, polltime); + + if (nb > 0) { + for (int i = 0; count && i < numSensorDrivers; i++) { + if (mPollFds[i].revents & (POLLIN | POLLPRI)) { + nb = 0; + if (i == mpl) { + /* Ignore res */ + mSensor->readEvents(NULL, 0); + mPollFds[i].revents = 0; + } + else if (i == compass) { + /* Ignore res */ + ((MPLSensor*) mSensor)->readCompassEvents(NULL, count); + mPollFds[i].revents = 0; + } + } + } + nb = ((MPLSensor*) mSensor)->executeOnData(data, count); + if (nb > 0) { + count -= nb; + nbEvents += nb; + data += nb; + } + + if (mPollFds[dmpOrient].revents & (POLLIN | POLLPRI)) { + nb = ((MPLSensor*) mSensor)->readDmpOrientEvents(data, count); + mPollFds[dmpOrient].revents= 0; + if (isDmpScreenAutoRotationEnabled() && nb > 0) { + count -= nb; + nbEvents += nb; + data += nb; + } + } + } + + return nbEvents; +} + +/******************************************************************************/ + +static int poll__close(struct hw_device_t *dev) +{ + FUNC_LOG; + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + if (ctx) { + delete ctx; + } + return 0; +} + +static int poll__activate(struct sensors_poll_device_t *dev, + int handle, int enabled) +{ + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->activate(handle, enabled); +} + +static int poll__setDelay(struct sensors_poll_device_t *dev, + int handle, int64_t ns) +{ + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + int s= ctx->setDelay(handle, ns); + return s; +} + +static int poll__poll(struct sensors_poll_device_t *dev, + sensors_event_t* data, int count) +{ + sensors_poll_context_t *ctx = (sensors_poll_context_t *)dev; + return ctx->pollEvents(data, count); +} + +/******************************************************************************/ + +/** Open a new instance of a sensor device using name */ +static int open_sensors(const struct hw_module_t* module, const char* id, + struct hw_device_t** device) +{ + FUNC_LOG; + int status = -EINVAL; + sensors_poll_context_t *dev = new sensors_poll_context_t(); + + memset(&dev->device, 0, sizeof(sensors_poll_device_t)); + + dev->device.common.tag = HARDWARE_DEVICE_TAG; + dev->device.common.version = 0; + dev->device.common.module = const_cast(module); + dev->device.common.close = poll__close; + dev->device.activate = poll__activate; + dev->device.setDelay = poll__setDelay; + dev->device.poll = poll__poll; + + *device = &dev->device.common; + status = 0; + + return status; +} diff --git a/60xx/libsensors_iio/software/build/android/common.mk b/60xx/libsensors_iio/software/build/android/common.mk new file mode 100644 index 0000000..84e7e9b --- /dev/null +++ b/60xx/libsensors_iio/software/build/android/common.mk @@ -0,0 +1,87 @@ +# Use bash for additional echo fancyness +SHELL = /bin/bash + +#################################################################################################### +## defines + +# Build for Jellybean +BUILD_ANDROID_JELLYBEAN = 1 + +## libraries ## +LIB_PREFIX = lib + +STATIC_LIB_EXT = a +SHARED_LIB_EXT = so + +# normally, overridden from outside +# ?= assignment sets it only if not already defined +TARGET ?= android + +MLLITE_LIB_NAME ?= mllite +MPL_LIB_NAME ?= mplmpu + +## applications ## +SHARED_APP_SUFFIX = -shared +STATIC_APP_SUFFIX = -static + +#################################################################################################### +## compile, includes, and linker + +ifeq ($(BUILD_ANDROID_JELLYBEAN),1) +ANDROID_COMPILE = -DANDROID_JELLYBEAN=1 +endif + +ANDROID_LINK = -nostdlib +ANDROID_LINK += -fpic +ANDROID_LINK += -Wl,--gc-sections +ANDROID_LINK += -Wl,--no-whole-archive +ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib +ANDROID_LINK += -L$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +ANDROID_LINK_EXECUTABLE = $(ANDROID_LINK) +ANDROID_LINK_EXECUTABLE += -Wl,-dynamic-linker,/system/bin/linker +ifneq ($(BUILD_ANDROID_JELLYBEAN),1) +ANDROID_LINK_EXECUTABLE += -Wl,-T,$(ANDROID_ROOT)/build/core/armelf.x +endif +ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtbegin_dynamic.o +ANDROID_LINK_EXECUTABLE += $(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib/crtend_android.o + +ANDROID_INCLUDES = -I$(ANDROID_ROOT)/system/core/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/libhardware/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/hardware/ril/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/dalvik/libnativehelper/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/base/include # ICS +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/frameworks/native/include # Jellybean +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/external/skia/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/arch-arm/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libstdc++/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/common +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libc/kernel/arch-arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/include/arch/arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libthread_db/include +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm/arm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/bionic/libm +ANDROID_INCLUDES += -I$(ANDROID_ROOT)/out/target/product/generic/obj/SHARED_LIBRARIES/libm_intermediates + +KERNEL_INCLUDES = -I$(KERNEL_ROOT)/include + +INV_INCLUDES = -I$(INV_ROOT)/software/core/driver/include +INV_INCLUDES += -I$(MLLITE_DIR) +INV_INCLUDES += -I$(MLLITE_DIR)/linux + +INV_DEFINES += -DINV_CACHE_DMP=1 + +#################################################################################################### +## macros + +ifndef echo_in_colors +define echo_in_colors + echo -ne "\e[1;32m"$(1)"\e[0m" +endef +endif + + + diff --git a/60xx/libsensors_iio/software/build/android/shared.mk b/60xx/libsensors_iio/software/build/android/shared.mk new file mode 100644 index 0000000..47dedfe --- /dev/null +++ b/60xx/libsensors_iio/software/build/android/shared.mk @@ -0,0 +1,74 @@ +SHELL=/bin/bash + +TARGET ?= android +PRODUCT ?= beagleboard +ANDROID_ROOT ?= /Android/trunk/0xdroid/beagle-eclair +KERNEL_ROOT ?= /Android/trunk/0xdroid/kernel +MLSDK_ROOT ?= $(CURDIR) + +ifeq ($(VERBOSE),1) + DUMP=1>/dev/stdout +else + DUMP=1>/dev/null +endif + +include common.mk + +################################################################################ +## targets + +INV_ROOT = ../.. +LIB_FOLDERS = $(INV_ROOT)/core/mllite/build/$(TARGET) +ifeq ($(BUILD_MPL),1) + LIB_FOLDERS += $(INV_ROOT)/core/mpl/build/$(TARGET) +endif +APP_FOLDERS = $(INV_ROOT)/simple_apps/mpu_iio/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/self_test/build/$(TARGET) +APP_FOLDERS += $(INV_ROOT)/simple_apps/gesture_test/build/$(TARGET) + +INSTALL_DIR = $(CURDIR) + +################################################################################ +## macros + +define echo_in_colors + echo -ne "\e[1;34m"$(1)"\e[0m" +endef + +################################################################################ +## rules + +.PHONY : all mllite mpl clean + +all: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + +clean: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + +cleanall: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + +install: + for DIR in $(LIB_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + for DIR in $(APP_FOLDERS); do ( \ + cd $$DIR && $(MAKE) -f shared.mk $@ ); \ + done + diff --git a/60xx/libsensors_iio/software/core/driver/include/log.h b/60xx/libsensors_iio/software/core/driver/include/log.h new file mode 100644 index 0000000..c519691 --- /dev/null +++ b/60xx/libsensors_iio/software/core/driver/include/log.h @@ -0,0 +1,364 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/* + * This file incorporates work covered by the following copyright and + * permission notice: + * + * Copyright (C) 2005 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * C/C++ logging functions. See the logging documentation for API details. + * + * We'd like these to be available from C code (in case we import some from + * somewhere), so this has a C interface. + * + * The output will be correct when the log file is shared between multiple + * threads and/or multiple processes so long as the operating system + * supports O_APPEND. These calls have mutex-protected data structures + * and so are NOT reentrant. Do not use MPL_LOG in a signal handler. + */ +#ifndef _LIBS_CUTILS_MPL_LOG_H +#define _LIBS_CUTILS_MPL_LOG_H + +#include +#include + +#ifdef ANDROID +#ifdef NDK_BUILD +#include "log_macros.h" +#else +#include /* For the LOG macro */ +#endif +#endif + +#ifdef __KERNEL__ +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + + +/* --------------------------------------------------------------------- */ + +/* + * Normally we strip MPL_LOGV (VERBOSE messages) from release builds. + * You can modify this (for example with "#define MPL_LOG_NDEBUG 0" + * at the top of your source file) to change that behavior. + */ +#ifndef MPL_LOG_NDEBUG +#ifdef NDEBUG +#define MPL_LOG_NDEBUG 1 +#else +#define MPL_LOG_NDEBUG 0 +#endif +#endif + +#ifdef __KERNEL__ +#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE +#define MPL_LOG_DEFAULT KERN_DEFAULT +#define MPL_LOG_VERBOSE KERN_CONT +#define MPL_LOG_DEBUG KERN_NOTICE +#define MPL_LOG_INFO KERN_INFO +#define MPL_LOG_WARN KERN_WARNING +#define MPL_LOG_ERROR KERN_ERR +#define MPL_LOG_SILENT MPL_LOG_VERBOSE + +#else + /* Based off the log priorities in android + /system/core/include/android/log.h */ +#define MPL_LOG_UNKNOWN (0) +#define MPL_LOG_DEFAULT (1) +#define MPL_LOG_VERBOSE (2) +#define MPL_LOG_DEBUG (3) +#define MPL_LOG_INFO (4) +#define MPL_LOG_WARN (5) +#define MPL_LOG_ERROR (6) +#define MPL_LOG_SILENT (8) +#endif + + +/* + * This is the local tag used for the following simplified + * logging macros. You can change this preprocessor definition + * before using the other macros to change the tag. + */ +#ifndef MPL_LOG_TAG +#ifdef __KERNEL__ +#define MPL_LOG_TAG +#else +#define MPL_LOG_TAG NULL +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Simplified macro to send a verbose log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGV +#if MPL_LOG_NDEBUG +#ifdef _WIN32 +#define MPL_LOGV(fmt, ...) \ + do { \ + __pragma (warning(suppress : 4127 )) \ + if (0) \ + MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ + __pragma (warning(suppress : 4127 )) \ + } while (0) +#else +#define MPL_LOGV(fmt, ...) \ + do { \ + if (0) \ + MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ + } while (0) +#endif + +#else +#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef CONDITION +#define CONDITION(cond) ((cond) != 0) +#endif + +#ifndef MPL_LOGV_IF +#if MPL_LOG_NDEBUG +#define MPL_LOGV_IF(cond, fmt, ...) \ + do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0) +#else +#define MPL_LOGV_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif +#endif + +/* + * Simplified macro to send a debug log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGD +#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif + +#ifndef MPL_LOGD_IF +#define MPL_LOGD_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send an info log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGI +#ifdef __KERNEL__ +#define MPL_LOGI(fmt, ...) pr_info(KERN_INFO MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGI_IF +#define MPL_LOGI_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send a warning log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGW +#ifdef __KERNEL__ +#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGW_IF +#define MPL_LOGW_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send an error log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGE +#ifdef __KERNEL__ +#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGE_IF +#define MPL_LOGE_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Log a fatal error. If the given condition fails, this stops program + * execution like a normal assertion, but also generating the given message. + * It is NOT stripped from release builds. Note that the condition test + * is -inverted- from the normal assert() semantics. + */ +#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? ((void)android_printAssert(#cond, MPL_LOG_TAG, \ + fmt, ##__VA_ARGS__)) \ + : (void)0) + +#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \ + (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__))) + +/* + * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that + * are stripped out of release builds. + */ +#if MPL_LOG_NDEBUG +#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ + do { \ + if (0) \ + MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \ + } while (0) +#define MPL_LOG_FATAL(fmt, ...) \ + do { \ + if (0) \ + MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) \ + } while (0) +#else +#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ + MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__) +#define MPL_LOG_FATAL(fmt, ...) \ + MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) +#endif + +/* + * Assertion that generates a log message when the assertion fails. + * Stripped out of release builds. Uses the current MPL_LOG_TAG. + */ +#define MPL_LOG_ASSERT(cond, fmt, ...) \ + MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__) + +/* --------------------------------------------------------------------- */ + +/* + * Basic log message macro. + * + * Example: + * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno); + * + * The second argument may be NULL or "" to indicate the "global" tag. + */ +#ifndef MPL_LOG +#define MPL_LOG(priority, tag, fmt, ...) \ + MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__) +#endif + +/* + * Log macro that allows you to specify a number for the priority. + */ +#ifndef MPL_LOG_PRI +#ifdef ANDROID +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + ALOG(priority, tag, fmt, ##__VA_ARGS__) +#elif defined __KERNEL__ +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__) +#else +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__) +#endif +#endif + +/* + * Log macro that allows you to pass in a varargs ("args" is a va_list). + */ +#ifndef MPL_LOG_PRI_VA +#ifdef ANDROID +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + android_vprintLog(priority, NULL, tag, fmt, args) +#elif defined __KERNEL__ +/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */ +#else +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + _MLPrintVaLog(priority, NULL, tag, fmt, args) +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * =========================================================================== + * + * The stuff in the rest of this file should not be used directly. + */ + +#ifndef ANDROID +int _MLPrintLog(int priority, const char *tag, const char *fmt, ...); +int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args); +/* Final implementation of actual writing to a character device */ +int _MLWriteLog(const char *buf, int buflen); +#endif + +static inline void __print_result_location(int result, + const char *file, + const char *func, int line) +{ + MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result); +} + +#ifdef _WIN32 +/* The pragma removes warning about expression being constant */ +#define LOG_RESULT_LOCATION(condition) \ + do { \ + __print_result_location((int)(condition), __FILE__, \ + __func__, __LINE__); \ + __pragma (warning(suppress : 4127 )) \ + } while (0) +#else +#define LOG_RESULT_LOCATION(condition) \ + do { \ + __print_result_location((int)(condition), __FILE__, \ + __func__, __LINE__); \ + } while (0) +#endif + + +#define INV_ERROR_CHECK(r_1329) \ + if (r_1329) { \ + LOG_RESULT_LOCATION(r_1329); \ + return r_1329; \ + } + +#ifdef __cplusplus +} +#endif +#endif /* _LIBS_CUTILS_MPL_LOG_H */ diff --git a/60xx/libsensors_iio/software/core/driver/include/mlinclude.h b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h new file mode 100644 index 0000000..9725199 --- /dev/null +++ b/60xx/libsensors_iio/software/core/driver/include/mlinclude.h @@ -0,0 +1,38 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef INV_INCLUDE_H__ +#define INV_INCLUDE_H__ + +#define INVENSENSE_FUNC_START typedef int invensensePutFunctionCallsHere + +#ifdef COVERAGE +#include "utestCommon.h" +#endif +#ifdef PROFILE +#include "profile.h" +#endif + +#ifdef WIN32 +#ifdef COVERAGE + +extern int functionEnterLog(const char *file, const char *func); +extern int functionExitLog(const char *file, const char *func); + +#undef INVENSENSE_FUNC_START +#define INVENSENSE_FUNC_START __pragma(message(__FILE__ "|"__FUNCTION__ )) \ + int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__) +#endif // COVERAGE +#endif // WIN32 + +#ifdef PROFILE +#undef INVENSENSE_FUNC_START +#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__) +#define return if ( profileExit(__FILE__, __FUNCTION__) ) return +#endif // PROFILE + +// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return + +#endif //INV_INCLUDE_H__ diff --git a/60xx/libsensors_iio/software/core/driver/include/mlmath.h b/60xx/libsensors_iio/software/core/driver/include/mlmath.h new file mode 100644 index 0000000..37194d6 --- /dev/null +++ b/60xx/libsensors_iio/software/core/driver/include/mlmath.h @@ -0,0 +1,95 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +/******************************************************************************* + * + * $Id: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#ifndef _ML_MATH_H_ +#define _ML_MATH_H_ + +#ifndef MLMATH +// This define makes Microsoft pickup things like M_PI +#define _USE_MATH_DEFINES +#include + +#ifdef WIN32 +// Microsoft doesn't follow standards +#define round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5)))) +#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) +#endif + +#else // MLMATH + +#ifdef __cplusplus +extern "C" { +#endif +/* MPL needs below functions */ +double ml_asin(double); +double ml_atan(double); +double ml_atan2(double, double); +double ml_log(double); +double ml_sqrt(double); +double ml_ceil(double); +double ml_floor(double); +double ml_cos(double); +double ml_sin(double); +double ml_acos(double); +#ifdef __cplusplus +} // extern "C" +#endif + +/* + * We rename functions here to provide the hook for other + * customized math functions. + */ +#define sqrt(x) ml_sqrt(x) +#define log(x) ml_log(x) +#define asin(x) ml_asin(x) +#define atan(x) ml_atan(x) +#define atan2(x,y) ml_atan2(x,y) +#define ceil(x) ml_ceil(x) +#define floor(x) ml_floor(x) +#define fabs(x) (((x)<0)?-(x):(x)) +#define round(x) (((double)((long long)((x)>0?(x)+.5:(x)-.5)))) +#define roundf(x) (((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) +#define cos(x) ml_cos(x) +#define sin(x) ml_sin(x) +#define acos(x) ml_acos(x) + +#define pow(x,y) ml_pow(x,y) + +#ifdef LINUX +/* stubs for float version of math functions */ +#define cosf(x) ml_cos(x) +#define sinf(x) ml_sin(x) +#define atan2f(x,y) ml_atan2(x,y) +#define sqrtf(x) ml_sqrt(x) +#endif + + + +#endif // MLMATH + +#ifndef M_PI +#define M_PI 3.14159265358979 +#endif + +#ifndef ABS +#define ABS(x) (((x)>=0)?(x):-(x)) +#endif + +#ifndef MIN +#define MIN(x,y) (((x)<(y))?(x):(y)) +#endif + +#ifndef MAX +#define MAX(x,y) (((x)>(y))?(x):(y)) +#endif + +/*---------------------------*/ +#endif /* !_ML_MATH_H_ */ diff --git a/60xx/libsensors_iio/software/core/driver/include/mlsl.h b/60xx/libsensors_iio/software/core/driver/include/mlsl.h new file mode 100644 index 0000000..12f2901 --- /dev/null +++ b/60xx/libsensors_iio/software/core/driver/include/mlsl.h @@ -0,0 +1,283 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef __MLSL_H__ +#define __MLSL_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @defgroup MLSL + * @brief Motion Library - Serial Layer. + * The Motion Library System Layer provides the Motion Library + * with the communication interface to the hardware. + * + * The communication interface is assumed to support serial + * transfers in burst of variable length up to + * SERIAL_MAX_TRANSFER_SIZE. + * The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes. + * Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will + * be subdivided in smaller transfers of length <= + * SERIAL_MAX_TRANSFER_SIZE. + * The SERIAL_MAX_TRANSFER_SIZE definition can be modified to + * overcome any host processor transfer size limitation down to + * 1 B, the minimum. + * An higher value for SERIAL_MAX_TRANSFER_SIZE will favor + * performance and efficiency while requiring higher resource usage + * (mostly buffering). A smaller value will increase overhead and + * decrease efficiency but allows to operate with more resource + * constrained processor and master serial controllers. + * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the + * mlsl.h header file and master serial controllers. + * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the + * mlsl.h header file. + * + * @{ + * @file mlsl.h + * @brief The Motion Library System Layer. + * + */ + +/* + * NOTE : to properly support Yamaha compass reads, + * the max transfer size should be at least 9 B. + * Length in bytes, typically a power of 2 >= 2 + */ +#define SERIAL_MAX_TRANSFER_SIZE 31 + +#ifndef __KERNEL__ +/** + * inv_serial_open() - used to open the serial port. + * @port The COM port specification associated with the device in use. + * @sl_handle a pointer to the file handle to the serial device to be open + * for the communication. + * This port is used to send and receive data to the device. + * + * This function is called by inv_serial_start(). + * Unlike previous MPL Software releases, explicitly calling + * inv_serial_start() is mandatory to instantiate the communication + * with the device. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_open(char const *port, void **sl_handle); + +/** + * inv_serial_close() - used to close the serial port. + * @sl_handle a file handle to the serial device used for the communication. + * + * This port is used to send and receive data to the device. + * + * This function is called by inv_serial_stop(). + * Unlike previous MPL Software releases, explicitly calling + * inv_serial_stop() is mandatory to properly shut-down the + * communication with the device. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_close(void *sl_handle); + +/** + * inv_serial_reset() - used to reset any buffering the driver may be doing + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_reset(void *sl_handle); +#endif + +/** + * inv_serial_single_write() - used to write a single byte of data. + * @sl_handle pointer to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to write. + * @data Single byte of data to write. + * + * It is called by the MPL to write a single byte of data to the MPU. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_single_write( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned char data); + +/** + * inv_serial_write() - used to write multiple bytes of data to registers. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to write. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_write( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char const *data); + +/** + * inv_serial_read() - used to read multiple bytes of data from registers. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to read. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_read( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_read_mem() - used to read multiple bytes of data from the memory. + * This should be sent by I2C or SPI. + * + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @mem_addr The location in the memory to read from. + * @length Length of burst data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_read_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned char bank_reg, + unsigned char addr_reg, + unsigned char mem_reg, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_write_mem() - used to write multiple bytes of data to the memory. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @mem_addr The location in the memory to write to. + * @length Length of burst data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_write_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned char bank_reg, + unsigned char addr_reg, + unsigned char mem_reg, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_read_fifo() - used to read multiple bytes of data from the fifo. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_read_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned char fifo_reg, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_write_fifo() - used to write multiple bytes of data to the fifo. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +int inv_serial_write_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned char fifo_reg, + unsigned short length, + unsigned char const *data); + +#ifndef __KERNEL__ +/** + * inv_serial_read_cfg() - used to get the configuration data. + * @cfg Pointer to the configuration data. + * @len Length of the configuration data. + * + * Is called by the MPL to get the configuration data + * used by the motion library. + * This data would typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_read_cfg(unsigned char *cfg, unsigned int len); + +/** + * inv_serial_write_cfg() - used to save the configuration data. + * @cfg Pointer to the configuration data. + * @len Length of the configuration data. + * + * Is called by the MPL to save the configuration data used by the + * motion library. + * This data would typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_write_cfg(unsigned char *cfg, unsigned int len); + +/** + * inv_serial_read_cal() - used to get the calibration data. + * @cfg Pointer to the calibration data. + * @len Length of the calibration data. + * + * It is called by the MPL to get the calibration data used by the + * motion library. + * This data is typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_read_cal(unsigned char *cal, unsigned int len); + +/** + * inv_serial_write_cal() - used to save the calibration data. + * + * @cfg Pointer to the calibration data. + * @len Length of the calibration data. + * + * It is called by the MPL to save the calibration data used by the + * motion library. + * This data is typically be saved in non-volatile memory. + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_write_cal(unsigned char *cal, unsigned int len); + +/** + * inv_serial_get_cal_length() - Get the calibration length from the storage. + * @len lenght to be returned + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +int inv_serial_get_cal_length(unsigned int *len); +#endif +#ifdef __cplusplus +} +#endif +/** + * @} + */ +#endif /* __MLSL_H__ */ diff --git a/60xx/libsensors_iio/software/core/driver/include/mltypes.h b/60xx/libsensors_iio/software/core/driver/include/mltypes.h new file mode 100644 index 0000000..09eccce --- /dev/null +++ b/60xx/libsensors_iio/software/core/driver/include/mltypes.h @@ -0,0 +1,235 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/** + * @defgroup MLERROR + * @brief Motion Library - Error definitions. + * Definition of the error codes used within the MPL and + * returned to the user. + * Every function tries to return a meaningful error code basing + * on the occuring error condition. The error code is numeric. + * + * The available error codes and their associated values are: + * - (0) INV_SUCCESS + * - (32) INV_ERROR + * - (22 / EINVAL) INV_ERROR_INVALID_PARAMETER + * - (1 / EPERM) INV_ERROR_FEATURE_NOT_ENABLED + * - (36) INV_ERROR_FEATURE_NOT_IMPLEMENTED + * - (38) INV_ERROR_DMP_NOT_STARTED + * - (39) INV_ERROR_DMP_STARTED + * - (40) INV_ERROR_NOT_OPENED + * - (41) INV_ERROR_OPENED + * - (19 / ENODEV) INV_ERROR_INVALID_MODULE + * - (12 / ENOMEM) INV_ERROR_MEMORY_EXAUSTED + * - (44) INV_ERROR_DIVIDE_BY_ZERO + * - (45) INV_ERROR_ASSERTION_FAILURE + * - (46) INV_ERROR_FILE_OPEN + * - (47) INV_ERROR_FILE_READ + * - (48) INV_ERROR_FILE_WRITE + * - (49) INV_ERROR_INVALID_CONFIGURATION + * - (52) INV_ERROR_SERIAL_CLOSED + * - (53) INV_ERROR_SERIAL_OPEN_ERROR + * - (54) INV_ERROR_SERIAL_READ + * - (55) INV_ERROR_SERIAL_WRITE + * - (56) INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED + * - (57) INV_ERROR_SM_TRANSITION + * - (58) INV_ERROR_SM_IMPROPER_STATE + * - (62) INV_ERROR_FIFO_OVERFLOW + * - (63) INV_ERROR_FIFO_FOOTER + * - (64) INV_ERROR_FIFO_READ_COUNT + * - (65) INV_ERROR_FIFO_READ_DATA + * - (72) INV_ERROR_MEMORY_SET + * - (82) INV_ERROR_LOG_MEMORY_ERROR + * - (83) INV_ERROR_LOG_OUTPUT_ERROR + * - (92) INV_ERROR_OS_BAD_PTR + * - (93) INV_ERROR_OS_BAD_HANDLE + * - (94) INV_ERROR_OS_CREATE_FAILED + * - (95) INV_ERROR_OS_LOCK_FAILED + * - (102) INV_ERROR_COMPASS_DATA_OVERFLOW + * - (103) INV_ERROR_COMPASS_DATA_UNDERFLOW + * - (104) INV_ERROR_COMPASS_DATA_NOT_READY + * - (105) INV_ERROR_COMPASS_DATA_ERROR + * - (107) INV_ERROR_CALIBRATION_LOAD + * - (108) INV_ERROR_CALIBRATION_STORE + * - (109) INV_ERROR_CALIBRATION_LEN + * - (110) INV_ERROR_CALIBRATION_CHECKSUM + * - (111) INV_ERROR_ACCEL_DATA_OVERFLOW + * - (112) INV_ERROR_ACCEL_DATA_UNDERFLOW + * - (113) INV_ERROR_ACCEL_DATA_NOT_READY + * - (114) INV_ERROR_ACCEL_DATA_ERROR + * + * The available warning codes and their associated values are: + * - (115) INV_WARNING_MOTION_RACE + * - (116) INV_WARNING_QUAT_TRASHED + * + * @{ + * @file mltypes.h + * @} + */ + +#ifndef MLTYPES_H +#define MLTYPES_H + +#ifdef __KERNEL__ +#include +#include +#else +#include "stdint_invensense.h" +#include +#endif +#include + +#ifndef REMOVE_INV_ERROR_T +/*--------------------------- + * ML Types + *--------------------------*/ + +/** + * @struct inv_error_t mltypes.h "mltypes" + * @brief The MPL Error Code return type. + * + * @code + * typedef unsigned char inv_error_t; + * @endcode + */ +//typedef unsigned char inv_error_t; +typedef int inv_error_t; +#endif + +typedef long long inv_time_t; + +#if !defined __GNUC__ && !defined __KERNEL__ +typedef int8_t __s8; +typedef int16_t __s16; +typedef int32_t __s32; +typedef int32_t __s64; + +typedef uint8_t __u8; +typedef uint16_t __u16; +typedef uint32_t __u32; +typedef uint64_t __u64; +#elif !defined __KERNEL__ +#include +#endif + +#ifndef __cplusplus +#ifndef __KERNEL__ +typedef int_fast8_t bool; + +#ifndef false +#define false 0 +#endif + +#ifndef true +#define true 1 +#endif + +#endif +#endif + +/*--------------------------- + * ML Defines + *--------------------------*/ + +#ifndef NULL +#define NULL 0 +#endif + +#ifndef __KERNEL__ +#ifndef ARRAY_SIZE +/* Dimension of an array */ +#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0])) +#endif +#endif +/* - ML Errors. - */ +#define ERROR_NAME(x) (#x) +#define ERROR_CHECK_FIRST(first, x) \ + { if (INV_SUCCESS == first) first = x; } + +#define INV_SUCCESS (0) +/* Generic Error code. Proprietary Error Codes only */ +#define INV_ERROR_BASE (0x20) +#define INV_ERROR (INV_ERROR_BASE) + +/* Compatibility and other generic error codes */ +#define INV_ERROR_INVALID_PARAMETER (EINVAL) +#define INV_ERROR_FEATURE_NOT_ENABLED (EPERM) +#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (INV_ERROR_BASE + 4) +#define INV_ERROR_DMP_NOT_STARTED (INV_ERROR_BASE + 6) +#define INV_ERROR_DMP_STARTED (INV_ERROR_BASE + 7) +#define INV_ERROR_NOT_OPENED (INV_ERROR_BASE + 8) +#define INV_ERROR_OPENED (INV_ERROR_BASE + 9) +#define INV_ERROR_INVALID_MODULE (ENODEV) +#define INV_ERROR_MEMORY_EXAUSTED (ENOMEM) +#define INV_ERROR_DIVIDE_BY_ZERO (INV_ERROR_BASE + 12) +#define INV_ERROR_ASSERTION_FAILURE (INV_ERROR_BASE + 13) +#define INV_ERROR_FILE_OPEN (INV_ERROR_BASE + 14) +#define INV_ERROR_FILE_READ (INV_ERROR_BASE + 15) +#define INV_ERROR_FILE_WRITE (INV_ERROR_BASE + 16) +#define INV_ERROR_INVALID_CONFIGURATION (INV_ERROR_BASE + 17) +#define INV_ERROR_NOT_AUTHORIZED (INV_ERROR_BASE + 18) + +/* Serial Communication */ +#define INV_ERROR_SERIAL_CLOSED (INV_ERROR_BASE + 20) +#define INV_ERROR_SERIAL_OPEN_ERROR (INV_ERROR_BASE + 21) +#define INV_ERROR_SERIAL_READ (INV_ERROR_BASE + 22) +#define INV_ERROR_SERIAL_WRITE (INV_ERROR_BASE + 23) +#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (INV_ERROR_BASE + 24) + +/* SM = State Machine */ +#define INV_ERROR_SM_TRANSITION (INV_ERROR_BASE + 25) +#define INV_ERROR_SM_IMPROPER_STATE (INV_ERROR_BASE + 26) + +/* Fifo */ +#define INV_ERROR_FIFO_OVERFLOW (INV_ERROR_BASE + 30) +#define INV_ERROR_FIFO_FOOTER (INV_ERROR_BASE + 31) +#define INV_ERROR_FIFO_READ_COUNT (INV_ERROR_BASE + 32) +#define INV_ERROR_FIFO_READ_DATA (INV_ERROR_BASE + 33) + +/* Memory & Registers, Set & Get */ +#define INV_ERROR_MEMORY_SET (INV_ERROR_BASE + 40) + +#define INV_ERROR_LOG_MEMORY_ERROR (INV_ERROR_BASE + 50) +#define INV_ERROR_LOG_OUTPUT_ERROR (INV_ERROR_BASE + 51) + +/* OS interface errors */ +#define INV_ERROR_OS_BAD_PTR (INV_ERROR_BASE + 60) +#define INV_ERROR_OS_BAD_HANDLE (INV_ERROR_BASE + 61) +#define INV_ERROR_OS_CREATE_FAILED (INV_ERROR_BASE + 62) +#define INV_ERROR_OS_LOCK_FAILED (INV_ERROR_BASE + 63) + +/* Compass errors */ +#define INV_ERROR_COMPASS_DATA_OVERFLOW (INV_ERROR_BASE + 70) +#define INV_ERROR_COMPASS_DATA_UNDERFLOW (INV_ERROR_BASE + 71) +#define INV_ERROR_COMPASS_DATA_NOT_READY (INV_ERROR_BASE + 72) +#define INV_ERROR_COMPASS_DATA_ERROR (INV_ERROR_BASE + 73) + +/* Load/Store calibration */ +#define INV_ERROR_CALIBRATION_LOAD (INV_ERROR_BASE + 75) +#define INV_ERROR_CALIBRATION_STORE (INV_ERROR_BASE + 76) +#define INV_ERROR_CALIBRATION_LEN (INV_ERROR_BASE + 77) +#define INV_ERROR_CALIBRATION_CHECKSUM (INV_ERROR_BASE + 78) + +/* Accel errors */ +#define INV_ERROR_ACCEL_DATA_OVERFLOW (INV_ERROR_BASE + 79) +#define INV_ERROR_ACCEL_DATA_UNDERFLOW (INV_ERROR_BASE + 80) +#define INV_ERROR_ACCEL_DATA_NOT_READY (INV_ERROR_BASE + 81) +#define INV_ERROR_ACCEL_DATA_ERROR (INV_ERROR_BASE + 82) + +/* No Motion Warning States */ +#define INV_WARNING_MOTION_RACE (INV_ERROR_BASE + 83) +#define INV_WARNING_QUAT_TRASHED (INV_ERROR_BASE + 84) +#define INV_WARNING_GYRO_MAG (INV_ERROR_BASE + 85) + +#define INV_WARNING_SEMAPHORE_TIMEOUT (INV_ERROR_BASE + 86) + + +/* For Linux coding compliance */ +#ifndef __KERNEL__ +#define EXPORT_SYMBOL(x) +#endif + +#endif /* MLTYPES_H */ diff --git a/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h new file mode 100644 index 0000000..b8c2511 --- /dev/null +++ b/60xx/libsensors_iio/software/core/driver/include/stdint_invensense.h @@ -0,0 +1,41 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ +#ifndef STDINT_INVENSENSE_H +#define STDINT_INVENSENSE_H + +#ifndef WIN32 + +#ifdef __KERNEL__ +#include +#else +#include +#endif + +#else + +#include + +typedef signed char int8_t; +typedef short int16_t; +typedef long int32_t; +typedef long long int64_t; + +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned long uint32_t; +typedef unsigned long long uint64_t; + +typedef int int_fast8_t; +typedef int int_fast16_t; +typedef long int_fast32_t; + +typedef unsigned int uint_fast8_t; +typedef unsigned int uint_fast16_t; +typedef unsigned long uint_fast32_t; + +#endif + +#endif // STDINT_INVENSENSE_H diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so new file mode 100755 index 0000000..9bdd5bc Binary files /dev/null and b/60xx/libsensors_iio/software/core/mllite/build/android/libmllite.so differ diff --git a/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk new file mode 100644 index 0000000..1418450 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/build/android/shared.mk @@ -0,0 +1,87 @@ +MLLITE_LIB_NAME = mllite +LIBRARY = $(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += -I$(MLLITE_DIR) +CFLAGS += -I$(INV_ROOT)/simple_apps/common +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -shared +LFLAGS += -Wl,-soname,$(LIBRARY) +LFLAGS += -Wl,-shared,-Bsymbolic +LFLAGS += $(ANDROID_LINK) +LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +#INV_SOURCES provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all mllite clean cleanall makefiles + +all: mllite + +mllite: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) + @$(call echo_in_colors, "\n\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(LIBRARY) $(OBJFOLDER) + diff --git a/60xx/libsensors_iio/software/core/mllite/build/filelist.mk b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk new file mode 100644 index 0000000..011120c --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/build/filelist.mk @@ -0,0 +1,42 @@ +#### filelist.mk for mllite #### + +# headers only +HEADERS := $(MLLITE_DIR)/invensense.h + +# headers for sources +HEADERS += $(MLLITE_DIR)/data_builder.h +HEADERS += $(MLLITE_DIR)/hal_outputs.h +HEADERS += $(MLLITE_DIR)/message_layer.h +HEADERS += $(MLLITE_DIR)/ml_math_func.h +HEADERS += $(MLLITE_DIR)/mpl.h +HEADERS += $(MLLITE_DIR)/results_holder.h +HEADERS += $(MLLITE_DIR)/start_manager.h +HEADERS += $(MLLITE_DIR)/storage_manager.h + +# headers (linux specific) +HEADERS += $(MLLITE_DIR)/linux/mlos.h +HEADERS += $(MLLITE_DIR)/linux/ml_stored_data.h +HEADERS += $(MLLITE_DIR)/linux/ml_load_dmp.h +HEADERS += $(MLLITE_DIR)/linux/ml_sysfs_helper.h + +# sources +SOURCES := $(MLLITE_DIR)/data_builder.c +SOURCES += $(MLLITE_DIR)/hal_outputs.c +SOURCES += $(MLLITE_DIR)/message_layer.c +SOURCES += $(MLLITE_DIR)/ml_math_func.c +SOURCES += $(MLLITE_DIR)/mpl.c +SOURCES += $(MLLITE_DIR)/results_holder.c +SOURCES += $(MLLITE_DIR)/start_manager.c +SOURCES += $(MLLITE_DIR)/storage_manager.c + +# sources (linux specific) +SOURCES += $(MLLITE_DIR)/linux/mlos_linux.c +SOURCES += $(MLLITE_DIR)/linux/ml_stored_data.c +SOURCES += $(MLLITE_DIR)/linux/ml_load_dmp.c +SOURCES += $(MLLITE_DIR)/linux/ml_sysfs_helper.c + + +INV_SOURCES += $(SOURCES) + +VPATH += $(MLLITE_DIR) $(MLLITE_DIR)/linux + diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.c b/60xx/libsensors_iio/software/core/mllite/data_builder.c new file mode 100644 index 0000000..0aa418d --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/data_builder.c @@ -0,0 +1,1228 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/** + * @defgroup Data_Builder data_builder + * @brief Motion Library - Data Builder + * Constructs and Creates the data for MPL + * + * @{ + * @file data_builder.c + * @brief Data Builder. + */ + +#undef MPL_LOG_NDEBUG +#define MPL_LOG_NDEBUG 0 /* Use 0 to turn on MPL_LOGV output */ + +#include + +#include "ml_math_func.h" +#include "data_builder.h" +#include "mlmath.h" +#include "storage_manager.h" +#include "message_layer.h" +#include "results_holder.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL" + +typedef inv_error_t (*inv_process_cb_func)(struct inv_sensor_cal_t *data); + +struct process_t { + inv_process_cb_func func; + int priority; + int data_required; +}; + +struct inv_db_save_t { + /** Compass Bias in Chip Frame in Hardware units scaled by 2^16 */ + long compass_bias[3]; + /** Gyro Bias in Chip Frame in Hardware units scaled by 2^16 */ + long gyro_bias[3]; + /** Temperature when *gyro_bias was stored. */ + long gyro_temp; + /** Accel Bias in Chip Frame in Hardware units scaled by 2^16 */ + long accel_bias[3]; + /** Temperature when accel bias was stored. */ + long accel_temp; + long gyro_temp_slope[3]; + /** Sensor Accuracy */ + int gyro_accuracy; + int accel_accuracy; + int compass_accuracy; +}; + +struct inv_data_builder_t { + int num_cb; + struct process_t process[INV_MAX_DATA_CB]; + struct inv_db_save_t save; + int compass_disturbance; + int mode; +#ifdef INV_PLAYBACK_DBG + int debug_mode; + int last_mode; + FILE *file; +#endif +}; + +void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias); +static void inv_set_contiguous(void); + +static struct inv_data_builder_t inv_data_builder; +static struct inv_sensor_cal_t sensors; + +/** Change this key if the data being stored by this file changes */ +#define INV_DB_SAVE_KEY 53395 + +#ifdef INV_PLAYBACK_DBG + +/** Turn on data logging to allow playback of same scenario at a later time. +* @param[in] file File to write to, must be open. +*/ +void inv_turn_on_data_logging(FILE *file) +{ + MPL_LOGV("input data logging started\n"); + inv_data_builder.file = file; + inv_data_builder.debug_mode = RD_RECORD; +} + +/** Turn off data logging to allow playback of same scenario at a later time. +* File passed to inv_turn_on_data_logging() must be closed after calling this. +*/ +void inv_turn_off_data_logging() +{ + MPL_LOGV("input data logging stopped\n"); + inv_data_builder.debug_mode = RD_NO_DEBUG; + inv_data_builder.file = NULL; +} +#endif + +/** This function receives the data that was stored in non-volatile memory between power off */ +static inv_error_t inv_db_load_func(const unsigned char *data) +{ + memcpy(&inv_data_builder.save, data, sizeof(inv_data_builder.save)); + // copy in the saved accuracy in the actual sensors accuracy + sensors.gyro.accuracy = inv_data_builder.save.gyro_accuracy; + sensors.accel.accuracy = inv_data_builder.save.accel_accuracy; + sensors.compass.accuracy = inv_data_builder.save.compass_accuracy; + // TODO + if (sensors.compass.accuracy == 3) { + inv_set_compass_bias_found(1); + } + return INV_SUCCESS; +} + +/** This function returns the data to be stored in non-volatile memory between power off */ +static inv_error_t inv_db_save_func(unsigned char *data) +{ + memcpy(data, &inv_data_builder.save, sizeof(inv_data_builder.save)); + return INV_SUCCESS; +} + +/** Initialize the data builder +*/ +inv_error_t inv_init_data_builder(void) +{ + /* TODO: Hardcode temperature scale/offset here. */ + memset(&inv_data_builder, 0, sizeof(inv_data_builder)); + memset(&sensors, 0, sizeof(sensors)); + return inv_register_load_store(inv_db_load_func, inv_db_save_func, + sizeof(inv_data_builder.save), + INV_DB_SAVE_KEY); +} + +/** Gyro sensitivity. +* @return A scale factor to convert device units to degrees per second scaled by 2^16 +* such that degrees_per_second = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum rate * 2^15. +*/ +long inv_get_gyro_sensitivity() +{ + return sensors.gyro.sensitivity; +} + +/** Accel sensitivity. +* @return A scale factor to convert device units to g's scaled by 2^16 +* such that g_s = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum accel value in g's * 2^15. +*/ +long inv_get_accel_sensitivity(void) +{ + return sensors.accel.sensitivity; +} + +/** Compass sensitivity. +* @return A scale factor to convert device units to micro Tesla scaled by 2^16 +* such that uT = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum uT * 2^15. +*/ +long inv_get_compass_sensitivity(void) +{ + return sensors.compass.sensitivity; +} + +/** Sets orientation and sensitivity field for a sensor. +* @param[out] sensor Structure to apply settings to +* @param[in] orientation Orientation description of how part is mounted. +* @param[in] sensitivity A Scale factor to convert from hardware units to +* standard units (dps, uT, g). +*/ +void set_sensor_orientation_and_scale(struct inv_single_sensor_t *sensor, + int orientation, long sensitivity) +{ + sensor->sensitivity = sensitivity; + sensor->orientation = orientation; +} + +/** Sets the Orientation and Sensitivity of the gyro data. +* @param[in] orientation A scalar defining the transformation from chip mounting +* to the body frame. The function inv_orientation_matrix_to_scalar() +* can convert the transformation matrix to this scalar and describes the +* scalar in further detail. +* @param[in] sensitivity A scale factor to convert device units to degrees per second scaled by 2^16 +* such that degrees_per_second = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum rate * 2^15. +*/ +void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_G_ORIENT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); + fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); + } +#endif + set_sensor_orientation_and_scale(&sensors.gyro, orientation, + sensitivity); +} + +/** Set Gyro Sample rate in micro seconds. +* @param[in] sample_rate_us Set Gyro Sample rate in us +*/ +void inv_set_gyro_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_G_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.gyro.sample_rate_us = sample_rate_us; + sensors.gyro.sample_rate_ms = sample_rate_us / 1000; + if (sensors.gyro.bandwidth == 0) { + sensors.gyro.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +/** Set Accel Sample rate in micro seconds. +* @param[in] sample_rate_us Set Accel Sample rate in us +*/ +void inv_set_accel_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_A_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.accel.sample_rate_us = sample_rate_us; + sensors.accel.sample_rate_ms = sample_rate_us / 1000; + if (sensors.accel.bandwidth == 0) { + sensors.accel.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +/** Pick the smallest non-negative number. Priority to td1 on equal +* If both are negative, return the largest. +*/ +static int inv_pick_best_time_difference(long td1, long td2) +{ + if (td1 >= 0) { + if (td2 >= 0) { + if (td1 <= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } else { + // td1 + return 0; + } + } else if (td2 >= 0) { + // td2 + return 1; + } else { + // Both are negative + if (td1 >= td2) { + // td1 + return 0; + } else { + // td2 + return 1; + } + } +} + +/** Returns timestame based upon a raw sensor, and returns if that sample has a new piece of data. +*/ +static int inv_raw_sensor_timestamp(int sensor_number, inv_time_t *ts) +{ + int status = 0; + switch (sensor_number) { + case 0: // Quat + *ts = sensors.quat.timestamp; + if (inv_data_builder.mode & INV_QUAT_NEW) + if (sensors.quat.timestamp_prev != sensors.quat.timestamp) + status = 1; + return status; + case 1: // Gyro + *ts = sensors.gyro.timestamp; + if (inv_data_builder.mode & INV_GYRO_NEW) + if (sensors.gyro.timestamp_prev != sensors.gyro.timestamp) + status = 1; + return status; + case 2: // Accel + *ts = sensors.accel.timestamp; + if (inv_data_builder.mode & INV_ACCEL_NEW) + if (sensors.accel.timestamp_prev != sensors.accel.timestamp) + status = 1; + return status; + case 3: // Compass + *ts = sensors.compass.timestamp; + if (inv_data_builder.mode & INV_MAG_NEW) + if (sensors.compass.timestamp_prev != sensors.compass.timestamp) + status = 1; + return status; + default: + *ts = 0; + return 0; + } + return 0; +} + +/** Gets best timestamp and if there is a new piece of data for a 9-axis sensor combination. +* It does this by finding a raw sensor that has the closest sample rate that is at least as +* often desired. It also returns if that raw sensor has a new piece of data. +* Priority is Quaternion-6axis, Quaternion 3-axis, Gyro, Accel +* @return Returns 1, if the raw sensor being attached has new data, 0 otherwise. +*/ +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts) +{ + long td[2]; + int idx; + + if ((sensors.quat.status & (INV_QUAT_6AXIS | INV_SENSOR_ON)) == (INV_QUAT_6AXIS | INV_SENSOR_ON)) { + // Sensor number is 0 (Quat) + return inv_raw_sensor_timestamp(0, ts); + } else if ((sensors.accel.status & INV_SENSOR_ON) == 0) { + return 0; // Accel must be on or 6-axis quat must be on + } + + // At this point, we know accel is on. Check if 3-axis quat is on + td[0] = sample_rate_us - sensors.quat.sample_rate_us; + td[1] = sample_rate_us - sensors.accel.sample_rate_us; + if ((sensors.quat.status & (INV_QUAT_3AXIS | INV_SENSOR_ON)) == (INV_QUAT_3AXIS | INV_SENSOR_ON)) { + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; + // 0 = quat, 3=accel + return inv_raw_sensor_timestamp(idx, ts); + } + + // No Quaternion data from outside, Gyro must be on + if ((sensors.gyro.status & INV_SENSOR_ON) == 0) { + return 0; // Gyro must be on + } + + td[0] = sample_rate_us - sensors.gyro.sample_rate_us; + idx = inv_pick_best_time_difference(td[0], td[1]); + idx *= 2; // 0=gyro 2=accel + idx++; + // 1 = gyro, 3=accel + return inv_raw_sensor_timestamp(idx, ts); +} + +/** Set Compass Sample rate in micro seconds. +* @param[in] sample_rate_us Set Gyro Sample rate in micro seconds. +*/ +void inv_set_compass_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_C_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.compass.sample_rate_us = sample_rate_us; + sensors.compass.sample_rate_ms = sample_rate_us / 1000; + if (sensors.compass.bandwidth == 0) { + sensors.compass.bandwidth = (int)(1000000L / sample_rate_us); + } +} + +void inv_get_gyro_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.gyro.sample_rate_ms; +} + +void inv_get_accel_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.accel.sample_rate_ms; +} + +void inv_get_compass_sample_rate_ms(long *sample_rate_ms) +{ + *sample_rate_ms = sensors.compass.sample_rate_ms; +} + +/** Set Quat Sample rate in micro seconds. +* @param[in] sample_rate_us Set Quat Sample rate in us +*/ +void inv_set_quat_sample_rate(long sample_rate_us) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&sample_rate_us, sizeof(sample_rate_us), 1, inv_data_builder.file); + } +#endif + sensors.quat.sample_rate_us = sample_rate_us; + sensors.quat.sample_rate_ms = sample_rate_us / 1000; +} + +/** Set Gyro Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_gyro_bandwidth(int bandwidth_hz) +{ + sensors.gyro.bandwidth = bandwidth_hz; +} + +/** Set Accel Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_accel_bandwidth(int bandwidth_hz) +{ + sensors.accel.bandwidth = bandwidth_hz; +} + +/** Set Compass Bandwidth in Hz +* @param[in] bandwidth_hz Gyro bandwidth in Hz +*/ +void inv_set_compass_bandwidth(int bandwidth_hz) +{ + sensors.compass.bandwidth = bandwidth_hz; +} + +/** Helper function stating whether the compass is on or off. + * @return TRUE if compass if on, 0 if compass if off +*/ +int inv_get_compass_on() +{ + return (sensors.compass.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Helper function stating whether the gyro is on or off. + * @return TRUE if gyro if on, 0 if gyro if off +*/ +int inv_get_gyro_on() +{ + return (sensors.gyro.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Helper function stating whether the acceleromter is on or off. + * @return TRUE if accel if on, 0 if accel if off +*/ +int inv_get_accel_on() +{ + return (sensors.accel.status & INV_SENSOR_ON) == INV_SENSOR_ON; +} + +/** Get last timestamp across all 3 sensors that are on. +* This find out which timestamp has the largest value for sensors that are on. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_time_t inv_get_last_timestamp() +{ + inv_time_t timestamp = 0; + if (sensors.accel.status & INV_SENSOR_ON) { + timestamp = sensors.accel.timestamp; + } + if (sensors.gyro.status & INV_SENSOR_ON) { + if (timestamp < sensors.gyro.timestamp) { + timestamp = sensors.gyro.timestamp; + } + } + if (sensors.compass.status & INV_SENSOR_ON) { + if (timestamp < sensors.compass.timestamp) { + timestamp = sensors.compass.timestamp; + } + } + if (sensors.temp.status & INV_SENSOR_ON) { + if (timestamp < sensors.temp.timestamp) + timestamp = sensors.temp.timestamp; + } + return timestamp; +} + +/** Sets the orientation and sensitivity of the gyro data. +* @param[in] orientation A scalar defining the transformation from chip mounting +* to the body frame. The function inv_orientation_matrix_to_scalar() +* can convert the transformation matrix to this scalar and describes the +* scalar in further detail. +* @param[in] sensitivity A scale factor to convert device units to g's +* such that g's = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum g_value * 2^15. +*/ +void inv_set_accel_orientation_and_scale(int orientation, long sensitivity) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_A_ORIENT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); + fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); + } +#endif + set_sensor_orientation_and_scale(&sensors.accel, orientation, + sensitivity); +} + +/** Sets the Orientation and Sensitivity of the gyro data. +* @param[in] orientation A scalar defining the transformation from chip mounting +* to the body frame. The function inv_orientation_matrix_to_scalar() +* can convert the transformation matrix to this scalar and describes the +* scalar in further detail. +* @param[in] sensitivity A scale factor to convert device units to uT +* such that uT = device_units * sensitivity / 2^30. Typically +* it works out to be the maximum uT_value * 2^15. +*/ +void inv_set_compass_orientation_and_scale(int orientation, long sensitivity) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_C_ORIENT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&orientation, sizeof(orientation), 1, inv_data_builder.file); + fwrite(&sensitivity, sizeof(sensitivity), 1, inv_data_builder.file); + } +#endif + set_sensor_orientation_and_scale(&sensors.compass, orientation, sensitivity); +} + +void inv_matrix_vector_mult(const long *A, const long *x, long *y) +{ + y[0] = inv_q30_mult(A[0], x[0]) + inv_q30_mult(A[1], x[1]) + inv_q30_mult(A[2], x[2]); + y[1] = inv_q30_mult(A[3], x[0]) + inv_q30_mult(A[4], x[1]) + inv_q30_mult(A[5], x[2]); + y[2] = inv_q30_mult(A[6], x[0]) + inv_q30_mult(A[7], x[1]) + inv_q30_mult(A[8], x[2]); +} + +/** Takes raw data stored in the sensor, removes bias, and converts it to +* calibrated data in the body frame. Also store raw data for body frame. +* @param[in,out] sensor structure to modify +* @param[in] bias bias in the mounting frame, in hardware units scaled by +* 2^16. Length 3. +*/ +void inv_apply_calibration(struct inv_single_sensor_t *sensor, const long *bias) +{ + long raw32[3]; + + // Convert raw to calibrated + raw32[0] = (long)sensor->raw[0] << 15; + raw32[1] = (long)sensor->raw[1] << 15; + raw32[2] = (long)sensor->raw[2] << 15; + + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->raw_scaled); + + raw32[0] -= bias[0] >> 1; + raw32[1] -= bias[1] >> 1; + raw32[2] -= bias[2] >> 1; + + inv_convert_to_body_with_scale(sensor->orientation, sensor->sensitivity << 1, raw32, sensor->calibrated); + + sensor->status |= INV_CALIBRATED; +} + +/** Returns the current bias for the compass +* @param[out] bias Compass bias in hardware units scaled by 2^16. In mounting frame. +* Length 3. +*/ +void inv_get_compass_bias(long *bias) +{ + if (bias != NULL) { + memcpy(bias, inv_data_builder.save.compass_bias, sizeof(inv_data_builder.save.compass_bias)); + } +} + +void inv_set_compass_bias(const long *bias, int accuracy) +{ + if (memcmp(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias))) { + memcpy(inv_data_builder.save.compass_bias, bias, sizeof(inv_data_builder.save.compass_bias)); + inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); + } + sensors.compass.accuracy = accuracy; + inv_data_builder.save.compass_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_CB_EVENT, INV_MSG_NEW_CB_EVENT, 0); +} + +/** Set the state of a compass disturbance +* @param[in] dist 1=disturbance, 0=no disturbance +*/ +void inv_set_compass_disturbance(int dist) +{ + inv_data_builder.compass_disturbance = dist; +} + +int inv_get_compass_disturbance(void) { + return inv_data_builder.compass_disturbance; +} +/** Sets the accel bias. +* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame +* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. +*/ +void inv_set_accel_bias(const long *bias, int accuracy) +{ + if (bias) { + if (memcmp(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias))) { + memcpy(inv_data_builder.save.accel_bias, bias, sizeof(inv_data_builder.save.accel_bias)); + inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); + } + } + sensors.accel.accuracy = accuracy; + inv_data_builder.save.accel_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + +/** Sets the accel accuracy. +* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. +*/ +void inv_set_accel_accuracy(int accuracy) +{ + sensors.accel.accuracy = accuracy; + inv_data_builder.save.accel_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + +/** Sets the accel bias with control over which axis. +* @param[in] bias Accel bias, length 3. In HW units scaled by 2^16 in body frame +* @param[in] accuracy Accuracy rating from 0 to 3, with 3 being most accurate. +* @param[in] mask Mask to select axis to apply bias set. +*/ +void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask) +{ + if (bias) { + if (mask & 1){ + inv_data_builder.save.accel_bias[0] = bias[0]; + } + if (mask & 2){ + inv_data_builder.save.accel_bias[1] = bias[1]; + } + if (mask & 4){ + inv_data_builder.save.accel_bias[2] = bias[2]; + } + + inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); + } + sensors.accel.accuracy = accuracy; + inv_data_builder.save.accel_accuracy = accuracy; + inv_set_message(INV_MSG_NEW_AB_EVENT, INV_MSG_NEW_AB_EVENT, 0); +} + + +/** Sets the gyro bias +* @param[in] bias Gyro bias in hardware units scaled by 2^16. In chip mounting frame. +* Length 3. +* @param[in] accuracy Accuracy of bias. 0 = least accurate, 3 = most accurate. +*/ +void inv_set_gyro_bias(const long *bias, int accuracy) +{ + if (bias != NULL) { + if (memcmp(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias))) { + memcpy(inv_data_builder.save.gyro_bias, bias, sizeof(inv_data_builder.save.gyro_bias)); + inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); + } + } + sensors.gyro.accuracy = accuracy; + inv_data_builder.save.gyro_accuracy = accuracy; + + /* TODO: What should we do if there's no temperature data? */ + if (sensors.temp.calibrated[0]) + inv_data_builder.save.gyro_temp = sensors.temp.calibrated[0]; + else + /* Set to 27 deg C for now until we've got a better solution. */ + inv_data_builder.save.gyro_temp = 1769472L; + inv_set_message(INV_MSG_NEW_GB_EVENT, INV_MSG_NEW_GB_EVENT, 0); +} + +/* TODO: Add this information to inv_sensor_cal_t */ +/** + * Get the gyro biases and temperature record from MPL + * @param[in] bias + * Gyro bias in hardware units scaled by 2^16. + * In chip mounting frame. + * Length 3. + * @param[in] temp + * Tempearature in degrees C. + */ +void inv_get_gyro_bias(long *bias, long *temp) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save.gyro_bias, + sizeof(inv_data_builder.save.gyro_bias)); + if (temp != NULL) + temp[0] = inv_data_builder.save.gyro_temp; +} + +/** Get Accel Bias +* @param[out] bias Accel bias where +* @param[out] temp Temperature where 1 C = 2^16 +*/ +void inv_get_accel_bias(long *bias, long *temp) +{ + if (bias != NULL) + memcpy(bias, inv_data_builder.save.accel_bias, + sizeof(inv_data_builder.save.accel_bias)); + if (temp != NULL) + temp[0] = inv_data_builder.save.accel_temp; +} + +/** + * Record new accel data for use when inv_execute_on_data() is called + * @param[in] accel accel data. + * Length 3. + * Calibrated data is in m/s^2 scaled by 2^16 in body frame. + * Raw data is in device units in chip mounting frame. + * @param[in] status + * Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 + * being most accurate. + * The upper bit INV_CALIBRATED, is set if the data was calibrated + * outside MPL and it is not set if the data being passed is raw. + * Raw data should be in device units, typically in a 16-bit range. + * @param[in] timestamp + * Monotonic time stamp, for Android it's in nanoseconds. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_build_accel(const long *accel, int status, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_ACCEL; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(accel, sizeof(accel[0]), 3, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + if ((status & INV_CALIBRATED) == 0) { + sensors.accel.raw[0] = (short)accel[0]; + sensors.accel.raw[1] = (short)accel[1]; + sensors.accel.raw[2] = (short)accel[2]; + sensors.accel.status |= INV_RAW_DATA; + inv_apply_calibration(&sensors.accel, inv_data_builder.save.accel_bias); + } else { + sensors.accel.calibrated[0] = accel[0]; + sensors.accel.calibrated[1] = accel[1]; + sensors.accel.calibrated[2] = accel[2]; + sensors.accel.status |= INV_CALIBRATED; + sensors.accel.accuracy = status & 3; + inv_data_builder.save.accel_accuracy = status & 3; + } + sensors.accel.status |= INV_NEW_DATA | INV_SENSOR_ON; + sensors.accel.timestamp_prev = sensors.accel.timestamp; + sensors.accel.timestamp = timestamp; + + return INV_SUCCESS; +} + +/** Record new gyro data and calls inv_execute_on_data() if previous +* sample has not been processed. +* @param[in] gyro Data is in device units. Length 3. +* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. +* @param[out] executed Set to 1 if data processing was done. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_GYRO; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(gyro, sizeof(gyro[0]), 3, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + memcpy(sensors.gyro.raw, gyro, 3 * sizeof(short)); + sensors.gyro.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.gyro.timestamp_prev = sensors.gyro.timestamp; + sensors.gyro.timestamp = timestamp; + inv_apply_calibration(&sensors.gyro, inv_data_builder.save.gyro_bias); + + return INV_SUCCESS; +} + +/** Record new compass data for use when inv_execute_on_data() is called +* @param[in] compass Compass data, if it was calibrated outside MPL, the units are uT scaled by 2^16. +* Length 3. +* @param[in] status Lower 2 bits are the accuracy, with 0 being inaccurate, and 3 being most accurate. +* The upper bit INV_CALIBRATED, is set if the data was calibrated outside MPL and it is +* not set if the data being passed is raw. Raw data should be in device units, typically +* in a 16-bit range. +* @param[in] timestamp Monotonic time stamp, for Android it's in nanoseconds. +* @param[out] executed Set to 1 if data processing was done. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_build_compass(const long *compass, int status, + inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_COMPASS; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(compass, sizeof(compass[0]), 3, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + if ((status & INV_CALIBRATED) == 0) { + sensors.compass.raw[0] = (short)compass[0]; + sensors.compass.raw[1] = (short)compass[1]; + sensors.compass.raw[2] = (short)compass[2]; + inv_apply_calibration(&sensors.compass, inv_data_builder.save.compass_bias); + sensors.compass.status |= INV_RAW_DATA; + } else { + sensors.compass.calibrated[0] = compass[0]; + sensors.compass.calibrated[1] = compass[1]; + sensors.compass.calibrated[2] = compass[2]; + sensors.compass.status |= INV_CALIBRATED; + sensors.compass.accuracy = status & 3; + inv_data_builder.save.compass_accuracy = status & 3; + } + sensors.compass.timestamp_prev = sensors.compass.timestamp; + sensors.compass.timestamp = timestamp; + sensors.compass.status |= INV_NEW_DATA | INV_SENSOR_ON; + + return INV_SUCCESS; +} + +/** Record new temperature data for use when inv_execute_on_data() is called. + * @param[in] temp Temperature data in q16 format. + * @param[in] timestamp Monotonic time stamp; for Android it's in + * nanoseconds. +* @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_build_temp(const long temp, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_TEMPERATURE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(&temp, sizeof(temp), 1, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + sensors.temp.calibrated[0] = temp; + sensors.temp.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.temp.timestamp_prev = sensors.temp.timestamp; + sensors.temp.timestamp = timestamp; + /* TODO: Apply scale, remove offset. */ + + return INV_SUCCESS; +} +/** quaternion data +* @param[in] quat Quaternion data. 2^30 = 1.0 or 2^14=1 for 16-bit data. +* Real part first. Length 4. +* @param[in] status number of axis, 16-bit or 32-bit +* @param[in] timestamp +* @param[in] timestamp Monotonic time stamp; for Android it's in +* nanoseconds. +* @param[out] executed Set to 1 if data processing was done. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp) +{ +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_QUAT; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + fwrite(quat, sizeof(quat[0]), 4, inv_data_builder.file); + fwrite(×tamp, sizeof(timestamp), 1, inv_data_builder.file); + } +#endif + + memcpy(sensors.quat.raw, quat, sizeof(sensors.quat.raw)); + sensors.quat.timestamp = timestamp; + sensors.quat.status |= INV_NEW_DATA | INV_RAW_DATA | INV_SENSOR_ON; + sensors.quat.status |= (INV_BIAS_APPLIED & status); + + return INV_SUCCESS; +} + +/** This should be called when the accel has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_accel_was_turned_off() +{ + sensors.accel.status = 0; +} + +/** This should be called when the compass has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_compass_was_turned_off() +{ + sensors.compass.status = 0; +} + +/** This should be called when the quaternion data from the DMP has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_quaternion_sensor_was_turned_off(void) +{ + sensors.quat.status = 0; +} + +/** This should be called when the gyro has been turned off. This is so +* that we will know if the data is contiguous. +*/ +void inv_gyro_was_turned_off() +{ + sensors.gyro.status = 0; +} + +/** This should be called when the temperature sensor has been turned off. + * This is so that we will know if the data is contiguous. + */ +void inv_temperature_was_turned_off() +{ + sensors.temp.status = 0; +} + +/** Registers to receive a callback when there is new sensor data. +* @internal +* @param[in] func Function pointer to receive callback when there is new sensor data +* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority +* numbers must be unique. +* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be +* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = +* gyro data, INV_MAG_NEW = compass data. So passing in +* INV_ACCEL_NEW | INV_MAG_NEW, a +* callback would be generated if there was new magnetomer data OR new accel data. +*/ +inv_error_t inv_register_data_cb( + inv_error_t (*func)(struct inv_sensor_cal_t *data), + int priority, int sensor_type) +{ + inv_error_t result = INV_SUCCESS; + int kk, nn; + + // Make sure we haven't registered this function already + // Or used the same priority + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if ((inv_data_builder.process[kk].func == func) || + (inv_data_builder.process[kk].priority == priority)) { + return INV_ERROR_INVALID_PARAMETER; //fixme give a warning + } + } + + // Make sure we have not filled up our number of allowable callbacks + if (inv_data_builder.num_cb <= INV_MAX_DATA_CB - 1) { + kk = 0; + if (inv_data_builder.num_cb != 0) { + // set kk to be where this new callback goes in the array + while ((kk < inv_data_builder.num_cb) && + (inv_data_builder.process[kk].priority < priority)) { + kk++; + } + if (kk != inv_data_builder.num_cb) { + // We need to move the others + for (nn = inv_data_builder.num_cb; nn > kk; --nn) { + inv_data_builder.process[nn] = + inv_data_builder.process[nn - 1]; + } + } + } + // Add new callback + inv_data_builder.process[kk].func = func; + inv_data_builder.process[kk].priority = priority; + inv_data_builder.process[kk].data_required = sensor_type; + inv_data_builder.num_cb++; + } else { + MPL_LOGE("Unable to add feature callback as too many were already registered\n"); + result = INV_ERROR_MEMORY_EXAUSTED; + } + + return result; +} + +/** Unregisters the callback that happens when new sensor data is received. +* @internal +* @param[in] func Function pointer to receive callback when there is new sensor data +* @param[in] priority Lower priority numbers receive a callback before larger numbers. All priority +* numbers must be unique. +* @param[in] sensor_type Sets the type of data that triggers the callback. Must be non-zero. May be +* a combination. INV_ACCEL_NEW = accel data, INV_GYRO_NEW = +* gyro data, INV_MAG_NEW = compass data. So passing in +* INV_ACCEL_NEW | INV_MAG_NEW, a +* callback would be generated if there was new magnetomer data OR new accel data. +*/ +inv_error_t inv_unregister_data_cb( + inv_error_t (*func)(struct inv_sensor_cal_t *data)) +{ + int kk, nn; + + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if (inv_data_builder.process[kk].func == func) { + // Delete this callback + for (nn = kk + 1; nn < inv_data_builder.num_cb; ++nn) { + inv_data_builder.process[nn - 1] = + inv_data_builder.process[nn]; + } + inv_data_builder.num_cb--; + return INV_SUCCESS; + } + } + + return INV_SUCCESS; // We did not find the callback +} + +/** After at least one of inv_build_gyro(), inv_build_accel(), or +* inv_build_compass() has been called, this function should be called. +* It will process the data it has received and update all the internal states +* and features that have been turned on. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_execute_on_data(void) +{ + inv_error_t result, first_error; + int kk; + +#ifdef INV_PLAYBACK_DBG + if (inv_data_builder.debug_mode == RD_RECORD) { + int type = PLAYBACK_DBG_TYPE_EXECUTE; + fwrite(&type, sizeof(type), 1, inv_data_builder.file); + } +#endif + // Determine what new data we have + inv_data_builder.mode = 0; + if (sensors.gyro.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_GYRO_NEW; + if (sensors.accel.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_ACCEL_NEW; + if (sensors.compass.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_MAG_NEW; + if (sensors.temp.status & INV_NEW_DATA) + inv_data_builder.mode |= INV_TEMP_NEW; + if (sensors.quat.status & INV_QUAT_NEW) + inv_data_builder.mode |= INV_QUAT_NEW; + + first_error = INV_SUCCESS; + + for (kk = 0; kk < inv_data_builder.num_cb; ++kk) { + if (inv_data_builder.mode & inv_data_builder.process[kk].data_required) { + result = inv_data_builder.process[kk].func(&sensors); + if (result && !first_error) { + first_error = result; + } + } + } + + inv_set_contiguous(); + + return first_error; +} + +/** Cleans up status bits after running all the callbacks. It sets the contiguous flag. +* +*/ +static void inv_set_contiguous(void) +{ + inv_time_t current_time = 0; + if (sensors.gyro.status & INV_NEW_DATA) { + sensors.gyro.status |= INV_CONTIGUOUS; + current_time = sensors.gyro.timestamp; + } + if (sensors.accel.status & INV_NEW_DATA) { + sensors.accel.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.accel.timestamp); + } + if (sensors.compass.status & INV_NEW_DATA) { + sensors.compass.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.compass.timestamp); + } + if (sensors.temp.status & INV_NEW_DATA) { + sensors.temp.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.temp.timestamp); + } + if (sensors.quat.status & INV_NEW_DATA) { + sensors.quat.status |= INV_CONTIGUOUS; + current_time = MAX(current_time, sensors.quat.timestamp); + } + +#if 0 + /* See if sensors are still on. These should be turned off by inv_*_was_turned_off() + * type functions. This is just in case that breaks down. We make sure + * all the data is within 2 seconds of the newest piece of data*/ + if (inv_delta_time_ms(current_time, sensors.gyro.timestamp) >= 2000) + inv_gyro_was_turned_off(); + if (inv_delta_time_ms(current_time, sensors.accel.timestamp) >= 2000) + inv_accel_was_turned_off(); + if (inv_delta_time_ms(current_time, sensors.compass.timestamp) >= 2000) + inv_compass_was_turned_off(); + /* TODO: Temperature might not need to be read this quickly. */ + if (inv_delta_time_ms(current_time, sensors.temp.timestamp) >= 2000) + inv_temperature_was_turned_off(); +#endif + + /* clear bits */ + sensors.gyro.status &= ~INV_NEW_DATA; + sensors.accel.status &= ~INV_NEW_DATA; + sensors.compass.status &= ~INV_NEW_DATA; + sensors.temp.status &= ~INV_NEW_DATA; + sensors.quat.status &= ~INV_NEW_DATA; +} + +/** Gets a whole set of accel data including data, accuracy and timestamp. + * @param[out] data Accel Data where 1g = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + if (data != NULL) { + memcpy(data, sensors.accel.calibrated, sizeof(sensors.accel.calibrated)); + } + if (timestamp != NULL) { + *timestamp = sensors.accel.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.accel.accuracy; + } +} + +/** Gets a whole set of gyro data including data, accuracy and timestamp. + * @param[out] data Gyro Data where 1 dps = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); + if (timestamp != NULL) { + *timestamp = sensors.gyro.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.gyro.accuracy; + } +} + +/** Gets a whole set of gyro raw data including data, accuracy and timestamp. + * @param[out] data Gyro Data where 1 dps = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.gyro.raw_scaled, sizeof(sensors.gyro.raw_scaled)); + if (timestamp != NULL) { + *timestamp = sensors.gyro.timestamp; + } + if (accuracy != NULL) { + *accuracy = sensors.gyro.accuracy; + } +} + +/** Get's latest gyro data. +* @param[out] gyro Gyro Data, Length 3. 1 dps = 2^16. +*/ +void inv_get_gyro(long *gyro) +{ + memcpy(gyro, sensors.gyro.calibrated, sizeof(sensors.gyro.calibrated)); +} + +/** Gets a whole set of compass data including data, accuracy and timestamp. + * @param[out] data Compass Data where 1 uT = 2^16 + * @param[out] accuracy Accuracy 0 being not accurate, and 3 being most accurate. + * @param[out] timestamp The timestamp of the data sample. +*/ +void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t *timestamp) +{ + memcpy(data, sensors.compass.calibrated, sizeof(sensors.compass.calibrated)); + if (timestamp != NULL) { + *timestamp = sensors.compass.timestamp; + } + if (accuracy != NULL) { + if (inv_data_builder.compass_disturbance) + *accuracy = 0; + else + *accuracy = sensors.compass.accuracy; + } +} + +/** Gets a whole set of temperature data including data, accuracy and timestamp. + * @param[out] data Temperature data where 1 degree C = 2^16 + * @param[out] accuracy 0 to 3, where 3 is most accurate. + * @param[out] timestamp The timestamp of the data sample. + */ +void inv_get_temp_set(long *data, int *accuracy, inv_time_t *timestamp) +{ + data[0] = sensors.temp.calibrated[0]; + if (timestamp) + *timestamp = sensors.temp.timestamp; + if (accuracy) + *accuracy = sensors.temp.accuracy; +} + +/** Returns accuracy of gyro. + * @return Accuracy of gyro with 0 being not accurate, and 3 being most accurate. +*/ +int inv_get_gyro_accuracy(void) +{ + return sensors.gyro.accuracy; +} + +/** Returns accuracy of compass. + * @return Accuracy of compass with 0 being not accurate, and 3 being most accurate. +*/ +int inv_get_mag_accuracy(void) +{ + if (inv_data_builder.compass_disturbance) + return 0; + return sensors.compass.accuracy; +} + +/** Returns accuracy of accel. + * @return Accuracy of accel with 0 being not accurate, and 3 being most accurate. +*/ +int inv_get_accel_accuracy(void) +{ + return sensors.accel.accuracy; +} + +inv_error_t inv_get_gyro_orient(int *orient) +{ + *orient = sensors.gyro.orientation; + return 0; +} + +inv_error_t inv_get_accel_orient(int *orient) +{ + *orient = sensors.accel.orientation; + return 0; +} + + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/data_builder.h b/60xx/libsensors_iio/software/core/mllite/data_builder.h new file mode 100644 index 0000000..c8c18cf --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/data_builder.h @@ -0,0 +1,240 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_DATA_BUILDER_H__ +#define INV_DATA_BUILDER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +// Uncomment this flag to enable playback debug and record or playback scenarios +//#define INV_PLAYBACK_DBG + +/** This is a new sample of accel data */ +#define INV_ACCEL_NEW 1 +/** This is a new sample of gyro data */ +#define INV_GYRO_NEW 2 +/** This is a new sample of compass data */ +#define INV_MAG_NEW 4 +/** This is a new sample of temperature data */ +#define INV_TEMP_NEW 8 +/** This is a new sample of quaternion data */ +#define INV_QUAT_NEW 16 +/** Set if quaternion is 6-axis from DMP */ +#define INV_QUAT_6AXIS 1024 +/** Set if quaternion is 3-axis from DMP */ +#define INV_QUAT_3AXIS 4096 + +/** Set if the data is contiguous. Typically not set if a sample was skipped */ +#define INV_CONTIGUOUS 16 +/** Set if the calibrated data has been solved for */ +#define INV_CALIBRATED 32 +/* INV_NEW_DATA set for a new set of data, cleared if not available. */ +#define INV_NEW_DATA 64 +/* Set if raw data exists */ +#define INV_RAW_DATA 128 +/* Set if the sensor is on */ +#define INV_SENSOR_ON 256 +/* Set if quaternion has bias correction applied */ +#define INV_BIAS_APPLIED 512 + +#define INV_PRIORITY_MOTION_NO_MOTION 100 +#define INV_PRIORITY_GYRO_TC 150 +#define INV_PRIORITY_QUATERNION_GYRO_ACCEL 200 +#define INV_PRIORITY_QUATERNION_NO_GYRO 250 +#define INV_PRIORITY_MAGNETIC_DISTURBANCE 300 +#define INV_PRIORITY_HEADING_FROM_GYRO 350 +#define INV_PRIORITY_COMPASS_BIAS_W_GYRO 375 +#define INV_PRIORITY_COMPASS_VECTOR_CAL 400 +#define INV_PRIORITY_COMPASS_ADV_BIAS 500 +#define INV_PRIORITY_9_AXIS_FUSION 600 +#define INV_PRIORITY_QUATERNION_ADJUST_9_AXIS 700 +#define INV_PRIORITY_QUATERNION_ACCURACY 750 +#define INV_PRIORITY_RESULTS_HOLDER 800 +#define INV_PRIORITY_INUSE_AUTO_CALIBRATION 850 +#define INV_PRIORITY_HAL_OUTPUTS 900 +#define INV_PRIORITY_GLYPH 950 +#define INV_PRIORITY_SHAKE 975 +#define INV_PRIORITY_SM 1000 + +struct inv_single_sensor_t { + /** Orientation Descriptor. Describes how to go from the mounting frame to the body frame when + * the rotation matrix could be thought of only having elements of 0,1,-1. + * 2 bits are used to describe the column of the 1 or -1 and the 3rd bit is used for the sign. + * Bit 8 is sign of +/- 1 in third row. Bit 6-7 is column of +/-1 in third row. + * Bit 5 is sign of +/- 1 in second row. Bit 3-4 is column of +/-1 in second row. + * Bit 2 is sign of +/- 1 in first row. Bit 0-1 is column of +/-1 in first row. + */ + int orientation; + /** The raw data in raw data units in the mounting frame */ + short raw[3]; + /** Raw data in body frame */ + long raw_scaled[3]; + /** Calibrated data */ + long calibrated[3]; + long sensitivity; + /** Sample rate in microseconds */ + long sample_rate_us; + long sample_rate_ms; + /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample + * skipped due to power savings turning off this sensor. + * INV_NEW_DATA set for a new set of data, cleared if not available. + * INV_CALIBRATED_SET if calibrated data has been solved for */ + int status; + /** 0 to 3 for how well sensor data and biases are known. 3 is most accurate. */ + int accuracy; + inv_time_t timestamp; + inv_time_t timestamp_prev; + /** Bandwidth in Hz */ + int bandwidth; +}; +struct inv_quat_sensor_t { + long raw[4]; + /** INV_CONTIGUOUS is set for contiguous data. Will not be set if there was a sample + * skipped due to power savings turning off this sensor. + * INV_NEW_DATA set for a new set of data, cleared if not available. + * INV_CALIBRATED_SET if calibrated data has been solved for */ + int status; + inv_time_t timestamp; + inv_time_t timestamp_prev; + long sample_rate_us; + long sample_rate_ms; +}; + +struct inv_sensor_cal_t { + struct inv_single_sensor_t gyro; + struct inv_single_sensor_t accel; + struct inv_single_sensor_t compass; + struct inv_single_sensor_t temp; + struct inv_quat_sensor_t quat; + /** Combinations of INV_GYRO_NEW, INV_ACCEL_NEW, INV_MAG_NEW to indicate + * which data is a new sample as these data points may have different sample rates. + */ + int status; +}; + +// Useful for debug record and playback +typedef enum { + RD_NO_DEBUG, + RD_RECORD, + RD_PLAYBACK +} rd_dbg_mode; + +typedef enum { + PLAYBACK_DBG_TYPE_GYRO, + PLAYBACK_DBG_TYPE_ACCEL, + PLAYBACK_DBG_TYPE_COMPASS, + PLAYBACK_DBG_TYPE_TEMPERATURE, + PLAYBACK_DBG_TYPE_EXECUTE, + PLAYBACK_DBG_TYPE_A_ORIENT, + PLAYBACK_DBG_TYPE_G_ORIENT, + PLAYBACK_DBG_TYPE_C_ORIENT, + PLAYBACK_DBG_TYPE_A_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_C_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_G_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_GYRO_OFF, + PLAYBACK_DBG_TYPE_ACCEL_OFF, + PLAYBACK_DBG_TYPE_COMPASS_OFF, + PLAYBACK_DBG_TYPE_Q_SAMPLE_RATE, + PLAYBACK_DBG_TYPE_QUAT + +} inv_rd_dbg_states; + +/** Maximum number of data callbacks that are supported. Safe to increase if needed.*/ +#define INV_MAX_DATA_CB 20 + +#ifdef INV_PLAYBACK_DBG +#include +void inv_turn_on_data_logging(FILE *file); +void inv_turn_off_data_logging(); +#endif + +void inv_set_gyro_orientation_and_scale(int orientation, long sensitivity); +void inv_set_accel_orientation_and_scale(int orientation, + long sensitivity); +void inv_set_compass_orientation_and_scale(int orientation, + long sensitivity); +void inv_set_gyro_sample_rate(long sample_rate_us); +void inv_set_compass_sample_rate(long sample_rate_us); +void inv_set_quat_sample_rate(long sample_rate_us); +void inv_set_accel_sample_rate(long sample_rate_us); +void inv_set_gyro_bandwidth(int bandwidth_hz); +void inv_set_accel_bandwidth(int bandwidth_hz); +void inv_set_compass_bandwidth(int bandwidth_hz); + +void inv_get_gyro_sample_rate_ms(long *sample_rate_ms); +void inv_get_accel_sample_rate_ms(long *sample_rate_ms); +void inv_get_compass_sample_rate_ms(long *sample_rate_ms); + +inv_error_t inv_register_data_cb(inv_error_t (*func) + (struct inv_sensor_cal_t * data), int priority, + int sensor_type); +inv_error_t inv_unregister_data_cb(inv_error_t (*func) + (struct inv_sensor_cal_t * data)); + +inv_error_t inv_build_gyro(const short *gyro, inv_time_t timestamp); +inv_error_t inv_build_compass(const long *compass, int status, + inv_time_t timestamp); +inv_error_t inv_build_accel(const long *accel, int status, + inv_time_t timestamp); +inv_error_t inv_build_temp(const long temp, inv_time_t timestamp); +inv_error_t inv_build_quat(const long *quat, int status, inv_time_t timestamp); +inv_error_t inv_execute_on_data(void); + +void inv_get_compass_bias(long *bias); + +void inv_set_compass_bias(const long *bias, int accuracy); +void inv_set_compass_disturbance(int dist); +void inv_set_gyro_bias(const long *bias, int accuracy); +void inv_set_accel_bias(const long *bias, int accuracy); +void inv_set_accel_accuracy(int accuracy); +void inv_set_accel_bias_mask(const long *bias, int accuracy, int mask); + +void inv_get_gyro_bias(long *bias, long *temp); +void inv_get_accel_bias(long *bias, long *temp); + +void inv_gyro_was_turned_off(void); +void inv_accel_was_turned_off(void); +void inv_compass_was_turned_off(void); +void inv_quaternion_sensor_was_turned_off(void); +inv_error_t inv_init_data_builder(void); +long inv_get_gyro_sensitivity(void); +long inv_get_accel_sensitivity(void); +long inv_get_compass_sensitivity(void); + +void inv_get_accel_set(long *data, int8_t *accuracy, inv_time_t * timestamp); +void inv_get_gyro_set(long *data, int8_t *accuracy, inv_time_t * timestamp); +void inv_get_gyro_set_raw(long *data, int8_t *accuracy, inv_time_t * timestamp); +void inv_get_compass_set(long *data, int8_t *accuracy, inv_time_t * timestamp); + +void inv_get_gyro(long *gyro); + +int inv_get_gyro_accuracy(void); +int inv_get_accel_accuracy(void); +int inv_get_mag_accuracy(void); + +int inv_get_compass_on(void); +int inv_get_gyro_on(void); +int inv_get_accel_on(void); + +inv_time_t inv_get_last_timestamp(void); +int inv_get_compass_disturbance(void); + +//New DMP Cal Functions +inv_error_t inv_get_gyro_orient(int *orient); +inv_error_t inv_get_accel_orient(int *orient); + +// internal +int inv_get_6_axis_gyro_accel_timestamp(long sample_rate_us, inv_time_t *ts); + +#ifdef __cplusplus +} +#endif + +#endif /* INV_DATA_BUILDER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.c b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c new file mode 100644 index 0000000..caa1db7 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.c @@ -0,0 +1,497 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/** + * @defgroup HAL_Outputs hal_outputs + * @brief Motion Library - HAL Outputs + * Sets up common outputs for HAL + * + * @{ + * @file hal_outputs.c + * @brief HAL Outputs. + */ + +#include + +#include "hal_outputs.h" +#include "log.h" +#include "ml_math_func.h" +#include "mlmath.h" +#include "start_manager.h" +#include "data_builder.h" +#include "results_holder.h" + +struct hal_output_t { + int accuracy_mag; /**< Compass accuracy */ +// int accuracy_gyro; /**< Gyro Accuracy */ +// int accuracy_accel; /**< Accel Accuracy */ + int accuracy_quat; /**< quat Accuracy */ + + inv_time_t nav_timestamp; + inv_time_t gam_timestamp; +// inv_time_t accel_timestamp; + inv_time_t mag_timestamp; + long nav_quat[4]; + int gyro_status; + int accel_status; + int compass_status; + int nine_axis_status; + inv_biquad_filter_t lp_filter[3]; + float compass_float[3]; + long linear_acceleration_sample_rate_us; + long gravity_sample_rate_us; +}; + +static struct hal_output_t hal_out; + +void inv_set_linear_acceleration_sample_rate(long sample_rate_us) +{ + hal_out.linear_acceleration_sample_rate_us = sample_rate_us; +} + +void inv_set_gravity_sample_rate(long sample_rate_us) +{ + hal_out.gravity_sample_rate_us = sample_rate_us; +} + +/** Acceleration (m/s^2) in body frame. +* @param[out] values Acceleration in m/s^2 includes gravity. So while not in motion, it +* should return a vector of magnitude near 9.81 m/s^2 +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to +* inv_build_accel(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + int status; + /* Converts fixed point to m/s^2. Fixed point has 1g = 2^16. + * So this 9.80665 / 2^16 */ +#define ACCEL_CONVERSION 0.000149637603759766f + long accel[3]; + inv_get_accel_set(accel, accuracy, timestamp); + values[0] = accel[0] * ACCEL_CONVERSION; + values[1] = accel[1] * ACCEL_CONVERSION; + values[2] = accel[2] * ACCEL_CONVERSION; + if (hal_out.accel_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} + +/** Linear Acceleration (m/s^2) in Body Frame. +* @param[out] values Linear Acceleration in body frame, length 3, (m/s^2). May show +* accel biases while at rest. +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to +* inv_build_accel(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_linear_acceleration(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long gravity[3], accel[3]; + inv_time_t timestamp1; + + inv_get_accel_set(accel, accuracy, ×tamp1); + inv_get_gravity(gravity); + accel[0] -= gravity[0] >> 14; + accel[1] -= gravity[1] >> 14; + accel[2] -= gravity[2] >> 14; + values[0] = accel[0] * ACCEL_CONVERSION; + values[1] = accel[1] * ACCEL_CONVERSION; + values[2] = accel[2] * ACCEL_CONVERSION; + + return inv_get_6_axis_gyro_accel_timestamp(hal_out.linear_acceleration_sample_rate_us, timestamp); +} + +/** Gravity vector (m/s^2) in Body Frame. +* @param[out] values Gravity vector in body frame, length 3, (m/s^2) +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to +* inv_build_accel(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long gravity[3]; + + *accuracy = (int8_t) hal_out.accuracy_quat; + inv_get_gravity(gravity); + values[0] = (gravity[0] >> 14) * ACCEL_CONVERSION; + values[1] = (gravity[1] >> 14) * ACCEL_CONVERSION; + values[2] = (gravity[2] >> 14) * ACCEL_CONVERSION; + return inv_get_6_axis_gyro_accel_timestamp(hal_out.gravity_sample_rate_us, timestamp); +} + +/* Converts fixed point to rad/sec. Fixed point has 1 dps = 2^16. + * So this is: pi / 2^16 / 180 */ +#define GYRO_CONVERSION 2.66316109007924e-007f + +/** Gyroscope calibrated data (rad/s) in body frame. +* @param[out] values Rotation Rate in rad/sec. +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to +* inv_build_gyro(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long gyro[3]; + int status; + + inv_get_gyro_set(gyro, accuracy, timestamp); + values[0] = gyro[0] * GYRO_CONVERSION; + values[1] = gyro[1] * GYRO_CONVERSION; + values[2] = gyro[2] * GYRO_CONVERSION; + if (hal_out.gyro_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} + +/** Gyroscope raw data (rad/s) in body frame. +* @param[out] values Rotation Rate in rad/sec. +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. Derived from the timestamp sent to +* inv_build_gyro(). +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + long gyro[3]; + int status; + + inv_get_gyro_set_raw(gyro, accuracy, timestamp); + values[0] = gyro[0] * GYRO_CONVERSION; + values[1] = gyro[1] * GYRO_CONVERSION; + values[2] = gyro[2] * GYRO_CONVERSION; + if (hal_out.gyro_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} + +/** +* This corresponds to Sensor.TYPE_ROTATION_VECTOR. +* The rotation vector represents the orientation of the device as a combination +* of an angle and an axis, in which the device has rotated through an angle @f$\theta@f$ +* around an axis {x, y, z}.
+* The three elements of the rotation vector are +* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)}, such that the magnitude of the rotation +* vector is equal to sin(@f$\theta@f$/2), and the direction of the rotation vector is +* equal to the direction of the axis of rotation. +* +* The three elements of the rotation vector are equal to the last three components of a unit quaternion +* {x*sin(@f$\theta@f$/2), y*sin(@f$\theta@f$/2), z*sin(@f$\theta@f$/2)>. The 4th element is cos(@f$\theta@f$/2). +* +* Elements of the rotation vector are unitless. The x,y and z axis are defined in the same way as the acceleration sensor. +* The reference coordinate system is defined as a direct orthonormal basis, where: + + -X is defined as the vector product Y.Z (It is tangential to the ground at the device's current location and roughly points East). + -Y is tangential to the ground at the device's current location and points towards the magnetic North Pole. + -Z points towards the sky and is perpendicular to the ground. +* @param[out] values Length 4. +* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate +* @param[out] timestamp Timestamp. In (ns) for Android. +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + *accuracy = (int8_t) hal_out.accuracy_quat; + *timestamp = hal_out.nav_timestamp; + + if (hal_out.nav_quat[0] >= 0) { + values[0] = hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; + values[1] = hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; + values[2] = hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; + values[3] = hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; + } else { + values[0] = -hal_out.nav_quat[1] * INV_TWO_POWER_NEG_30; + values[1] = -hal_out.nav_quat[2] * INV_TWO_POWER_NEG_30; + values[2] = -hal_out.nav_quat[3] * INV_TWO_POWER_NEG_30; + values[3] = -hal_out.nav_quat[0] * INV_TWO_POWER_NEG_30; + } + values[4] = inv_get_heading_confidence_interval(); + + return hal_out.nine_axis_status; +} + + +/** Compass data (uT) in body frame. +* @param[out] values Compass data in (uT), length 3. May be calibrated by having +* biases removed and sensitivity adjusted +* @param[out] accuracy Accuracy 0 to 3, 3 = most accurate +* @param[out] timestamp Timestamp. In (ns) for Android. +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + int status; + /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. + * So this is: 1 / 2^16*/ +//#define COMPASS_CONVERSION 1.52587890625e-005f + int i; + + *timestamp = hal_out.mag_timestamp; + *accuracy = (int8_t) hal_out.accuracy_mag; + + for (i=0; i<3; i++) { + values[i] = hal_out.compass_float[i]; + } + if (hal_out.compass_status & INV_NEW_DATA) + status = 1; + else + status = 0; + return status; +} + +static void inv_get_rotation(float r[3][3]) +{ + long rot[9]; + float conv = 1.f / (1L<<30); + + inv_quaternion_to_rotation(hal_out.nav_quat, rot); + r[0][0] = rot[0]*conv; + r[0][1] = rot[1]*conv; + r[0][2] = rot[2]*conv; + r[1][0] = rot[3]*conv; + r[1][1] = rot[4]*conv; + r[1][2] = rot[5]*conv; + r[2][0] = rot[6]*conv; + r[2][1] = rot[7]*conv; + r[2][2] = rot[8]*conv; +} + +static void google_orientation(float *g) +{ + float rad2deg = (float)(180.0 / M_PI); + float R[3][3]; + + inv_get_rotation(R); + + g[0] = atan2f(-R[1][0], R[0][0]) * rad2deg; + g[1] = atan2f(-R[2][1], R[2][2]) * rad2deg; + g[2] = asinf ( R[2][0]) * rad2deg; + if (g[0] < 0) + g[0] += 360; +} + + +/** This corresponds to Sensor.TYPE_ORIENTATION. All values are angles in degrees. +* @param[out] values Length 3, Degrees.
+* - values[0]: Azimuth, angle between the magnetic north direction +* and the y-axis, around the z-axis (0 to 359). 0=North, 90=East, 180=South, 270=West
+* - values[1]: Pitch, rotation around x-axis (-180 to 180), with positive values +* when the z-axis moves toward the y-axis.
+* - values[2]: Roll, rotation around y-axis (-90 to 90), with positive +* values when the x-axis moves toward the z-axis.
+* +* @note This definition is different from yaw, pitch and roll used in aviation +* where the X axis is along the long side of the plane (tail to nose). +* Note: This sensor type exists for legacy reasons, please use getRotationMatrix() +* in conjunction with remapCoordinateSystem() and getOrientation() to compute +* these values instead. +* Important note: For historical reasons the roll angle is positive in the +* clockwise direction (mathematically speaking, it should be positive in +* the counter-clockwise direction). +* @param[out] accuracy Accuracy of the measurment, 0 is least accurate, while 3 is most accurate. +* @param[out] timestamp The timestamp for this sensor. +* @return Returns 1 if the data was updated or 0 if it was not updated. +*/ +int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, + inv_time_t * timestamp) +{ + *accuracy = (int8_t) hal_out.accuracy_quat; + *timestamp = hal_out.nav_timestamp; + + google_orientation(values); + + return hal_out.nine_axis_status; +} + +/** Main callback to generate HAL outputs. Typically not called by library users. +* @param[in] sensor_cal Input variable to take sensor data whenever there is new +* sensor data. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_generate_hal_outputs(struct inv_sensor_cal_t *sensor_cal) +{ + int use_sensor = 0; + long sr = 1000; + long compass[3]; + int8_t accuracy; + int i; + (void) sensor_cal; + + inv_get_quaternion_set(hal_out.nav_quat, &hal_out.accuracy_quat, + &hal_out.nav_timestamp); + hal_out.gyro_status = sensor_cal->gyro.status; + hal_out.accel_status = sensor_cal->accel.status; + hal_out.compass_status = sensor_cal->compass.status; + + // Find the highest sample rate and tie generating 9-axis to that one. + if (sensor_cal->gyro.status & INV_SENSOR_ON) { + sr = sensor_cal->gyro.sample_rate_ms; + use_sensor = 0; + } + if ((sensor_cal->accel.status & INV_SENSOR_ON) && (sr > sensor_cal->accel.sample_rate_ms)) { + sr = sensor_cal->accel.sample_rate_ms; + use_sensor = 1; + } + if ((sensor_cal->compass.status & INV_SENSOR_ON) && (sr > sensor_cal->compass.sample_rate_ms)) { + sr = sensor_cal->compass.sample_rate_ms; + use_sensor = 2; + } + if ((sensor_cal->quat.status & INV_SENSOR_ON) && (sr > sensor_cal->quat.sample_rate_ms)) { + sr = sensor_cal->quat.sample_rate_ms; + use_sensor = 3; + } + + // Only output 9-axis if all 9 sensors are on. + if (sensor_cal->quat.status & INV_SENSOR_ON) { + // If quaternion sensor is on, gyros are not required as quaternion already has that part + if ((sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { + use_sensor = -1; + } + } else { + if ((sensor_cal->gyro.status & sensor_cal->accel.status & sensor_cal->compass.status & INV_SENSOR_ON) == 0) { + use_sensor = -1; + } + } + + switch (use_sensor) { + case 0: + hal_out.nine_axis_status = (sensor_cal->gyro.status & INV_NEW_DATA) ? 1 : 0; + hal_out.nav_timestamp = sensor_cal->gyro.timestamp; + break; + case 1: + hal_out.nine_axis_status = (sensor_cal->accel.status & INV_NEW_DATA) ? 1 : 0; + hal_out.nav_timestamp = sensor_cal->accel.timestamp; + break; + case 2: + hal_out.nine_axis_status = (sensor_cal->compass.status & INV_NEW_DATA) ? 1 : 0; + hal_out.nav_timestamp = sensor_cal->compass.timestamp; + break; + case 3: + hal_out.nine_axis_status = (sensor_cal->quat.status & INV_NEW_DATA) ? 1 : 0; + hal_out.nav_timestamp = sensor_cal->quat.timestamp; + break; + default: + hal_out.nine_axis_status = 0; // Don't output quaternion related info + break; + } + + /* Converts fixed point to uT. Fixed point has 1 uT = 2^16. + * So this is: 1 / 2^16*/ + #define COMPASS_CONVERSION 1.52587890625e-005f + + inv_get_compass_set(compass, &accuracy, &(hal_out.mag_timestamp) ); + hal_out.accuracy_mag = (int ) accuracy; + + for (i=0; i<3; i++) { + if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_CONTIGUOUS)) == + INV_NEW_DATA ) { + // set the state variables to match output with input + inv_calc_state_to_match_output(&hal_out.lp_filter[i], (float ) compass[i]); + } + + if ((sensor_cal->compass.status & (INV_NEW_DATA | INV_RAW_DATA)) == + (INV_NEW_DATA | INV_RAW_DATA) ) { + + hal_out.compass_float[i] = inv_biquad_filter_process(&hal_out.lp_filter[i], + (float ) compass[i]) * COMPASS_CONVERSION; + + } else if ((sensor_cal->compass.status & INV_NEW_DATA) == INV_NEW_DATA ) { + hal_out.compass_float[i] = (float ) compass[i] * COMPASS_CONVERSION; + } + + } + return INV_SUCCESS; +} + +/** Turns off generation of HAL outputs. +* @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_stop_hal_outputs(void) +{ + inv_error_t result; + result = inv_unregister_data_cb(inv_generate_hal_outputs); + return result; +} + +/** Turns on generation of HAL outputs. This should be called after inv_stop_hal_outputs() +* to turn generation of HAL outputs back on. It is automatically called by inv_enable_hal_outputs(). +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_start_hal_outputs(void) +{ + inv_error_t result; + result = + inv_register_data_cb(inv_generate_hal_outputs, + INV_PRIORITY_HAL_OUTPUTS, + INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); + return result; +} +/* file name: lowPassFilterCoeff_1_6.c */ +float compass_low_pass_filter_coeff[5] = +{+2.000000000000f, +1.000000000000f, -1.279632424998f, +0.477592250073f, +0.049489956269f}; + +/** Initializes hal outputs class. This is called automatically by the +* enable function. It may be called any time the feature is enabled, but +* is typically not needed to be called by outside callers. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_init_hal_outputs(void) +{ + int i; + memset(&hal_out, 0, sizeof(hal_out)); + for (i=0; i<3; i++) { + inv_init_biquad_filter(&hal_out.lp_filter[i], compass_low_pass_filter_coeff); + } + + return INV_SUCCESS; +} + +/** Turns on creation and storage of HAL type results. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_enable_hal_outputs(void) +{ + inv_error_t result; + + // don't need to check the result for inv_init_hal_outputs + // since it's always INV_SUCCESS + inv_init_hal_outputs(); + + result = inv_register_mpl_start_notification(inv_start_hal_outputs); + return result; +} + +/** Turns off creation and storage of HAL type results. +*/ +inv_error_t inv_disable_hal_outputs(void) +{ + inv_error_t result; + + inv_stop_hal_outputs(); // Ignore error if we have already stopped this + result = inv_unregister_mpl_start_notification(inv_start_hal_outputs); + return result; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/hal_outputs.h b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h new file mode 100644 index 0000000..41b72c6 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/hal_outputs.h @@ -0,0 +1,49 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_HAL_OUTPUTS_H__ +#define INV_HAL_OUTPUTS_H__ + +#ifdef __cplusplus +extern "C" { +#endif + + int inv_get_sensor_type_orientation(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_accelerometer(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_gyroscope(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_gyroscope_raw(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_magnetic_field(float *values, int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_rotation_vector(float *values, int8_t *accuracy, + inv_time_t * timestamp); + + int inv_get_sensor_type_linear_acceleration(float *values, + int8_t *accuracy, + inv_time_t * timestamp); + int inv_get_sensor_type_gravity(float *values, int8_t *accuracy, + inv_time_t * timestamp); + + inv_error_t inv_enable_hal_outputs(void); + inv_error_t inv_disable_hal_outputs(void); + inv_error_t inv_init_hal_outputs(void); + inv_error_t inv_start_hal_outputs(void); + inv_error_t inv_stop_hal_outputs(void); + + // Set data rates for virtual sensors + void inv_set_linear_acceleration_sample_rate(long sample_rate_us); + void inv_set_gravity_sample_rate(long sample_rate_us); + +#ifdef __cplusplus +} +#endif + +#endif // INV_HAL_OUTPUTS_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/invensense.h b/60xx/libsensors_iio/software/core/mllite/invensense.h new file mode 100644 index 0000000..fb8e12b --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/invensense.h @@ -0,0 +1,28 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/** + * Main header file for Invensense's basic library. + */ + +#include "data_builder.h" +#include "hal_outputs.h" +#include "message_layer.h" +#include "mlmath.h" +#include "ml_math_func.h" +#include "mpl.h" +#include "results_holder.h" +#include "start_manager.h" +#include "storage_manager.h" +#include "log.h" +#include "mlinclude.h" +//#include "..\HAL\include\mlos.h" diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c new file mode 100644 index 0000000..72940f7 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.c @@ -0,0 +1,279 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id:$ + * + *****************************************************************************/ + +/** + * @defgroup ML_LOAD_DMP + * + * @{ + * @file ml_load_dmp.c + * @brief functions for writing dmp firmware. + */ +#include + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-loaddmp" + +#include "ml_load_dmp.h" +#include "mlos.h" + +#define LOADDMP_LOG MPL_LOGI +#define LOADDMP_LOG MPL_LOGI + +#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) +#define DMP_CODE_SIZE 3065 + +static const unsigned char dmpMemory[DMP_CODE_SIZE] = { + /* bank # 0 */ + 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x24, 0x00, 0x00, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, + 0x03, 0x0c, 0x30, 0xc3, 0x0e, 0x8c, 0x8c, 0xe9, 0x14, 0xd5, 0x40, 0x02, 0x13, 0x71, 0x0f, 0x8e, + 0x38, 0x83, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, 0x25, 0x8e, 0xf8, 0x83, 0x30, 0x00, 0xf8, 0x83, + 0x00, 0x00, 0x00, 0x01, 0x0f, 0xfe, 0xa9, 0xd6, 0x24, 0x00, 0x04, 0x00, 0x1a, 0x82, 0x79, 0xa1, + 0xff, 0xff, 0x00, 0x00, 0x00, 0x14, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x12, 0x00, 0x02, + 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x00, 0x0c, 0x00, 0x00, 0x00, 0x0c, 0x18, 0x6e, 0x00, 0x00, 0x06, 0x92, 0x0a, 0x16, 0xc0, 0xdf, + 0xff, 0xff, 0x02, 0x56, 0xfd, 0x8c, 0xd3, 0x77, 0xff, 0xe1, 0xc4, 0x96, 0xe0, 0xc5, 0xbe, 0xaa, + 0x00, 0x00, 0x00, 0x00, 0xff, 0xff, 0x0b, 0x2b, 0x00, 0x00, 0x16, 0x57, 0x00, 0x00, 0x03, 0x59, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1d, 0xfa, 0x00, 0x02, 0x6c, 0x1d, 0x00, 0x00, 0x00, 0x00, + 0x3f, 0xff, 0xdf, 0xeb, 0x00, 0x3e, 0xb3, 0xb6, 0x00, 0x0d, 0x22, 0x78, 0x00, 0x00, 0x2f, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x19, 0x42, 0xb5, 0x00, 0x00, 0x39, 0xa2, 0x00, 0x00, 0xb3, 0x65, + 0x3f, 0x84, 0x05, 0x99, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, + 0x00, 0x00, 0x00, 0x00, 0xf2, 0xbc, 0x3a, 0x39, 0xf1, 0x53, 0x7a, 0x76, 0xd9, 0x0e, 0x9f, 0xc8, + /* bank # 1 */ + 0x10, 0x00, 0x00, 0x00, 0x10, 0x00, 0xfa, 0x92, 0x10, 0x00, 0x22, 0x5e, 0x00, 0x0d, 0x22, 0x9f, + 0x00, 0x01, 0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0xff, 0x46, 0x00, 0x00, 0x63, 0xd4, 0x00, 0x00, + 0x10, 0x00, 0x00, 0x00, 0x04, 0xd6, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, 0x04, 0xcc, 0x00, 0x00, + 0x00, 0x00, 0x10, 0x72, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x05, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, + 0x00, 0x00, 0x00, 0x32, 0xf8, 0x98, 0x00, 0x00, 0xff, 0x65, 0x00, 0x00, 0x83, 0x0f, 0x00, 0x00, + 0xff, 0x9b, 0xfc, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x02, 0x00, 0x00, + 0x00, 0x01, 0xfb, 0x83, 0x00, 0x68, 0x00, 0x00, 0x00, 0xd9, 0xfc, 0x00, 0x7c, 0xf1, 0xff, 0x83, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x64, 0x03, 0xe8, 0x00, 0x64, 0x00, 0x28, + 0x00, 0x00, 0x00, 0x25, 0x00, 0x00, 0x00, 0x00, 0x16, 0xa0, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, + 0x00, 0x00, 0x10, 0x00, 0x00, 0x2f, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf4, 0x00, 0x00, 0x10, 0x00, + /* bank # 2 */ + 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xa2, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0e, 0x00, 0x0e, + 0x00, 0x00, 0x0a, 0xc7, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xff, 0xff, 0xff, 0x9c, + 0x00, 0x00, 0x0b, 0x2b, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, + 0xff, 0xe5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xd3, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3c, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x0c, 0x0a, 0x4e, 0x68, 0xcd, 0xcf, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xc6, 0x19, 0xce, 0x82, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xd7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc7, 0x93, 0x8f, 0x9d, 0x1e, 0x1b, 0x1c, 0x19, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x67, 0x7d, 0xdf, 0x7e, 0x72, 0x90, 0x2e, 0x55, 0x4c, 0xf6, 0xe6, 0x88, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + /* bank # 4 */ + 0xd8, 0xdc, 0xb4, 0xb8, 0xb0, 0xd8, 0xb9, 0xab, 0xf3, 0xf8, 0xfa, 0xb3, 0xb7, 0xbb, 0x8e, 0x9e, + 0xae, 0xf1, 0x32, 0xf5, 0x1b, 0xf1, 0xb4, 0xb8, 0xb0, 0x80, 0x97, 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, + 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0x0c, 0xc9, 0x2c, 0x97, 0x97, 0x97, + 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb2, 0x89, 0x99, 0xa9, 0x2d, 0x55, 0x7d, 0xb0, 0x99, + 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, 0x99, + 0x99, 0x99, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xba, 0xa3, 0xb4, 0xb2, 0x80, 0xc0, + 0xb8, 0xa8, 0x97, 0x11, 0xb2, 0x83, 0x98, 0xba, 0xa3, 0xf0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, + 0xb2, 0xb9, 0xb4, 0x98, 0x83, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xba, 0xb5, 0xb1, 0xa3, 0x83, 0x93, + 0xf0, 0x00, 0x28, 0x50, 0xf5, 0xb2, 0xb6, 0xaa, 0x83, 0x93, 0x28, 0x54, 0x7c, 0xb9, 0xf1, 0xa3, + 0x82, 0x93, 0x61, 0xba, 0xa2, 0xda, 0xde, 0xdf, 0xdb, 0x8b, 0x9a, 0xb9, 0xae, 0xf5, 0x60, 0x68, + 0x70, 0xf1, 0xda, 0xba, 0xa2, 0xdf, 0xd9, 0xa2, 0xfa, 0xb9, 0xa3, 0x82, 0x92, 0xdb, 0x31, 0xba, + 0xa2, 0xd9, 0xf8, 0xdf, 0xa4, 0x83, 0xc2, 0xc5, 0xc7, 0x85, 0xc1, 0xb8, 0xa2, 0xdf, 0xdf, 0xdf, + 0xba, 0xa0, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb3, 0x8d, 0xb4, 0x98, 0x0d, 0x35, + 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, 0x19, + 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xd8, 0xb8, + 0xb4, 0xb0, 0xf1, 0x99, 0x82, 0xa8, 0x2d, 0x55, 0x7d, 0x98, 0xa8, 0x0e, 0x16, 0x1e, 0xa2, 0x2c, + /* bank # 5 */ + 0x54, 0x7c, 0xa8, 0x92, 0xf5, 0x2c, 0x54, 0x88, 0x98, 0xf1, 0x35, 0xd9, 0xf4, 0x18, 0xd8, 0xf1, + 0xa2, 0xd0, 0xf8, 0xf9, 0xa8, 0x84, 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0x83, 0xc5, 0xda, 0xdf, 0x69, + 0xdf, 0x83, 0xc1, 0xd8, 0xf4, 0x01, 0x14, 0xf1, 0xa8, 0x82, 0x4e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, + 0x82, 0xf5, 0xd9, 0x92, 0x28, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, + 0xc0, 0xf9, 0xd1, 0xd9, 0x97, 0x82, 0xf1, 0x29, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, + 0x82, 0xf4, 0xc2, 0x03, 0xd8, 0xde, 0xdf, 0x1a, 0xd8, 0xf1, 0xa2, 0xfa, 0xf9, 0xa8, 0x84, 0x98, + 0xd9, 0xc7, 0xdf, 0xf8, 0xf8, 0xf8, 0x83, 0xc7, 0xda, 0xdf, 0x69, 0xdf, 0xf8, 0x83, 0xc3, 0xd8, + 0xf4, 0x01, 0x14, 0xf1, 0x98, 0xa8, 0x82, 0x2e, 0xa8, 0x84, 0xf3, 0x11, 0xd1, 0x82, 0xf5, 0xd9, + 0x92, 0x50, 0x97, 0x88, 0xf1, 0x09, 0xf4, 0x1c, 0xd8, 0x84, 0xa8, 0xf3, 0xc0, 0xf8, 0xf9, 0xd1, + 0xd9, 0x97, 0x82, 0xf1, 0x49, 0xf4, 0x0d, 0xd8, 0xf3, 0xf9, 0xf9, 0xd1, 0xd9, 0x82, 0xf4, 0xc4, + 0x03, 0xd8, 0xde, 0xdf, 0xd8, 0xf1, 0xad, 0x88, 0x98, 0xcc, 0xa8, 0x09, 0xf9, 0xd9, 0x82, 0x92, + 0xa4, 0xf0, 0x2c, 0x50, 0x78, 0xa8, 0x82, 0xf5, 0x7c, 0xf1, 0x88, 0x3a, 0xcf, 0x94, 0x4a, 0x6e, + 0x98, 0xdb, 0x69, 0x31, 0xd9, 0x84, 0xc4, 0xcd, 0xfc, 0xdb, 0x6d, 0xd9, 0xa8, 0xfc, 0xdb, 0x25, + 0xda, 0xad, 0xf2, 0xde, 0xf9, 0xd8, 0xf2, 0xa5, 0xf8, 0x8d, 0x94, 0xd1, 0xda, 0xf4, 0x19, 0xd8, + 0xa8, 0xf2, 0x05, 0xd1, 0xa4, 0xda, 0xc0, 0xa5, 0xf7, 0xde, 0xf9, 0xd8, 0xa5, 0xf8, 0x85, 0x95, + 0x18, 0xdf, 0xf1, 0xad, 0x8e, 0xc3, 0xc5, 0xc7, 0xd8, 0xf2, 0xa5, 0xf8, 0xd1, 0xd9, 0xf1, 0xad, + /* bank # 6 */ + 0x8f, 0xc3, 0xc5, 0xc7, 0xd8, 0xa5, 0xf2, 0xf9, 0xf9, 0xa8, 0x8d, 0x9d, 0xf0, 0x28, 0x50, 0x78, + 0x84, 0x98, 0xf1, 0x26, 0x84, 0x95, 0x2d, 0x88, 0x98, 0x11, 0x52, 0x87, 0x12, 0x12, 0x88, 0x31, + 0xf9, 0xd9, 0xf1, 0x8d, 0x9d, 0x7a, 0xf5, 0x7c, 0xf1, 0x88, 0x7a, 0x98, 0x45, 0x8e, 0x0e, 0xf5, + 0x82, 0x92, 0x7c, 0x88, 0xf1, 0x5a, 0x98, 0x5a, 0x82, 0x92, 0x7e, 0x88, 0x94, 0x69, 0x98, 0x1e, + 0x11, 0x08, 0xd0, 0xf5, 0x04, 0xf1, 0x1e, 0x97, 0x02, 0x02, 0x98, 0x36, 0x25, 0xdb, 0xf9, 0xd9, + 0xf0, 0x8d, 0x92, 0xa8, 0x49, 0x30, 0x2c, 0x50, 0x8e, 0xc9, 0x88, 0x98, 0x34, 0xf5, 0x04, 0xf1, + 0x61, 0xdb, 0xf9, 0xd9, 0xf2, 0xa5, 0xf8, 0xdb, 0xf9, 0xd9, 0xf3, 0xfa, 0xd8, 0xf2, 0xa5, 0xf8, + 0xf9, 0xd9, 0xf1, 0xaf, 0x8e, 0xc3, 0xc5, 0xc7, 0xae, 0x82, 0xc3, 0xc5, 0xc7, 0xd8, 0xa4, 0xf2, + 0xf8, 0xd1, 0xa5, 0xf3, 0xd9, 0xde, 0xf9, 0xdf, 0xd8, 0xa4, 0xf2, 0xf9, 0xa5, 0xf8, 0xf8, 0xd1, + 0xda, 0xf9, 0xf9, 0xf4, 0x18, 0xd8, 0xf2, 0xf9, 0xf9, 0xf3, 0xfb, 0xf9, 0xd1, 0xda, 0xf4, 0x1d, + 0xd9, 0xf3, 0xa4, 0x84, 0xc8, 0xa8, 0x9f, 0x01, 0xdb, 0xd1, 0xd8, 0xf4, 0x10, 0x1c, 0xd8, 0xbb, + 0xaf, 0xd0, 0xf2, 0xde, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xb8, 0xd8, 0xf3, 0xaf, + 0x84, 0xc0, 0xa5, 0xfa, 0xf8, 0xf2, 0x85, 0xa5, 0xc6, 0xd8, 0xd8, 0xf2, 0xf9, 0xf6, 0xb5, 0xb9, + 0xb0, 0x8a, 0x95, 0xa3, 0xde, 0x3c, 0xa3, 0xd9, 0xf8, 0xd8, 0x5c, 0xa3, 0xd9, 0xf8, 0xd8, 0x7c, + 0xa3, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa5, 0xd9, 0xdf, 0xda, 0xfa, 0xd8, 0xb1, 0x85, 0x30, + 0xf7, 0xd9, 0xde, 0xd8, 0xf8, 0x30, 0xad, 0xda, 0xde, 0xd8, 0xf2, 0xb4, 0x8c, 0x99, 0xa3, 0x2d, + /* bank # 7 */ + 0x55, 0x7d, 0xa0, 0x83, 0xdf, 0xdf, 0xdf, 0xb5, 0x91, 0xa0, 0xf6, 0x29, 0xd9, 0xfb, 0xd8, 0xa0, + 0xfc, 0x29, 0xd9, 0xfa, 0xd8, 0xa0, 0xd0, 0x51, 0xd9, 0xf8, 0xd8, 0xfc, 0x51, 0xd9, 0xf9, 0xd8, + 0x79, 0xd9, 0xfb, 0xd8, 0xa0, 0xd0, 0xfc, 0x79, 0xd9, 0xfa, 0xd8, 0xa1, 0xf9, 0xf9, 0xf9, 0xf9, + 0xf9, 0xa0, 0xda, 0xdf, 0xdf, 0xdf, 0xd8, 0xa1, 0xf8, 0xf8, 0xf8, 0xf8, 0xf8, 0xac, 0xde, 0xf8, + 0xad, 0xde, 0x83, 0x93, 0xac, 0x2c, 0x54, 0x7c, 0xf1, 0xa8, 0xdf, 0xdf, 0xdf, 0xf6, 0x9d, 0x2c, + 0xda, 0xa0, 0xdf, 0xd9, 0xfa, 0xdb, 0x2d, 0xf8, 0xd8, 0xa8, 0x50, 0xda, 0xa0, 0xd0, 0xde, 0xd9, + 0xd0, 0xf8, 0xf8, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa8, 0x78, 0xda, 0xa0, 0xd0, 0xdf, 0xd9, 0xd0, + 0xfa, 0xf8, 0xf8, 0xf8, 0xf8, 0xdb, 0x7d, 0xf8, 0xd8, 0x9c, 0xa8, 0x8c, 0xf5, 0x30, 0xdb, 0x38, + 0xd9, 0xd0, 0xde, 0xdf, 0xa0, 0xd0, 0xde, 0xdf, 0xd8, 0xa8, 0x48, 0xdb, 0x58, 0xd9, 0xdf, 0xd0, + 0xde, 0xa0, 0xdf, 0xd0, 0xde, 0xd8, 0xa8, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xa0, 0xdf, 0xdf, + 0xd8, 0xf1, 0xa8, 0x88, 0x90, 0x2c, 0x54, 0x7c, 0x98, 0xa8, 0xd0, 0x5c, 0x38, 0xd1, 0xda, 0xf2, + 0xae, 0x8c, 0xdf, 0xf9, 0xd8, 0xb0, 0x87, 0xa8, 0xc1, 0xc1, 0xb1, 0x88, 0xa8, 0xc6, 0xf9, 0xf9, + 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xa8, 0xf9, 0xda, + 0x36, 0xd8, 0xa8, 0xf9, 0xda, 0x36, 0xd8, 0xf7, 0x8d, 0x9d, 0xad, 0xf8, 0x18, 0xda, 0xf2, 0xae, + 0xdf, 0xd8, 0xf7, 0xad, 0xfa, 0x30, 0xd9, 0xa4, 0xde, 0xf9, 0xd8, 0xf2, 0xae, 0xde, 0xfa, 0xf9, + 0x83, 0xa7, 0xd9, 0xc3, 0xc5, 0xc7, 0xf1, 0x88, 0x9b, 0xa7, 0x7a, 0xad, 0xf7, 0xde, 0xdf, 0xa4, + /* bank # 8 */ + 0xf8, 0x84, 0x94, 0x08, 0xa7, 0x97, 0xf3, 0x00, 0xae, 0xf2, 0x98, 0x19, 0xa4, 0x88, 0xc6, 0xa3, + 0x94, 0x88, 0xf6, 0x32, 0xdf, 0xf2, 0x83, 0x93, 0xdb, 0x09, 0xd9, 0xf2, 0xaa, 0xdf, 0xd8, 0xd8, + 0xae, 0xf8, 0xf9, 0xd1, 0xda, 0xf3, 0xa4, 0xde, 0xa7, 0xf1, 0x88, 0x9b, 0x7a, 0xd8, 0xf3, 0x84, + 0x94, 0xae, 0x19, 0xf9, 0xda, 0xaa, 0xf1, 0xdf, 0xd8, 0xa8, 0x81, 0xc0, 0xc3, 0xc5, 0xc7, 0xa3, + 0x92, 0x83, 0xf6, 0x28, 0xad, 0xde, 0xd9, 0xf8, 0xd8, 0xa3, 0x50, 0xad, 0xd9, 0xf8, 0xd8, 0xa3, + 0x78, 0xad, 0xd9, 0xf8, 0xd8, 0xf8, 0xf9, 0xd1, 0xa1, 0xda, 0xde, 0xc3, 0xc5, 0xc7, 0xd8, 0xa1, + 0x81, 0x94, 0xf8, 0x18, 0xf2, 0xb0, 0x89, 0xac, 0xc3, 0xc5, 0xc7, 0xf1, 0xd8, 0xb8, 0xb4, 0xb0, + 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, 0xf0, 0x0c, 0x20, + 0x14, 0x40, 0xb0, 0xb4, 0xb8, 0xf0, 0xa8, 0x8a, 0x9a, 0x28, 0x50, 0x78, 0xb7, 0x9b, 0xa8, 0x29, + 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xf1, 0xbb, 0xab, 0x88, 0x00, + 0x2c, 0x54, 0x7c, 0xf0, 0xb3, 0x8b, 0xb8, 0xa8, 0x04, 0x28, 0x50, 0x78, 0xf1, 0xb0, 0x88, 0xb4, + 0x97, 0x26, 0xa8, 0x59, 0x98, 0xbb, 0xab, 0xb3, 0x8b, 0x02, 0x26, 0x46, 0x66, 0xb0, 0xb8, 0xf0, + 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, 0x59, 0x8b, 0x20, + 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, 0x31, 0x8b, 0x30, + 0x49, 0x60, 0x88, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, + 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, + /* bank # 9 */ + 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, + 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, + 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, + 0x88, 0x2d, 0x55, 0x7d, 0xf5, 0xb0, 0xb4, 0xb8, 0x88, 0x98, 0xad, 0x2c, 0x54, 0x7c, 0xb5, 0xb9, + 0x9e, 0xa3, 0xdf, 0xdf, 0xdf, 0xf6, 0xa3, 0x30, 0xd9, 0xfa, 0xdb, 0x35, 0xf8, 0xd8, 0xa3, 0x50, + 0xd0, 0xd9, 0xf8, 0xdb, 0x55, 0xf8, 0xd8, 0xa3, 0x70, 0xd0, 0xd9, 0xfa, 0xdb, 0x75, 0xf8, 0xd8, + 0xa3, 0xb4, 0x8d, 0x9d, 0x30, 0xdb, 0x38, 0xd9, 0xd0, 0xde, 0xdf, 0xd8, 0xa3, 0x48, 0xdb, 0x58, + 0xd9, 0xdf, 0xd0, 0xde, 0xd8, 0xa3, 0x68, 0xdb, 0x70, 0xd9, 0xdf, 0xdf, 0xd8, 0xf1, 0xa3, 0xb1, + 0xb5, 0x82, 0xc0, 0x83, 0x93, 0x0e, 0x0a, 0x0a, 0x16, 0x12, 0x1e, 0x58, 0x38, 0x1a, 0xa3, 0x8f, + 0x7c, 0x83, 0x0e, 0xa3, 0x12, 0x86, 0x61, 0xd1, 0xd9, 0xc7, 0xc7, 0xd8, 0xa3, 0x6d, 0xd1, 0xd9, + 0xc7, 0xd8, 0xa3, 0x71, 0xd1, 0xa6, 0xd9, 0xc5, 0xda, 0xdf, 0xd8, 0x9f, 0x83, 0xf3, 0xa3, 0x65, + 0xd1, 0xaf, 0xc6, 0xd9, 0xfa, 0xda, 0xdf, 0xd8, 0xa3, 0x8f, 0xdf, 0x1d, 0xd1, 0xa3, 0xd9, 0xfa, + 0xd8, 0xa3, 0x31, 0xda, 0xfa, 0xd9, 0xaf, 0xdf, 0xd8, 0xa3, 0xfa, 0xf9, 0xd1, 0xd9, 0xaf, 0xd0, + 0x96, 0xc1, 0xae, 0xd0, 0x0c, 0x8e, 0x94, 0xa3, 0xf7, 0x72, 0xdf, 0xf3, 0x83, 0x93, 0xdb, 0x09, + 0xd9, 0xf2, 0xaa, 0xd0, 0xde, 0xd8, 0xd8, 0xd8, 0xf2, 0xab, 0xf8, 0xf9, 0xd9, 0xb0, 0x87, 0xc4, + 0xaa, 0xf1, 0xdf, 0xdf, 0xbb, 0xaf, 0xdf, 0xdf, 0xb9, 0xd8, 0xb1, 0xf1, 0xa3, 0x97, 0x8e, 0x60, + /* bank # 10 */ + 0xdf, 0xb0, 0x84, 0xf2, 0xc8, 0x93, 0x85, 0xf1, 0x4a, 0xb1, 0x83, 0xa3, 0x08, 0xb5, 0x83, 0x9a, + 0x08, 0x10, 0xb7, 0x9f, 0x10, 0xb5, 0xd8, 0xf1, 0xb0, 0xba, 0xae, 0xb0, 0x8a, 0xc2, 0xb2, 0xb6, + 0x8e, 0x9e, 0xf1, 0xfb, 0xd9, 0xf4, 0x1d, 0xd8, 0xf9, 0xd9, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, + 0x61, 0xd9, 0xae, 0xfb, 0xd8, 0xf4, 0x0c, 0xf1, 0xd8, 0xf8, 0xf8, 0xad, 0x19, 0xd9, 0xae, 0xfb, + 0xdf, 0xd8, 0xf4, 0x16, 0xf1, 0xd8, 0xf8, 0xad, 0x8d, 0x61, 0xd9, 0xf4, 0xf4, 0xac, 0xf5, 0x9c, + 0x9c, 0x8d, 0xdf, 0x2b, 0xba, 0xb6, 0xae, 0xfa, 0xf8, 0xf4, 0x0b, 0xd8, 0xf1, 0xae, 0xd0, 0xf8, + 0xad, 0x51, 0xda, 0xae, 0xfa, 0xf8, 0xf1, 0xd8, 0xb9, 0xb1, 0xb6, 0xa3, 0x83, 0x9c, 0x08, 0xb9, + 0xb1, 0x83, 0x9a, 0xb5, 0xaa, 0xc0, 0xfd, 0x30, 0x83, 0xb7, 0x9f, 0x10, 0xb5, 0x8b, 0x93, 0xf2, + 0x02, 0x02, 0xd1, 0xab, 0xda, 0xde, 0xd8, 0xd8, 0xb1, 0xb9, 0xf3, 0x8b, 0xa3, 0x91, 0xb6, 0x09, + 0xb4, 0xd9, 0xab, 0xde, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xb3, 0x8b, 0x8b, 0x8b, 0x8b, + 0x8b, 0xb0, 0x87, 0xa3, 0xa3, 0xa3, 0xa3, 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xf1, 0x87, 0xb5, 0x9a, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xba, + 0xac, 0xdf, 0xb9, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xa3, 0xa3, 0xd8, 0xd8, 0xd8, 0xbb, 0xb3, 0xb7, 0xf1, 0xaa, 0xf9, 0xda, 0xff, 0xd9, 0x80, + 0x9a, 0xaa, 0x28, 0xb4, 0x80, 0x98, 0xa7, 0x20, 0xb7, 0x97, 0x87, 0xa8, 0x66, 0x88, 0xf0, 0x79, + 0x51, 0xf1, 0x90, 0x2c, 0x87, 0x0c, 0xa7, 0x81, 0x97, 0x62, 0x93, 0xf0, 0x71, 0x71, 0x60, 0x85, + /* bank # 11 */ + 0x94, 0x01, 0x29, 0x51, 0x79, 0x90, 0xa5, 0xf1, 0x28, 0x4c, 0x6c, 0x87, 0x0c, 0x95, 0x18, 0x85, + 0x78, 0xa3, 0x83, 0x90, 0x28, 0x4c, 0x6c, 0x88, 0x6c, 0xd8, 0xf3, 0xa2, 0x82, 0x00, 0xf2, 0x10, + 0xa8, 0x92, 0x19, 0x80, 0xa2, 0xf2, 0xd9, 0x26, 0xd8, 0xf1, 0x88, 0xa8, 0x4d, 0xd9, 0x48, 0xd8, + 0x96, 0xa8, 0x39, 0x80, 0xd9, 0x3c, 0xd8, 0x95, 0x80, 0xa8, 0x39, 0xa6, 0x86, 0x98, 0xd9, 0x2c, + 0xda, 0x87, 0xa7, 0x2c, 0xd8, 0xa8, 0x89, 0x95, 0x19, 0xa9, 0x80, 0xd9, 0x38, 0xd8, 0xa8, 0x89, + 0x39, 0xa9, 0x80, 0xda, 0x3c, 0xd8, 0xa8, 0x2e, 0xa8, 0x39, 0x90, 0xd9, 0x0c, 0xd8, 0xa8, 0x95, + 0x31, 0x98, 0xd9, 0x0c, 0xd8, 0xa8, 0x09, 0xd9, 0xff, 0xd8, 0x01, 0xda, 0xff, 0xd8, 0x95, 0x39, + 0xa9, 0xda, 0x26, 0xff, 0xd8, 0x90, 0xa8, 0x0d, 0x89, 0x99, 0xa8, 0x10, 0x80, 0x98, 0x21, 0xda, + 0x2e, 0xd8, 0x89, 0x99, 0xa8, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x86, 0x96, 0x31, 0x80, 0xda, + 0x2e, 0xd8, 0xa8, 0x87, 0x31, 0x80, 0xda, 0x2e, 0xd8, 0xa8, 0x82, 0x92, 0xf3, 0x41, 0x80, 0xf1, + 0xd9, 0x2e, 0xd8, 0xa8, 0x82, 0xf3, 0x19, 0x80, 0xf1, 0xd9, 0x2e, 0xd8, 0x82, 0xac, 0xf3, 0xc0, + 0xa2, 0x80, 0x22, 0xf1, 0xa6, 0x2e, 0xa7, 0x2e, 0xa9, 0x22, 0x98, 0xa8, 0x29, 0xda, 0xac, 0xde, + 0xff, 0xd8, 0xa2, 0xf2, 0x2a, 0xf1, 0xa9, 0x2e, 0x82, 0x92, 0xa8, 0xf2, 0x31, 0x80, 0xa6, 0x96, + 0xf1, 0xd9, 0x00, 0xac, 0x8c, 0x9c, 0x0c, 0x30, 0xac, 0xde, 0xd0, 0xde, 0xff, 0xd8, 0x8c, 0x9c, + 0xac, 0xd0, 0x10, 0xac, 0xde, 0x80, 0x92, 0xa2, 0xf2, 0x4c, 0x82, 0xa8, 0xf1, 0xca, 0xf2, 0x35, + 0xf1, 0x96, 0x88, 0xa6, 0xd9, 0x00, 0xd8, 0xf1, 0xff +}; + +#define DMP_VERSION (dmpMemory) + +inv_error_t inv_write_dmp_data(FILE *fd, const unsigned char *dmp, size_t len) +{ + inv_error_t result = INV_SUCCESS; + int bytesWritten = 0; + + if (len <= 0) { + MPL_LOGE("Nothing to write"); + return INV_ERROR_FILE_WRITE; + } else { + MPL_LOGI("dmp firmware size to write = %d", len); + } + if (fd == NULL) { + return INV_ERROR_FILE_OPEN; + } + bytesWritten = fwrite(dmp, 1, len, fd); + if (bytesWritten != len) { + MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", + bytesWritten, len); + result = INV_ERROR_FILE_WRITE; + } else { + MPL_LOGI("Bytes written = %d", bytesWritten); + } + return result; +} + +inv_error_t inv_load_dmp(FILE *fd) +{ + inv_error_t result = INV_SUCCESS; + result = inv_write_dmp_data(fd, DMP_VERSION, DMP_CODE_SIZE); + return result; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h new file mode 100644 index 0000000..4d98692 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_load_dmp.h @@ -0,0 +1,33 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +#ifndef INV_LOAD_DMP_H +#define INV_LOAD_DMP_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Includes. +*/ +#include "mltypes.h" + +/* + APIs +*/ +inv_error_t inv_load_dmp(FILE *fd); + +#ifdef __cplusplus +} +#endif +#endif /* INV_LOAD_DMP_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c new file mode 100644 index 0000000..b4c4249 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.c @@ -0,0 +1,353 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/****************************************************************************** + * + * $Id: ml_stored_data.c 6132 2011-10-01 03:17:27Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML_STORED_DATA + * + * @{ + * @file ml_stored_data.c + * @brief functions for reading and writing stored data sets. + * Typically, these functions process stored calibration data. + */ + +#include + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-storeload" + +#include "ml_stored_data.h" +#include "storage_manager.h" +#include "mlos.h" + +#define LOADCAL_DEBUG 0 +#define STORECAL_DEBUG 0 + +#define DEFAULT_KEY 29681 + +#define STORECAL_LOG MPL_LOGI +#define LOADCAL_LOG MPL_LOGI + +inv_error_t inv_read_cal(unsigned char **calData, size_t *bytesRead) +{ + FILE *fp; + inv_error_t result = INV_SUCCESS; + size_t fsize; + + fp = fopen(MLCAL_FILE,"rb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + + // obtain file size + fseek (fp, 0 , SEEK_END); + fsize = ftell (fp); + rewind (fp); + + *calData = (unsigned char *)inv_malloc(fsize); + if (*calData==NULL) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", fsize); + fclose(fp); + return INV_ERROR_MEMORY_EXAUSTED; + } + + *bytesRead = fread(*calData, 1, fsize, fp); + if (*bytesRead != fsize) { + MPL_LOGE("bytes read (%d) don't match file size (%d)\n", + *bytesRead, fsize); + result = INV_ERROR_FILE_READ; + goto read_cal_end; + } + else { + MPL_LOGI("Bytes read = %d", *bytesRead); + } + +read_cal_end: + fclose(fp); + return result; +} + +inv_error_t inv_write_cal(unsigned char *cal, size_t len) +{ + FILE *fp; + int bytesWritten; + inv_error_t result = INV_SUCCESS; + + if (len <= 0) { + MPL_LOGE("Nothing to write"); + return INV_ERROR_FILE_WRITE; + } + else { + MPL_LOGI("cal data size to write = %d", len); + } + fp = fopen(MLCAL_FILE,"wb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesWritten = fwrite(cal, 1, len, fp); + if (bytesWritten != len) { + MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", + bytesWritten, len); + result = INV_ERROR_FILE_WRITE; + } + else { + MPL_LOGI("Bytes written = %d", bytesWritten); + } + fclose(fp); + return result; +} + +/** + * @brief Loads a type 0 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * This calibration data is produced internally by the MPL and its + * size is 2777 bytes (header and checksum included). + * Calibration format type 1 is currently used for ITG3500 + * + * @pre inv_init_storage_manager() + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len) +{ + inv_error_t result; + + LOADCAL_LOG("Entering inv_load_cal_V0\n"); + + /*if (len != expLen) { + MPL_LOGE("Calibration data type 0 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + }*/ + + result = inv_load_mpl_states(calData, len); + return result; +} + +/** + * @brief Loads a type 1 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes, + * - accel biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 36 bytes (header and checksum included). + * Calibration format type 1 is produced by the MPU Self Test and + * substitutes the type 0 : inv_load_cal_V0(). + * + * @pre + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len) +{ + return INV_SUCCESS; +} + +/** + * @brief Loads a set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * + * @pre + * + * + * @param calData + * A pointer to an array of bytes to be parsed. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal(unsigned char *calData) +{ + int calType = 0; + int len = 0; + //int ptr; + //uint32_t chk = 0; + //uint32_t cmp_chk = 0; + + /*load_func_t loaders[] = { + inv_load_cal_V0, + inv_load_cal_V1, + }; + */ + + inv_load_cal_V0(calData, len); + + /* read the header (type and len) + len is the total record length including header and checksum */ + len = 0; + len += 16777216L * ((int)calData[0]); + len += 65536L * ((int)calData[1]); + len += 256 * ((int)calData[2]); + len += (int)calData[3]; + + calType = ((int)calData[4]) * 256 + ((int)calData[5]); + if (calType > 5) { + MPL_LOGE("Unsupported calibration file format %d. " + "Valid types 0..5\n", calType); + return INV_ERROR_INVALID_PARAMETER; + } + + /* call the proper method to read in the data */ + //return loaders[calType] (calData, len); + return 0; +} + +/** + * @brief Stores a set of calibration data. + * It generates a binary data set containing calibration data. + * The binary data set is intended to be stored into a file. + * + * @pre inv_dmp_open() + * + * @param calData + * A pointer to an array of bytes to be stored. + * @param length + * The amount of bytes available in the array. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_store_cal(unsigned char *calData, size_t length) +{ + inv_error_t res = 0; + size_t size; + + STORECAL_LOG("Entering inv_store_cal\n"); + + inv_get_mpl_state_size(&size); + + MPL_LOGI("inv_get_mpl_state_size() : size=%d", size); + + /* store data */ + res = inv_save_mpl_states(calData, size); + if(res != 0) + { + MPL_LOGE("inv_save_mpl_states() failed"); + } + + STORECAL_LOG("Exiting inv_store_cal\n"); + return INV_SUCCESS; +} + +/** + * @brief Load a calibration file. + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have NOT + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_load_calibration(void) +{ + unsigned char *calData= NULL; + inv_error_t result = 0; + size_t bytesRead = 0; + + result = inv_read_cal(&calData, &bytesRead); + if(result != INV_SUCCESS) { + MPL_LOGE("Could not load cal file - " + "aborting\n"); + goto free_mem_n_exit; + } + + result = inv_load_mpl_states(calData, bytesRead); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not load the calibration data - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + } + +free_mem_n_exit: + inv_free(calData); + return result; +} + +/** + * @brief Store runtime calibration data to a file + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have NOT + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_store_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + size_t length; + + result = inv_get_mpl_state_size(&length); + calData = (unsigned char *)inv_malloc(length); + if (!calData) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", length); + return INV_ERROR_MEMORY_EXAUSTED; + } + else { + MPL_LOGI("mpl state size = %d", length); + } + + result = inv_save_mpl_states(calData, length); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not save mpl states - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + } + else { + MPL_LOGE("calData from inv_save_mpl_states, size=%d", + strlen((char *)calData)); + } + + result = inv_write_cal(calData, length); + if (result != INV_SUCCESS) { + MPL_LOGE("Could not store calibrated data on file - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + +free_mem_n_exit: + inv_free(calData); + return result; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h new file mode 100644 index 0000000..115b34c --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_stored_data.h @@ -0,0 +1,53 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: ml_stored_data.h 5873 2011-08-11 03:13:48Z mcaramello $ + * + ******************************************************************************/ + +#ifndef INV_MPL_STORED_DATA_H +#define INV_MPL_STORED_DATA_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Includes. +*/ +#include "mltypes.h" + +/* + Defines +*/ +#define MLCAL_FILE "/data/inv_cal_data.bin" + +/* + APIs +*/ +inv_error_t inv_load_calibration(void); +inv_error_t inv_store_calibration(void); + +/* + Internal APIs +*/ +inv_error_t inv_read_cal(unsigned char **, size_t *); +inv_error_t inv_write_cal(unsigned char *cal, size_t len); +inv_error_t inv_load_cal_V0(unsigned char *calData, size_t len); +inv_error_t inv_load_cal_V1(unsigned char *calData, size_t len); + +/* + Other prototypes +*/ +inv_error_t inv_load_cal(unsigned char *calData); +inv_error_t inv_store_cal(unsigned char *calData, size_t length); + +#ifdef __cplusplus +} +#endif +#endif /* INV_MPL_STORED_DATA_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c new file mode 100644 index 0000000..d0c4513 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.c @@ -0,0 +1,423 @@ +#include +#include +#include "ml_sysfs_helper.h" +#include +#include +#define MPU_SYSFS_ABS_PATH "/sys/class/invensense/mpu" + +enum PROC_SYSFS_CMD { + CMD_GET_SYSFS_PATH, + CMD_GET_DMP_PATH, + CMD_GET_CHIP_NAME, + CMD_GET_SYSFS_KEY, + CMD_GET_TRIGGER_PATH, + CMD_GET_DEVICE_NODE +}; +static char sysfs_path[100]; +static char *chip_name[] = { + "ITG3500", + "MPU6050", + "MPU9150", + "MPU3050", + "MPU6500" +}; +static int chip_ind; +static int initialized =0; +static int status = 0; +static int iio_initialized = 0; +static int iio_dev_num = 0; + +#define IIO_MAX_NAME_LENGTH 30 + +#define FORMAT_SCAN_ELEMENTS_DIR "%s/scan_elements" +#define FORMAT_TYPE_FILE "%s_type" + +#define CHIP_NUM ARRAY_SIZE(chip_name) + +static const char *iio_dir = "/sys/bus/iio/devices/"; + +/** + * find_type_by_name() - function to match top level types by name + * @name: top level type instance name + * @type: the type of top level instance being sort + * + * Typical types this is used for are device and trigger. + **/ +int find_type_by_name(const char *name, const char *type) +{ + const struct dirent *ent; + int number, numstrlen; + + FILE *nameFile; + DIR *dp; + char thisname[IIO_MAX_NAME_LENGTH]; + char *filename; + + dp = opendir(iio_dir); + if (dp == NULL) { + printf("No industrialio devices available"); + return -ENODEV; + } + + while (ent = readdir(dp), ent != NULL) { + if (strcmp(ent->d_name, ".") != 0 && + strcmp(ent->d_name, "..") != 0 && + strlen(ent->d_name) > strlen(type) && + strncmp(ent->d_name, type, strlen(type)) == 0) { + numstrlen = sscanf(ent->d_name + strlen(type), + "%d", + &number); + /* verify the next character is not a colon */ + if (strncmp(ent->d_name + strlen(type) + numstrlen, + ":", + 1) != 0) { + filename = malloc(strlen(iio_dir) + + strlen(type) + + numstrlen + + 6); + if (filename == NULL) + return -ENOMEM; + sprintf(filename, "%s%s%d/name", + iio_dir, + type, + number); + nameFile = fopen(filename, "r"); + if (!nameFile) + continue; + free(filename); + fscanf(nameFile, "%s", thisname); + if (strcmp(name, thisname) == 0) + return number; + fclose(nameFile); + } + } + } + return -ENODEV; +} + +/* mode 0: search for which chip in the system and fill sysfs path + mode 1: return event number + */ +static int parsing_proc_input(int mode, char *name){ + const char input[] = "/proc/bus/input/devices"; + char line[4096], d; + char tmp[100]; + FILE *fp; + int i, j, result, find_flag; + int event_number = -1; + int input_number = -1; + + if(NULL == (fp = fopen(input, "rt")) ){ + return -1; + } + result = 1; + find_flag = 0; + while(result != 0 && find_flag < 2){ + i = 0; + d = 0; + memset(line, 0, 100); + while(d != '\n'){ + result = fread(&d, 1, 1, fp); + if(result == 0){ + line[0] = 0; + break; + } + sprintf(&line[i], "%c", d); + i ++; + } + if(line[0] == 'N'){ + i = 1; + while(line[i] != '"'){ + i++; + } + i++; + j = 0; + find_flag = 0; + if (mode == 0){ + while(j < CHIP_NUM){ + if(!memcmp(&line[i], chip_name[j], strlen(chip_name[j]))){ + find_flag = 1; + chip_ind = j; + } + j++; + } + } else if (mode != 0){ + if(!memcmp(&line[i], name, strlen(name))){ + find_flag = 1; + } + } + } + if(find_flag){ + if(mode == 0){ + if(line[0] == 'S'){ + memset(tmp, 0, 100); + i =1; + while(line[i] != '=') i++; + i++; + j = 0; + while(line[i] != '\n'){ + tmp[j] = line[i]; + i ++; j++; + } + sprintf(sysfs_path, "%s%s", "/sys", tmp); + find_flag++; + } + } else if(mode == 1){ + if(line[0] == 'H') { + i = 2; + while(line[i] != '=') i++; + while(line[i] != 't') i++; + i++; + event_number = 0; + while(line[i] != '\n'){ + if(line[i] >= '0' && line[i] <= '9') + event_number = event_number*10 + line[i]-0x30; + i ++; + } + find_flag ++; + } + } else if (mode == 2) { + if(line[0] == 'S'){ + memset(tmp, 0, 100); + i =1; + while(line[i] != '=') i++; + i++; + j = 0; + while(line[i] != '\n'){ + tmp[j] = line[i]; + i ++; j++; + } + input_number = 0; + if(tmp[j-2] >= '0' && tmp[j-2] <= '9') + input_number += (tmp[j-2]-0x30)*10; + if(tmp[j-1] >= '0' && tmp[j-1] <= '9') + input_number += (tmp[j-1]-0x30); + find_flag++; + } + } + } + } + fclose(fp); + if(find_flag == 0){ + return -1; + } + if(0 == mode) + status = 1; + if (mode == 1) + return event_number; + if (mode == 2) + return input_number; + return 0; + +} +static void init_iio() { + int i, j; + char iio_chip[10]; + int dev_num; + for(j=0; j< CHIP_NUM; j++) { + for (i=0; i= 0) { + iio_initialized = 1; + iio_dev_num = dev_num; + chip_ind = j; + } + } +} + +static int process_sysfs_request(enum PROC_SYSFS_CMD cmd, char *data) +{ + char key_path[100]; + FILE *fp; + int i, result; + if(initialized == 0){ + parsing_proc_input(0, NULL); + initialized = 1; + } + if(initialized && status == 0) { + init_iio(); + if (iio_initialized == 0) + return -1; + } + + memset(key_path, 0, 100); + switch(cmd){ + case CMD_GET_SYSFS_PATH: + if (iio_initialized == 1) + sprintf(data, "/sys/bus/iio/devices/iio:device%d", iio_dev_num); + else + sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu"); + break; + case CMD_GET_DMP_PATH: + if (iio_initialized == 1) + sprintf(data, "/sys/bus/iio/devices/iio:device%d/dmp_firmware", iio_dev_num); + else + sprintf(data, "%s%s", sysfs_path, "/device/invensense/mpu/dmp_firmware"); + break; + case CMD_GET_CHIP_NAME: + sprintf(data, "%s", chip_name[chip_ind]); + break; + case CMD_GET_TRIGGER_PATH: + sprintf(data, "/sys/bus/iio/devices/trigger%d", iio_dev_num); + break; + case CMD_GET_DEVICE_NODE: + sprintf(data, "/dev/iio:device%d", iio_dev_num); + break; + case CMD_GET_SYSFS_KEY: + memset(key_path, 0, 100); + if (iio_initialized == 1) + sprintf(key_path, "/sys/bus/iio/devices/iio:device%d/key", iio_dev_num); + else + sprintf(key_path, "%s%s", sysfs_path, "/device/invensense/mpu/key"); + + if((fp = fopen(key_path, "rt")) == NULL) + return -1; + for(i=0;i<16;i++){ + fscanf(fp, "%02x", &result); + data[i] = (char)result; + } + + fclose(fp); + break; + default: + break; + } + return 0; +} +/** + * @brief return sysfs key. if the key is not available + * return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the key + * It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_sysfs_key(unsigned char *key) +{ + if (process_sysfs_request(CMD_GET_SYSFS_KEY, (char*)key) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +/** + * @brief return the sysfs path. If the path is not + * found yet. return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the sysfs + * path. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_sysfs_path(char *name) +{ + if (process_sysfs_request(CMD_GET_SYSFS_PATH, name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +inv_error_t inv_get_sysfs_abs_path(char *name) +{ + strcpy(name, MPU_SYSFS_ABS_PATH); + return INV_SUCCESS; +} + +/** + * @brief return the dmp file path. If the path is not + * found yet. return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the dmp file + * path. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_dmpfile(char *name) +{ + if (process_sysfs_request(CMD_GET_DMP_PATH, name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} +/** + * @brief return the chip name. If the chip is not + * found yet. return false. So the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the chip name + * path(8 bytes). It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_chip_name(char *name) +{ + if (process_sysfs_request(CMD_GET_CHIP_NAME, name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} +/** + * @brief return event handler number. If the handler number is not found + * return false. the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the chip name + * path(8 bytes). It should be zeroed before calling this function. + * Or it could have unpredicable result. + * @int *num: event number store + */ +inv_error_t inv_get_handler_number(const char *name, int *num) +{ + initialized = 0; + if ((*num = parsing_proc_input(1, (char *)name)) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +/** + * @brief return input number. If the handler number is not found + * return false. the return value must be checked + * to make sure the path is valid. + * @unsigned char *name: This should be array big enough to hold the chip name + * path(8 bytes). It should be zeroed before calling this function. + * Or it could have unpredicable result. + * @int *num: input number store + */ +inv_error_t inv_get_input_number(const char *name, int *num) +{ + initialized = 0; + if ((*num = parsing_proc_input(2, (char *)name)) < 0) + return INV_ERROR_NOT_OPENED; + else { + return INV_SUCCESS; + } +} + +/** + * @brief return iio trigger name. If iio is not initialized, return false. + * So the return must be checked to make sure the numeber is valid. + * @unsigned char *name: This should be array big enough to hold the trigger + * name. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_iio_trigger_path(char *name) +{ + if (process_sysfs_request(CMD_GET_TRIGGER_PATH, (char *)name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} + +/** + * @brief return iio device node. If iio is not initialized, return false. + * So the return must be checked to make sure the numeber is valid. + * @unsigned char *name: This should be array big enough to hold the device + * node. It should be zeroed before calling this function. + * Or it could have unpredicable result. + */ +inv_error_t inv_get_iio_device_node(char *name) +{ + if (process_sysfs_request(CMD_GET_DEVICE_NODE, (char *)name) < 0) + return INV_ERROR_NOT_OPENED; + else + return INV_SUCCESS; +} diff --git a/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h new file mode 100644 index 0000000..9470874 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/ml_sysfs_helper.h @@ -0,0 +1,36 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id$ + * + ******************************************************************************/ + +#ifndef MLDMP_SYSFS_HELPER_H__ +#define MLDMP_SYSFS_HELPER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "invensense.h" + +int find_type_by_name(const char *name, const char *type); +inv_error_t inv_get_sysfs_path(char *name); +inv_error_t inv_get_sysfs_abs_path(char *name); +inv_error_t inv_get_dmpfile(char *name); +inv_error_t inv_get_chip_name(char *name); +inv_error_t inv_get_sysfs_key(unsigned char *key); +inv_error_t inv_get_handler_number(const char *name, int *num); +inv_error_t inv_get_input_number(const char *name, int *num); +inv_error_t inv_get_iio_trigger_path(char *name); +inv_error_t inv_get_iio_device_node(char *name); + +#ifdef __cplusplus +} +#endif +#endif /* MLDMP_SYSFS_HELPER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos.h b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h new file mode 100644 index 0000000..d4f8912 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos.h @@ -0,0 +1,99 @@ +/* + $License: + Copyright (C) 2011 InvenSense Corporation, All Rights Reserved. + $ + */ + +#ifndef _MLOS_H +#define _MLOS_H + +#ifndef __KERNEL__ +#include +#endif +#include + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(LINUX) || defined(__KERNEL__) +typedef pthread_mutex_t* HANDLE; +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /* - MLOSCreateFile defines. - */ + +#define MLOS_GENERIC_READ ((unsigned int)0x80000000) +#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) +#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) +#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) +#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) + + /* ---------- */ + /* - Enums. - */ + /* ---------- */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + +#ifndef __KERNEL__ +#include + void *inv_malloc(unsigned int numBytes); + inv_error_t inv_free(void *ptr); + inv_error_t inv_create_mutex(HANDLE *mutex); + inv_error_t inv_lock_mutex(HANDLE mutex); + inv_error_t inv_unlock_mutex(HANDLE mutex); + FILE *inv_fopen(char *filename); + void inv_fclose(FILE *fp); + + inv_error_t inv_destroy_mutex(HANDLE handle); + + void inv_sleep(int mSecs); + unsigned long inv_get_tick_count(void); + + /* Kernel implmentations */ +#define GFP_KERNEL (0x70) + static inline void *kmalloc(size_t size, + unsigned int gfp_flags) + { + (void)gfp_flags; + return inv_malloc((unsigned int)size); + } + static inline void *kzalloc(size_t size, unsigned int gfp_flags) + { + void *tmp = inv_malloc((unsigned int)size); + (void)gfp_flags; + if (tmp) + memset(tmp, 0, size); + return tmp; + } + static inline void kfree(void *ptr) + { + inv_free(ptr); + } + static inline void msleep(long msecs) + { + inv_sleep(msecs); + } + static inline void udelay(unsigned long usecs) + { + inv_sleep((usecs + 999) / 1000); + } +#else +#include +#endif + +#ifdef __cplusplus +} +#endif +#endif /* _MLOS_H */ diff --git a/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c new file mode 100644 index 0000000..5424508 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/linux/mlos_linux.c @@ -0,0 +1,190 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + ******************************************************************************/ + +/** + * @defgroup MLOS + * @brief OS Interface. + * + * @{ + * @file mlos.c + * @brief OS Interface. + */ + +/* ------------- */ +/* - Includes. - */ +/* ------------- */ + +#include +#include +#include +#include +#include + +#include "stdint_invensense.h" +#include "mlos.h" + + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Allocate space + * @param num_bytes number of bytes + * @return pointer to allocated space + */ +void *inv_malloc(unsigned int num_bytes) +{ + // Allocate space. + void *alloc_ptr = malloc(num_bytes); + return alloc_ptr; +} + + +/** + * @brief Free allocated space + * @param ptr pointer to space to deallocate + * @return error code. + */ +inv_error_t inv_free(void *ptr) +{ + if (ptr) + free(ptr); + return INV_SUCCESS; +} + + +/** + * @brief Mutex create function + * @param mutex pointer to mutex handle + * @return error code. + */ +inv_error_t inv_create_mutex(HANDLE *mutex) +{ + int res; + pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); + if(pm == NULL) + return INV_ERROR; + + res = pthread_mutex_init(pm, NULL); + if(res == -1) { + free(pm); + return INV_ERROR_OS_CREATE_FAILED; + } + + *mutex = (HANDLE)pm; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex lock function + * @param mutex Mutex handle + * @return error code. + */ +inv_error_t inv_lock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t *)mutex; + + res = pthread_mutex_lock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex unlock function + * @param mutex mutex handle + * @return error code. + */ +inv_error_t inv_unlock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t *)mutex; + + res = pthread_mutex_unlock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief open file + * @param filename name of the file to open. + * @return error code. + */ +FILE *inv_fopen(char *filename) +{ + FILE *fp = fopen(filename, "r"); + return fp; +} + + +/** + * @brief close the file. + * @param fp handle to file to close. + * @return error code. + */ +void inv_fclose(FILE *fp) +{ + fclose(fp); +} + +/** + * @brief Close Handle + * @param handle handle to the resource. + * @return Zero if success, an error code otherwise. + */ +inv_error_t inv_destroy_mutex(HANDLE handle) +{ + int error; + pthread_mutex_t *pm = (pthread_mutex_t *)handle; + error = pthread_mutex_destroy(pm); + if (error) + return errno; + free((void*) handle); + + return INV_SUCCESS;} + + +/** + * @brief Sleep function. + */ +void inv_sleep(int m_secs) +{ + usleep(m_secs * 1000); +} + + +/** + * @brief get system's internal tick count. + * Used for time reference. + * @return current tick count. + */ +unsigned long inv_get_tick_count() +{ + struct timeval tv; + + if (gettimeofday(&tv, NULL) != 0) + return 0; + + return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); +} + +/** @} */ + diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.c b/60xx/libsensors_iio/software/core/mllite/message_layer.c new file mode 100644 index 0000000..8317957 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/message_layer.c @@ -0,0 +1,59 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @defgroup Message_Layer message_layer + * @brief Motion Library - Message Layer + * Holds Low Occurance messages + * + * @{ + * @file message_layer.c + * @brief Holds Low Occurance Messages. + */ +#include "message_layer.h" +#include "log.h" + +struct message_holder_t { + long message; +}; + +static struct message_holder_t mh; + +/** Sets a message. +* @param[in] set The flags to set. +* @param[in] clear Before setting anything this will clear these messages, +* which is useful for mutually exclusive messages such +* a motion or no motion message. +* @param[in] level Level of the messages. It starts at 0, and may increase +* in the future to allow more messages if the bit storage runs out. +*/ +void inv_set_message(long set, long clear, int level) +{ + if (level == 0) { + mh.message &= ~clear; + mh.message |= set; + } +} + +/** Returns Message Flags for Level 0 Messages. +* Levels are to allow expansion of more messages in the future. +* @param[in] clear If set, will clear the message. Typically this will be set +* for one reader, so that you don't get the same message over and over. +* @return bit field to corresponding message. +*/ +long inv_get_message_level_0(int clear) +{ + long msg; + msg = mh.message; + if (clear) { + mh.message = 0; + } + return msg; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/message_layer.h b/60xx/libsensors_iio/software/core/mllite/message_layer.h new file mode 100644 index 0000000..df0c0e2 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/message_layer.h @@ -0,0 +1,35 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_MESSAGE_LAYER_H__ +#define INV_MESSAGE_LAYER_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + /* Level 0 Type Messages */ + /** A motion event has occured */ +#define INV_MSG_MOTION_EVENT (0x01) + /** A no motion event has occured */ +#define INV_MSG_NO_MOTION_EVENT (0x02) + /** A setting of the gyro bias has occured */ +#define INV_MSG_NEW_GB_EVENT (0x04) + /** A setting of the compass bias has occured */ +#define INV_MSG_NEW_CB_EVENT (0x08) + /** A setting of the accel bias has occured */ +#define INV_MSG_NEW_AB_EVENT (0x10) + + void inv_set_message(long set, long clear, int level); + long inv_get_message_level_0(int clear); + +#ifdef __cplusplus +} +#endif + +#endif // INV_MESSAGE_LAYER_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.c b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c new file mode 100644 index 0000000..c30d693 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.c @@ -0,0 +1,706 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/** + * @defgroup ML_MATH_FUNC ml_math_func + * @brief Motion Library - Math Functions + * Common math functions the Motion Library + * + * @{ + * @file ml_math_func.c + * @brief Math Functions. + */ + +#include "mlmath.h" +#include "ml_math_func.h" +#include "mlinclude.h" +#include + +/** @internal + * Does the cross product of compass by gravity, then converts that + * to the world frame using the quaternion, then computes the angle that + * is made. + * + * @param[in] compass Compass Vector (Body Frame), length 3 + * @param[in] grav Gravity Vector (Body Frame), length 3 + * @param[in] quat Quaternion, Length 4 + * @return Angle Cross Product makes after quaternion rotation. + */ +float inv_compass_angle(const long *compass, const long *grav, const float *quat) +{ + float cgcross[4], q1[4], q2[4], qi[4]; + float angW; + + // Compass cross Gravity + cgcross[0] = 0.f; + cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; + cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; + cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; + + // Now convert cross product into world frame + inv_q_multf(quat, cgcross, q1); + inv_q_invertf(quat, qi); + inv_q_multf(q1, qi, q2); + + // Protect against atan2 of 0,0 + if ((q2[2] == 0.f) && (q2[1] == 0.f)) + return 0.f; + + // This is the unfiltered heading correction + angW = -atan2f(q2[2], q2[1]); + return angW; +} + +/** + * @brief The gyro data magnitude squared : + * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. + * @param[in] gyro Gyro data scaled with 1 dps = 2^16 + * @return the computed magnitude squared output of the gyroscope. + */ +unsigned long inv_get_gyro_sum_of_sqr(const long *gyro) +{ + unsigned long gmag = 0; + long temp; + int kk; + + for (kk = 0; kk < 3; ++kk) { + temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2)); + gmag += temp * temp; + } + + return gmag; +} + +/** Performs a multiply and shift by 29. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>29 +*/ +long inv_q29_mult(long a, long b) +{ +#ifdef UMPL_ELIMINATE_64BIT + long result; + result = (long)((float)a * b / (1L << 29)); + return result; +#else + long long temp; + long result; + temp = (long long)a * b; + result = (long)(temp >> 29); + return result; +#endif +} + +/** Performs a multiply and shift by 30. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>30 +*/ +long inv_q30_mult(long a, long b) +{ +#ifdef UMPL_ELIMINATE_64BIT + long result; + result = (long)((float)a * b / (1L << 30)); + return result; +#else + long long temp; + long result; + temp = (long long)a * b; + result = (long)(temp >> 30); + return result; +#endif +} + +#ifndef UMPL_ELIMINATE_64BIT +long inv_q30_div(long a, long b) +{ + long long temp; + long result; + temp = (((long long)a) << 30) / b; + result = (long)temp; + return result; +} +#endif + +/** Performs a multiply and shift by shift. These are good functions to write + * in assembly on with devices with small memory where you want to get rid of + * the long long which some assemblers don't handle well + * @param[in] a First multicand + * @param[in] b Second multicand + * @param[in] shift Shift amount after multiplying + * @return ((long long)a*b)<> shift); + return result; +} +#endif + +/** Performs a fixed point quaternion multiply. +* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled +* to 2^30 +* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled +* to 2^30 +* @param[out] qProd Product after quaternion multiply. Length 4. +* 1.0 scaled to 2^30. +*/ +void inv_q_mult(const long *q1, const long *q2, long *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) - + inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]); + + qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) + + inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]); + + qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) + + inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]); + + qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) - + inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]); +} + +/** Performs a fixed point quaternion addition. +* @param[in] q1 First Quaternion term, length 4. 1.0 scaled +* to 2^30 +* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled +* to 2^30 +* @param[out] qSum Sum after quaternion summation. Length 4. +* 1.0 scaled to 2^30. +*/ +void inv_q_add(long *q1, long *q2, long *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_vector_normalize(long *vec, int length) +{ + INVENSENSE_FUNC_START; + double normSF = 0; + int ii; + for (ii = 0; ii < length; ii++) { + normSF += + inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]); + } + if (normSF > 0) { + normSF = 1 / sqrt(normSF); + for (ii = 0; ii < length; ii++) { + vec[ii] = (int)((double)vec[ii] * normSF); + } + } else { + vec[0] = 1073741824L; + for (ii = 1; ii < length; ii++) { + vec[ii] = 0; + } + } +} + +void inv_q_normalize(long *q) +{ + INVENSENSE_FUNC_START; + inv_vector_normalize(q, 4); +} + +void inv_q_invert(const long *q, long *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +double quaternion_to_rotation_angle(const long *quat) { + double quat0 = (double )quat[0] / 1073741824; + if (quat0 > 1.0f) { + quat0 = 1.0; + } else if (quat0 < -1.0f) { + quat0 = -1.0; + } + + return acos(quat0)*2*180/M_PI; +} + +/** Rotates a 3-element vector by Rotation defined by Q +*/ +void inv_q_rotate(const long *q, const long *in, long *out) +{ + long q_temp1[4], q_temp2[4]; + long in4[4], out4[4]; + + // Fixme optimize + in4[0] = 0; + memcpy(&in4[1], in, 3 * sizeof(long)); + inv_q_mult(q, in4, q_temp1); + inv_q_invert(q, q_temp2); + inv_q_mult(q_temp1, q_temp2, out4); + memcpy(out, &out4[1], 3 * sizeof(long)); +} + +void inv_q_multf(const float *q1, const float *q2, float *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = + (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); + qProd[1] = + (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); + qProd[2] = + (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); + qProd[3] = + (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); +} + +void inv_q_addf(const float *q1, const float *q2, float *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_q_normalizef(float *q) +{ + INVENSENSE_FUNC_START; + float normSF = 0; + float xHalf = 0; + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (normSF < 2) { + xHalf = 0.5f * normSF; + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + q[0] *= normSF; + q[1] *= normSF; + q[2] *= normSF; + q[3] *= normSF; + } else { + q[0] = 1.0; + q[1] = 0.0; + q[2] = 0.0; + q[3] = 0.0; + } + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); +} + +/** Performs a length 4 vector normalization with a square root. +* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero. +*/ +void inv_q_norm4(float *q) +{ + float mag; + mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (mag) { + q[0] /= mag; + q[1] /= mag; + q[2] /= mag; + q[3] /= mag; + } else { + q[0] = 1.f; + q[1] = 0.f; + q[2] = 0.f; + q[3] = 0.f; + } +} + +void inv_q_invertf(const float *q, float *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +/** + * Converts a quaternion to a rotation matrix. + * @param[in] quat 4-element quaternion in fixed point. One is 2^30. + * @param[out] rot Rotation matrix in fixed point. One is 2^30. The + * First 3 elements of the rotation matrix, represent + * the first row of the matrix. Rotation matrix multiplied + * by a 3 element column vector transform a vector from Body + * to World. + */ +void inv_quaternion_to_rotation(const long *quat, long *rot) +{ + rot[0] = + inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], + quat[0]) - + 1073741824L; + rot[1] = + inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); + rot[2] = + inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); + rot[3] = + inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); + rot[4] = + inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], + quat[0]) - + 1073741824L; + rot[5] = + inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); + rot[6] = + inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); + rot[7] = + inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); + rot[8] = + inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], + quat[0]) - + 1073741824L; +} + +/** + * Converts a quaternion to a rotation vector. A rotation vector is + * a method to represent a 4-element quaternion vector in 3-elements. + * To get the quaternion from the 3-elements, The last 3-elements of + * the quaternion will be the given rotation vector. The first element + * of the quaternion will be the positive value that will be required + * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units. + * @param[in] quat 4-element quaternion in fixed point. One is 2^30. + * @param[out] rot Rotation vector in fixed point. One is 2^30. + */ +void inv_quaternion_to_rotation_vector(const long *quat, long *rot) +{ + rot[0] = quat[1]; + rot[1] = quat[2]; + rot[2] = quat[3]; + + if (quat[0] < 0.0) { + rot[0] = -rot[0]; + rot[1] = -rot[1]; + rot[2] = -rot[2]; + } +} + +/** Converts a 32-bit long to a big endian byte stream */ +unsigned char *inv_int32_to_big8(long x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 24) & 0xff); + big8[1] = (unsigned char)((x >> 16) & 0xff); + big8[2] = (unsigned char)((x >> 8) & 0xff); + big8[3] = (unsigned char)(x & 0xff); + return big8; +} + +/** Converts a big endian byte stream into a 32-bit long */ +long inv_big8_to_int32(const unsigned char *big8) +{ + long x; + x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) + | ((long)big8[3]); + return x; +} + +/** Converts a big endian byte stream into a 16-bit integer (short) */ +short inv_big8_to_int16(const unsigned char *big8) +{ + short x; + x = ((short)big8[0] << 8) | ((short)big8[1]); + return x; +} + +/** Converts a little endian byte stream into a 16-bit integer (short) */ +short inv_little8_to_int16(const unsigned char *little8) +{ + short x; + x = ((short)little8[1] << 8) | ((short)little8[0]); + return x; +} + +/** Converts a 16-bit short to a big endian byte stream */ +unsigned char *inv_int16_to_big8(short x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 8) & 0xff); + big8[1] = (unsigned char)(x & 0xff); + return big8; +} + +void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 6 * k + l) = *(a + 6 * i + j); + } + } + *n = *n - 1; +} + +void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 6 * k + l) = *(a + 6 * i + j); + } + } + *n = *n - 1; +} + +float inv_matrix_det(float *p, int *n) +{ + float d[6][6], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 7) - *(p + 1) ** (p + 6)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_inc(p, &d[0][0], n, i, j); + sum = + sum + *(p + 6 * i + j) * SIGNM(i + + j) * + inv_matrix_det(&d[0][0], n); + } + + return (sum); +} + +double inv_matrix_detd(double *p, int *n) +{ + double d[6][6], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 7) - *(p + 1) ** (p + 6)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_incd(p, &d[0][0], n, i, j); + sum = + sum + *(p + 6 * i + j) * SIGNM(i + + j) * + inv_matrix_detd(&d[0][0], n); + } + + return (sum); +} + +/** Wraps angle from (-M_PI,M_PI] + * @param[in] ang Angle in radians to wrap + * @return Wrapped angle from (-M_PI,M_PI] + */ +float inv_wrap_angle(float ang) +{ + if (ang > M_PI) + return ang - 2 * (float)M_PI; + else if (ang <= -(float)M_PI) + return ang + 2 * (float)M_PI; + else + return ang; +} + +/** Finds the minimum angle difference ang1-ang2 such that difference + * is between [-M_PI,M_PI] + * @param[in] ang1 + * @param[in] ang2 + * @return angle difference ang1-ang2 + */ +float inv_angle_diff(float ang1, float ang2) +{ + float d; + ang1 = inv_wrap_angle(ang1); + ang2 = inv_wrap_angle(ang2); + d = ang1 - ang2; + if (d > M_PI) + d -= 2 * (float)M_PI; + else if (d < -(float)M_PI) + d += 2 * (float)M_PI; + return d; +} + +/** bernstein hash, derived from public domain source */ +uint32_t inv_checksum(const unsigned char *str, int len) +{ + uint32_t hash = 5381; + int i, c; + + for (i = 0; i < len; i++) { + c = *(str + i); + hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ + } + + return hash; +} + +static unsigned short inv_row_2_scale(const signed char *row) +{ + unsigned short b; + + if (row[0] > 0) + b = 0; + else if (row[0] < 0) + b = 4; + else if (row[1] > 0) + b = 1; + else if (row[1] < 0) + b = 5; + else if (row[2] > 0) + b = 2; + else if (row[2] < 0) + b = 6; + else + b = 7; // error + return b; +} + + +/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation. +* @param[in] mtx Orientation matrix to convert to a scalar. +* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the +* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent +* the column the one is on for the second row with bit number 5 being the sign. +* The next 2 bits (6 and 7) represent the column the one is on for the third row with +* bit number 8 being the sign. In binary the identity matrix would therefor be: +* 010_001_000 or 0x88 in hex. +*/ +unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) +{ + + unsigned short scalar; + + /* + XYZ 010_001_000 Identity Matrix + XZY 001_010_000 + YXZ 010_000_001 + YZX 000_010_001 + ZXY 001_000_010 + ZYX 000_001_010 + */ + + scalar = inv_row_2_scale(mtx); + scalar |= inv_row_2_scale(mtx + 3) << 3; + scalar |= inv_row_2_scale(mtx + 6) << 6; + + + return scalar; +} + +/** Uses the scalar orientation value to convert from chip frame to body frame +* @param[in] orientation A scalar that represent how to go from chip to body frame +* @param[in] input Input vector, length 3 +* @param[out] output Output vector, length 3 +*/ +void inv_convert_to_body(unsigned short orientation, const long *input, long *output) +{ + output[0] = input[orientation & 0x03] * SIGNSET(orientation & 0x004); + output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020); + output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100); +} + +/** Uses the scalar orientation value to convert from body frame to chip frame +* @param[in] orientation A scalar that represent how to go from chip to body frame +* @param[in] input Input vector, length 3 +* @param[out] output Output vector, length 3 +*/ +void inv_convert_to_chip(unsigned short orientation, const long *input, long *output) +{ + output[orientation & 0x03] = input[0] * SIGNSET(orientation & 0x004); + output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020); + output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100); +} + + +/** Uses the scalar orientation value to convert from chip frame to body frame and +* apply appropriate scaling. +* @param[in] orientation A scalar that represent how to go from chip to body frame +* @param[in] sensitivity Sensitivity scale +* @param[in] input Input vector, length 3 +* @param[out] output Output vector, length 3 +*/ +void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output) +{ + output[0] = inv_q30_mult(input[orientation & 0x03] * + SIGNSET(orientation & 0x004), sensitivity); + output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] * + SIGNSET(orientation & 0x020), sensitivity); + output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] * + SIGNSET(orientation & 0x100), sensitivity); +} + +/** find a norm for a vector +* @param[in] x a vector [3x1] +* @return the normalize vector. +*/ +double inv_vector_norm(const float *x) +{ + return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]); +} + +void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) { + int i; + // initial state to zero + pFilter->state[0] = 0; + pFilter->state[1] = 0; + + // set up coefficients + for (i=0; i<5; i++) { + pFilter->c[i] = pBiquadCoeff[i]; + } +} + +void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input) +{ + pFilter->input = input; + pFilter->output = input; + pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]); + pFilter->state[1] = pFilter->state[0]; +} + +float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input) { + float stateZero; + + pFilter->input = input; + // calculate the new state; + stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0] + - pFilter->c[3]*pFilter->state[1]; + + pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0] + + pFilter->c[1]*pFilter->state[1]; + + // update the output and state + pFilter->output = pFilter->output * pFilter->c[4]; + pFilter->state[1] = pFilter->state[0]; + pFilter->state[0] = stateZero; + return pFilter->output; +} + +void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]) { + + cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1]; + cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2]; + cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0]; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/ml_math_func.h b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h new file mode 100644 index 0000000..535d849 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/ml_math_func.h @@ -0,0 +1,120 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INVENSENSE_INV_MATH_FUNC_H__ +#define INVENSENSE_INV_MATH_FUNC_H__ + +#include "mltypes.h" + +#define GYRO_MAG_SQR_SHIFT 6 +#define NUM_ROTATION_MATRIX_ELEMENTS (9) +#define ROT_MATRIX_SCALE_LONG (1073741824L) +#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) +#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ + ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) +#define SIGNM(k)((int)(k)&1?-1:1) +#define SIGNSET(x) ((x) ? -1 : +1) + +#define INV_TWO_POWER_NEG_30 9.313225746154785e-010f + +#ifdef __cplusplus +extern "C" { +#endif + + typedef struct { + float state[4]; + float c[5]; + float input; + float output; + } inv_biquad_filter_t; + + static inline float inv_q30_to_float(long q30) + { + return (float) q30 / ((float)(1L << 30)); + } + + static inline double inv_q30_to_double(long q30) + { + return (double) q30 / ((double)(1L << 30)); + } + + static inline float inv_q16_to_float(long q16) + { + return (float) q16 / (1L << 16); + } + + static inline double inv_q16_to_double(long q16) + { + return (double) q16 / (1L << 16); + } + + + + + long inv_q29_mult(long a, long b); + long inv_q30_mult(long a, long b); + + /* UMPL_ELIMINATE_64BIT Notes: + * An alternate implementation using float instead of long long accudoublemulators + * is provided for q29_mult and q30_mult. + * When long long accumulators are used and an alternate implementation is not + * available, we eliminate the entire function and header with a macro. + */ +#ifndef UMPL_ELIMINATE_64BIT + long inv_q30_div(long a, long b); + long inv_q_shift_mult(long a, long b, int shift); +#endif + + void inv_q_mult(const long *q1, const long *q2, long *qProd); + void inv_q_add(long *q1, long *q2, long *qSum); + void inv_q_normalize(long *q); + void inv_q_invert(const long *q, long *qInverted); + void inv_q_multf(const float *q1, const float *q2, float *qProd); + void inv_q_addf(const float *q1, const float *q2, float *qSum); + void inv_q_normalizef(float *q); + void inv_q_norm4(float *q); + void inv_q_invertf(const float *q, float *qInverted); + void inv_quaternion_to_rotation(const long *quat, long *rot); + unsigned char *inv_int32_to_big8(long x, unsigned char *big8); + long inv_big8_to_int32(const unsigned char *big8); + short inv_big8_to_int16(const unsigned char *big8); + short inv_little8_to_int16(const unsigned char *little8); + unsigned char *inv_int16_to_big8(short x, unsigned char *big8); + float inv_matrix_det(float *p, int *n); + void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); + double inv_matrix_detd(double *p, int *n); + void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); + float inv_wrap_angle(float ang); + float inv_angle_diff(float ang1, float ang2); + void inv_quaternion_to_rotation_vector(const long *quat, long *rot); + unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); + void inv_convert_to_body(unsigned short orientation, const long *input, long *output); + void inv_convert_to_chip(unsigned short orientation, const long *input, long *output); + void inv_convert_to_body_with_scale(unsigned short orientation, long sensitivity, const long *input, long *output); + void inv_q_rotate(const long *q, const long *in, long *out); + void inv_vector_normalize(long *vec, int length); + uint32_t inv_checksum(const unsigned char *str, int len); + float inv_compass_angle(const long *compass, const long *grav, + const float *quat); + unsigned long inv_get_gyro_sum_of_sqr(const long *gyro); + + static inline long inv_delta_time_ms(inv_time_t t1, inv_time_t t2) + { + return (long)((t1 - t2) / 1000000L); + } + + double quaternion_to_rotation_angle(const long *quat); + double inv_vector_norm(const float *x); + + void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff); + float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input); + void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input); + void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3]); + +#ifdef __cplusplus +} +#endif +#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.c b/60xx/libsensors_iio/software/core/mllite/mpl.c new file mode 100644 index 0000000..9141cc6 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/mpl.c @@ -0,0 +1,72 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @defgroup MPL mpl + * @brief Motion Library - Start Point + * Initializes MPL. + * + * @{ + * @file mpl.c + * @brief MPL start point. + */ + +#include "storage_manager.h" +#include "log.h" +#include "mpl.h" +#include "start_manager.h" +#include "data_builder.h" +#include "results_holder.h" +#include "mlinclude.h" + +/** + * @brief Initializes the MPL. Should be called first and once + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_init_mpl(void) +{ + inv_init_storage_manager(); + + /* initialize the start callback manager */ + INV_ERROR_CHECK(inv_init_start_manager()); + + /* initialize the data builder */ + INV_ERROR_CHECK(inv_init_data_builder()); + + INV_ERROR_CHECK(inv_enable_results_holder()); + + return INV_SUCCESS; +} + +const char ml_ver[] = "InvenSense MPL 5.1.2 beta RC9"; + +/** + * @brief used to get the MPL version. + * @param version a string where the MPL version gets stored. + * @return INV_SUCCESS if successful or a non-zero error code otherwise. + */ +inv_error_t inv_get_version(char **version) +{ + INVENSENSE_FUNC_START; + /* cast out the const */ + *version = (char *)&ml_ver; + return INV_SUCCESS; +} + +/** + * @brief Starts the MPL. Typically called after inv_init_mpl() or after a + * inv_stop_mpl() to start the MPL back up an running. + * @return INV_SUCCESS if successful or a non-zero error code otherwise. + */ +inv_error_t inv_start_mpl(void) +{ + INV_ERROR_CHECK(inv_execute_mpl_start_notification()); + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/mpl.h b/60xx/libsensors_iio/software/core/mllite/mpl.h new file mode 100644 index 0000000..a6b5ac7 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/mpl.h @@ -0,0 +1,24 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_MPL_H__ +#define INV_MPL_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_init_mpl(void); +inv_error_t inv_start_mpl(void); +inv_error_t inv_get_version(char **version); + +#ifdef __cplusplus +} +#endif + +#endif // INV_MPL_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/results_holder.c b/60xx/libsensors_iio/software/core/mllite/results_holder.c new file mode 100644 index 0000000..df58f40 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/results_holder.c @@ -0,0 +1,522 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +/** + * @defgroup Results_Holder results_holder + * @brief Motion Library - Results Holder + * Holds the data for MPL + * + * @{ + * @file results_holder.c + * @brief Results Holder for HAL. + */ + +#include + +#include "results_holder.h" +#include "ml_math_func.h" +#include "mlmath.h" +#include "start_manager.h" +#include "data_builder.h" +#include "message_layer.h" +#include "log.h" + +// These 2 status bits are used to control when the 9 axis quaternion is updated +#define INV_COMPASS_CORRECTION_SET 1 +#define INV_6_AXIS_QUAT_SET 2 + +struct results_t { + long nav_quat[4]; + long gam_quat[4]; + inv_time_t nav_timestamp; + inv_time_t gam_timestamp; + long local_field[3]; /**< local earth's magnetic field */ + long mag_scale[3]; /**< scale factor to apply to magnetic field reading */ + long compass_correction[4]; /**< quaternion going from gyro,accel quaternion to 9 axis */ + int acc_state; /**< Describes accel state */ + int got_accel_bias; /**< Flag describing if accel bias is known */ + long compass_bias_error[3]; /**< Error Squared */ + unsigned char motion_state; + unsigned int motion_state_counter; /**< Incremented for each no motion event in a row */ + long compass_count; /**< compass state internal counter */ + int got_compass_bias; /**< Flag describing if compass bias is known */ + int large_mag_field; /**< Flag describing if there is a large magnetic field */ + int compass_state; /**< Internal compass state */ + long status; + struct inv_sensor_cal_t *sensor; + float quat_confidence_interval; +}; +static struct results_t rh; + +/** @internal +* Store a quaternion more suitable for gaming. This quaternion is often determined +* using only gyro and accel. +* @param[in] quat Length 4, Quaternion scaled by 2^30 +*/ +void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp) +{ + rh.status |= INV_6_AXIS_QUAT_SET; + memcpy(&rh.gam_quat, quat, sizeof(rh.gam_quat)); + rh.gam_timestamp = timestamp; +} + +/** @internal +* Sets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. +* @param[in] data Quaternion Adjustment +* @param[in] timestamp Timestamp of when this is valid +*/ +void inv_set_compass_correction(const long *data, inv_time_t timestamp) +{ + rh.status |= INV_COMPASS_CORRECTION_SET; + memcpy(rh.compass_correction, data, sizeof(rh.compass_correction)); + rh.nav_timestamp = timestamp; +} + +/** @internal +* Gets the quaternion adjustment from 6 axis (accel, gyro) to 9 axis quaternion. +* @param[out] data Quaternion Adjustment +* @param[out] timestamp Timestamp of when this is valid +*/ +void inv_get_compass_correction(long *data, inv_time_t *timestamp) +{ + memcpy(data, rh.compass_correction, sizeof(rh.compass_correction)); + *timestamp = rh.nav_timestamp; +} + +/** Returns non-zero if there is a large magnetic field. See inv_set_large_mag_field() for setting this variable. + * @return Returns non-zero if there is a large magnetic field. + */ +int inv_get_large_mag_field() +{ + return rh.large_mag_field; +} + +/** Set to non-zero if there as a large magnetic field. See inv_get_large_mag_field() for getting this variable. + * @param[in] state value to set for magnetic field strength. Should be non-zero if it is large. + */ +void inv_set_large_mag_field(int state) +{ + rh.large_mag_field = state; +} + +/** Gets the accel state set by inv_set_acc_state() + * @return accel state. + */ +int inv_get_acc_state() +{ + return rh.acc_state; +} + +/** Sets the accel state. See inv_get_acc_state() to get the value. + * @param[in] state value to set accel state to. + */ +void inv_set_acc_state(int state) +{ + rh.acc_state = state; + return; +} + +/** Returns the motion state +* @param[out] cntr Number of previous times a no motion event has occured in a row. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +int inv_get_motion_state(unsigned int *cntr) +{ + *cntr = rh.motion_state_counter; + return rh.motion_state; +} + +/** Sets the motion state + * @param[in] state motion state where INV_NO_MOTION is not moving + * and INV_MOTION is moving. + */ +void inv_set_motion_state(unsigned char state) +{ + long set; + if (state == rh.motion_state) { + if (state == INV_NO_MOTION) { + rh.motion_state_counter++; + } else { + rh.motion_state_counter = 0; + } + return; + } + rh.motion_state_counter = 0; + rh.motion_state = state; + /* Equivalent to set = state, but #define's may change. */ + if (state == INV_MOTION) + set = INV_MSG_MOTION_EVENT; + else + set = INV_MSG_NO_MOTION_EVENT; + inv_set_message(set, (INV_MSG_MOTION_EVENT | INV_MSG_NO_MOTION_EVENT), 0); +} + +/** Sets the local earth's magnetic field +* @param[in] data Local earth's magnetic field in uT scaled by 2^16. +* Length = 3. Y typically points north, Z typically points down in +* northern hemisphere and up in southern hemisphere. +*/ +void inv_set_local_field(const long *data) +{ + memcpy(rh.local_field, data, sizeof(rh.local_field)); +} + +/** Gets the local earth's magnetic field +* @param[out] data Local earth's magnetic field in uT scaled by 2^16. +* Length = 3. Y typically points north, Z typically points down in +* northern hemisphere and up in southern hemisphere. +*/ +void inv_get_local_field(long *data) +{ + memcpy(data, rh.local_field, sizeof(rh.local_field)); +} + +/** Sets the compass sensitivity + * @param[in] data Length 3, sensitivity for each compass axis + * scaled such that 1.0 = 2^30. + */ +void inv_set_mag_scale(const long *data) +{ + memcpy(rh.mag_scale, data, sizeof(rh.mag_scale)); +} + +/** Gets the compass sensitivity + * @param[out] data Length 3, sensitivity for each compass axis + * scaled such that 1.0 = 2^30. + */ +void inv_get_mag_scale(long *data) +{ + memcpy(data, rh.mag_scale, sizeof(rh.mag_scale)); +} + +/** Gets gravity vector + * @param[out] data gravity vector in body frame scaled such that 1.0 = 2^30. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_get_gravity(long *data) +{ + data[0] = + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[3]) - inv_q29_mult(rh.nav_quat[2], rh.nav_quat[0]); + data[1] = + inv_q29_mult(rh.nav_quat[2], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[1], rh.nav_quat[0]); + data[2] = + (inv_q29_mult(rh.nav_quat[3], rh.nav_quat[3]) + inv_q29_mult(rh.nav_quat[0], rh.nav_quat[0])) - + 1073741824L; + + return INV_SUCCESS; +} + +/** Returns a quaternion based only on gyro and accel. + * @param[out] data 6-axis gyro and accel quaternion scaled such that 1.0 = 2^30. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_get_6axis_quaternion(long *data) +{ + memcpy(data, rh.gam_quat, sizeof(rh.gam_quat)); + return INV_SUCCESS; +} + +/** Returns a quaternion. + * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_get_quaternion(long *data) +{ + if (rh.status & (INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET)) { + inv_q_mult(rh.compass_correction, rh.gam_quat, rh.nav_quat); + rh.status &= ~(INV_COMPASS_CORRECTION_SET | INV_6_AXIS_QUAT_SET); + } + memcpy(data, rh.nav_quat, sizeof(rh.nav_quat)); + return INV_SUCCESS; +} + +/** Returns a quaternion. + * @param[out] data 9-axis quaternion. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_get_quaternion_float(float *data) +{ + long ldata[4]; + inv_error_t result = inv_get_quaternion(ldata); + data[0] = inv_q30_to_float(ldata[0]); + data[1] = inv_q30_to_float(ldata[1]); + data[2] = inv_q30_to_float(ldata[2]); + data[3] = inv_q30_to_float(ldata[3]); + return result; +} + +/** Returns a quaternion with accuracy and timestamp. + * @param[out] data 9-axis quaternion scaled such that 1.0 = 2^30. + * @param[out] accuracy Accuracy of quaternion, 0-3, where 3 is most accurate. + * @param[out] timestamp Timestamp of this quaternion in nanoseconds + */ +void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp) +{ + inv_get_quaternion(data); + *timestamp = inv_get_last_timestamp(); + if (inv_get_compass_on()) { + *accuracy = inv_get_mag_accuracy(); + } else if (inv_get_gyro_on()) { + *accuracy = inv_get_gyro_accuracy(); + }else if (inv_get_accel_on()) { + *accuracy = inv_get_accel_accuracy(); + } else { + *accuracy = 0; + } +} + +/** Callback that gets called everytime there is new data. It is + * registered by inv_start_results_holder(). + * @param[in] sensor_cal New sensor data to process. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_generate_results(struct inv_sensor_cal_t *sensor_cal) +{ + rh.sensor = sensor_cal; + return INV_SUCCESS; +} + +/** Function to turn on this module. This is automatically called by + * inv_enable_results_holder(). Typically not called by users. + * @return Returns INV_SUCCESS if successful or an error code if not. + */ +inv_error_t inv_start_results_holder(void) +{ + inv_register_data_cb(inv_generate_results, INV_PRIORITY_RESULTS_HOLDER, + INV_GYRO_NEW | INV_ACCEL_NEW | INV_MAG_NEW); + return INV_SUCCESS; +} + +/** Initializes results holder. This is called automatically by the +* enable function inv_enable_results_holder(). It may be called any time the feature is enabled, but +* is typically not needed to be called by outside callers. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_init_results_holder(void) +{ + memset(&rh, 0, sizeof(rh)); + rh.mag_scale[0] = 1L<<30; + rh.mag_scale[1] = 1L<<30; + rh.mag_scale[2] = 1L<<30; + rh.compass_correction[0] = 1L<<30; + rh.gam_quat[0] = 1L<<30; + rh.nav_quat[0] = 1L<<30; + rh.quat_confidence_interval = (float)M_PI; + return INV_SUCCESS; +} + +/** Turns on storage of results. +*/ +inv_error_t inv_enable_results_holder() +{ + inv_error_t result; + result = inv_init_results_holder(); + if ( result ) { + return result; + } + + result = inv_register_mpl_start_notification(inv_start_results_holder); + return result; +} + +/** Sets state of if we know the accel bias. + * @return return 1 if we know the accel bias, 0 if not. + * it is set with inv_set_accel_bias_found() + */ +int inv_got_accel_bias() +{ + return rh.got_accel_bias; +} + +/** Sets whether we know the accel bias + * @param[in] state Set to 1 if we know the accel bias. + * Can be retrieved with inv_got_accel_bias() + */ +void inv_set_accel_bias_found(int state) +{ + rh.got_accel_bias = state; +} + +/** Sets state of if we know the compass bias. + * @return return 1 if we know the compass bias, 0 if not. + * it is set with inv_set_compass_bias_found() + */ +int inv_got_compass_bias() +{ + return rh.got_compass_bias; +} + +/** Sets whether we know the compass bias + * @param[in] state Set to 1 if we know the compass bias. + * Can be retrieved with inv_got_compass_bias() + */ +void inv_set_compass_bias_found(int state) +{ + rh.got_compass_bias = state; +} + +/** Sets the compass state. + * @param[in] state Compass state. It can be retrieved with inv_get_compass_state(). + */ +void inv_set_compass_state(int state) +{ + rh.compass_state = state; +} + +/** Get's the compass state + * @return the compass state that was set with inv_set_compass_state() + */ +int inv_get_compass_state() +{ + return rh.compass_state; +} + +/** Set compass bias error. See inv_get_compass_bias_error() + * @param[in] bias_error Set's how accurate we know the compass bias. It is the + * error squared. + */ +void inv_set_compass_bias_error(const long *bias_error) +{ + memcpy(rh.compass_bias_error, bias_error, sizeof(rh.compass_bias_error)); +} + +/** Get's compass bias error. See inv_set_compass_bias_error() for setting. + * @param[out] bias_error Accuracy as to how well the compass bias is known. It is the error squared. + */ +void inv_get_compass_bias_error(long *bias_error) +{ + memcpy(bias_error, rh.compass_bias_error, sizeof(rh.compass_bias_error)); +} + +/** + * @brief Returns 3-element vector of accelerometer data in body frame + * with gravity removed + * @param[out] data 3-element vector of accelerometer data in body frame + * with gravity removed + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_linear_accel(long *data) +{ + long gravity[3]; + + if (data != NULL) + { + inv_get_accel_set(data, NULL, NULL); + inv_get_gravity(gravity); + data[0] -= gravity[0] >> 14; + data[1] -= gravity[1] >> 14; + data[2] -= gravity[2] >> 14; + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** + * @brief Returns 3-element vector of accelerometer data in body frame + * @param[out] data 3-element vector of accelerometer data in body frame + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_accel(long *data) +{ + if (data != NULL) { + inv_get_accel_set(data, NULL, NULL); + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** + * @brief Returns 3-element vector of accelerometer float data + * @param[out] data 3-element vector of accelerometer float data + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_accel_float(float *data) +{ + long tdata[3]; + unsigned char i; + + if (data != NULL && !inv_get_accel(tdata)) { + for (i = 0; i < 3; ++i) { + data[i] = ((float)tdata[i] / (1L << 16)); + } + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** + * @brief Returns 3-element vector of gyro float data + * @param[out] data 3-element vector of gyro float data + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_gyro_float(float *data) +{ + long tdata[3]; + unsigned char i; + + if (data != NULL) { + inv_get_gyro_set(tdata, NULL, NULL); + for (i = 0; i < 3; ++i) { + data[i] = ((float)tdata[i] / (1L << 16)); + } + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** Set 9 axis 95% heading confidence interval for quaternion +* @param[in] ci Confidence interval in radians. +*/ +void inv_set_heading_confidence_interval(float ci) +{ + rh.quat_confidence_interval = ci; +} + +/** Get 9 axis 95% heading confidence interval for quaternion +* @return Confidence interval in radians. +*/ +float inv_get_heading_confidence_interval(void) +{ + return rh.quat_confidence_interval; +} + +/** + * @brief Returns 3-element vector of linear accel float data + * @param[out] data 3-element vector of linear aceel float data + * @return INV_SUCCESS if successful + * INV_ERROR_INVALID_PARAMETER if invalid input pointer + */ +inv_error_t inv_get_linear_accel_float(float *data) +{ + long tdata[3]; + unsigned char i; + + if (data != NULL && !inv_get_linear_accel(tdata)) { + for (i = 0; i < 3; ++i) { + data[i] = ((float)tdata[i] / (1L << 16)); + } + return INV_SUCCESS; + } + else { + return INV_ERROR_INVALID_PARAMETER; + } +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/results_holder.h b/60xx/libsensors_iio/software/core/mllite/results_holder.h new file mode 100644 index 0000000..09da24e --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/results_holder.h @@ -0,0 +1,81 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_RESULTS_HOLDER_H__ +#define INV_RESULTS_HOLDER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#define INV_MOTION 0x0001 +#define INV_NO_MOTION 0x0002 + + /**************************************************************************/ + /* The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = */ + /* 2^GYRO_MAG_SQR_SHIFT. This number must be >=0 and even. */ + /* The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = */ + /* 2^ACC_MAG_SQR_SHIFT */ + /**************************************************************************/ +#define ACC_MAG_SQR_SHIFT 16 + +void inv_store_gaming_quaternion(const long *quat, inv_time_t timestamp); + +// States +#define SF_NORMAL 0 +#define SF_UNCALIBRATED 1 +#define SF_STARTUP_SETTLE 2 +#define SF_FAST_SETTLE 3 +#define SF_DISTURBANCE 4 +#define SF_SLOW_SETTLE 5 + +int inv_get_acc_state(); +void inv_set_acc_state(int state); +int inv_get_motion_state(unsigned int *cntr); +void inv_set_motion_state(unsigned char state); +inv_error_t inv_get_gravity(long *data); +inv_error_t inv_get_6axis_quaternion(long *data); +inv_error_t inv_get_quaternion(long *data); +inv_error_t inv_get_quaternion_float(float *data); +void inv_get_quaternion_set(long *data, int *accuracy, inv_time_t *timestamp); + +inv_error_t inv_enable_results_holder(); +inv_error_t inv_init_results_holder(void); + +/* Magnetic Field Parameters*/ +void inv_set_local_field(const long *data); +void inv_get_local_field(long *data); +void inv_set_mag_scale(const long *data); +void inv_get_mag_scale(long *data); +void inv_set_compass_correction(const long *data, inv_time_t timestamp); +void inv_get_compass_correction(long *data, inv_time_t *timestamp); +int inv_got_compass_bias(); +void inv_set_compass_bias_found(int state); +int inv_get_large_mag_field(); +void inv_set_large_mag_field(int state); +void inv_set_compass_state(int state); +int inv_get_compass_state(); +void inv_set_compass_bias_error(const long *bias_error); +void inv_get_compass_bias_error(long *bias_error); +inv_error_t inv_get_linear_accel(long *data); +inv_error_t inv_get_accel(long *data); +inv_error_t inv_get_accel_float(float *data); +inv_error_t inv_get_gyro_float(float *data); +inv_error_t inv_get_linear_accel_float(float *data); +void inv_set_heading_confidence_interval(float ci); +float inv_get_heading_confidence_interval(void); + +int inv_got_accel_bias(); +void inv_set_accel_bias_found(int state); + + +#ifdef __cplusplus +} +#endif + +#endif // INV_RESULTS_HOLDER_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.c b/60xx/libsensors_iio/software/core/mllite/start_manager.c new file mode 100644 index 0000000..fb758e7 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/start_manager.c @@ -0,0 +1,105 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ +/** + * @defgroup Start_Manager start_manager + * @brief Motion Library - Start Manager + * Start Manager + * + * @{ + * @file start_manager.c + * @brief This handles all the callbacks when inv_start_mpl() is called. + */ + + +#include +#include "log.h" +#include "start_manager.h" + +typedef inv_error_t (*inv_start_cb_func)(); +struct inv_start_cb_t { + int num_cb; + inv_start_cb_func start_cb[INV_MAX_START_CB]; +}; + +static struct inv_start_cb_t inv_start_cb; + +/** Initilize the start manager. Typically called by inv_start_mpl(); +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_init_start_manager(void) +{ + memset(&inv_start_cb, 0, sizeof(inv_start_cb)); + return INV_SUCCESS; +} + +/** Removes a callback from start notification +* @param[in] start_cb function to remove from start notification +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void)) +{ + int kk; + + for (kk=0; kk= INV_MAX_START_CB) + return INV_ERROR_INVALID_PARAMETER; + + inv_start_cb.start_cb[inv_start_cb.num_cb] = start_cb; + inv_start_cb.num_cb++; + return INV_SUCCESS; +} + +/** Callback all the functions that want to be notified when inv_start_mpl() was +* called. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_execute_mpl_start_notification(void) +{ + inv_error_t result,first_error; + int kk; + + first_error = INV_SUCCESS; + + for (kk = 0; kk < inv_start_cb.num_cb; ++kk) { + result = inv_start_cb.start_cb[kk](); + if (result && (first_error == INV_SUCCESS)) { + first_error = result; + } + } + return first_error; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/start_manager.h b/60xx/libsensors_iio/software/core/mllite/start_manager.h new file mode 100644 index 0000000..899e3f5 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/start_manager.h @@ -0,0 +1,27 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_START_MANAGER_H__ +#define INV_START_MANAGER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" + +/** Max number of start callbacks we can handle. */ +#define INV_MAX_START_CB 20 + +inv_error_t inv_init_start_manager(void); +inv_error_t inv_register_mpl_start_notification(inv_error_t (*start_cb)(void)); +inv_error_t inv_execute_mpl_start_notification(void); +inv_error_t inv_unregister_mpl_start_notification(inv_error_t (*start_cb)(void)); + +#ifdef __cplusplus +} +#endif +#endif // INV_START_MANAGER_H__ diff --git a/60xx/libsensors_iio/software/core/mllite/storage_manager.c b/60xx/libsensors_iio/software/core/mllite/storage_manager.c new file mode 100644 index 0000000..3e4e619 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/storage_manager.c @@ -0,0 +1,204 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/** + * @defgroup Storage_Manager storage_manager + * @brief Motion Library - Stores Data for functions. + * + * + * @{ + * @file storage_manager.c + * @brief Load and Store Manager. + */ + +#include + +#include "storage_manager.h" +#include "log.h" +#include "ml_math_func.h" +#include "mlmath.h" + +/* Must be changed if the format of storage changes */ +#define DEFAULT_KEY 29681 + +typedef inv_error_t (*load_func_t)(const unsigned char *data); +typedef inv_error_t (*save_func_t)(unsigned char *data); +/** Max number of entites that can be stored */ +#define NUM_STORAGE_BOXES 20 + +struct data_header_t { + long size; + uint32_t checksum; + unsigned int key; +}; + +struct data_storage_t { + int num; /**< Number of differnt save entities */ + size_t total_size; /**< Size in bytes to store non volatile data */ + load_func_t load[NUM_STORAGE_BOXES]; /**< Callback to load data */ + save_func_t save[NUM_STORAGE_BOXES]; /**< Callback to save data */ + struct data_header_t hd[NUM_STORAGE_BOXES]; /**< Header info for each entity */ +}; +static struct data_storage_t ds; + +/** Should be called once before using any of the storage methods. Typically +* called first by inv_init_mpl().*/ +void inv_init_storage_manager() +{ + memset(&ds, 0, sizeof(ds)); + ds.total_size = sizeof(struct data_header_t); +} + +/** Used to register your mechanism to load and store non-volative data. This should typical be +* called during the enable function for your feature. +* @param[in] load_func function pointer you will use to receive data that was stored for you. +* @param[in] save_func function pointer you will use to save any data you want saved to +* non-volatile memory between runs. +* @param[in] size The size in bytes of the amount of data you want loaded and saved. +* @param[in] key The key associated with your data type should be unique across MPL. +* The key should change when your type of data for storage changes. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_register_load_store(inv_error_t (*load_func)(const unsigned char *data), + inv_error_t (*save_func)(unsigned char *data), size_t size, unsigned int key) +{ + int kk; + // Check if this has been registered already + for (kk=0; kk= NUM_STORAGE_BOXES) { + return INV_ERROR_INVALID_PARAMETER; + } + // Add to list + ds.hd[ds.num].key = key; + ds.hd[ds.num].size = size; + ds.load[ds.num] = load_func; + ds.save[ds.num] = save_func; + ds.total_size += size + sizeof(struct data_header_t); + ds.num++; + + return INV_SUCCESS; +} + +/** Returns the memory size needed to perform a store +* @param[out] size Size in bytes of memory needed to store. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_get_mpl_state_size(size_t *size) +{ + *size = ds.total_size; + return INV_SUCCESS; +} + +/** @internal + * Finds key in ds.hd[] array and returns location + * @return location where key exists in array, -1 if not found. + */ +static int inv_find_entry(unsigned int key) +{ + int kk; + for (kk=0; kkkey != DEFAULT_KEY) + return INV_ERROR_CALIBRATION_LOAD; // Key changed or data corruption + len = MIN(hd->size, len); + len = hd->size; + len -= sizeof(struct data_header_t); + data += sizeof(struct data_header_t); + checksum = inv_checksum(data, len); + if (checksum != hd->checksum) + return INV_ERROR_CALIBRATION_LOAD; // Data corruption + + while (len > (long)sizeof(struct data_header_t)) { + hd = (struct data_header_t *)data; + entry = inv_find_entry(hd->key); + data += sizeof(struct data_header_t); + len -= sizeof(struct data_header_t); + if (entry >= 0 && len >= hd->size) { + if (hd->size == ds.hd[entry].size) { + checksum = inv_checksum(data, hd->size); + if (checksum == hd->checksum) { + ds.load[entry](data); + } else { + return INV_ERROR_CALIBRATION_LOAD; + } + } + } + len -= hd->size; + if (len >= 0) + data = data + hd->size; + } + + return INV_SUCCESS; +} + +/** This function fills up a block of memory to be stored in non-volatile memory. +* @param[out] data Place to store data, size of sz, must be at least size +* returned by inv_get_mpl_state_size() +* @param[in] sz Size of data. +* @return Returns INV_SUCCESS if successful or an error code if not. +*/ +inv_error_t inv_save_mpl_states(unsigned char *data, size_t sz) +{ + unsigned char *cur; + int kk; + struct data_header_t *hd; + + if (sz >= ds.total_size) { + cur = data + sizeof(struct data_header_t); + for (kk = 0; kk < ds.num; ++kk) { + hd = (struct data_header_t *)cur; + cur += sizeof(struct data_header_t); + ds.save[kk](cur); + hd->checksum = inv_checksum(cur, ds.hd[kk].size); + hd->size = ds.hd[kk].size; + hd->key = ds.hd[kk].key; + cur += ds.hd[kk].size; + } + } else { + return INV_ERROR_CALIBRATION_LOAD; + } + + hd = (struct data_header_t *)data; + hd->checksum = inv_checksum(data + sizeof(struct data_header_t), + ds.total_size - sizeof(struct data_header_t)); + hd->key = DEFAULT_KEY; + hd->size = ds.total_size; + + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/60xx/libsensors_iio/software/core/mllite/storage_manager.h b/60xx/libsensors_iio/software/core/mllite/storage_manager.h new file mode 100644 index 0000000..7255591 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mllite/storage_manager.h @@ -0,0 +1,30 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#include "mltypes.h" + +#ifndef INV_STORAGE_MANAGER_H__ +#define INV_STORAGE_MANAGER_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_register_load_store( + inv_error_t (*load_func)(const unsigned char *data), + inv_error_t (*save_func)(unsigned char *data), + size_t size, unsigned int key); +void inv_init_storage_manager(void); + +inv_error_t inv_get_mpl_state_size(size_t *size); +inv_error_t inv_load_mpl_states(const unsigned char *data, size_t len); +inv_error_t inv_save_mpl_states(unsigned char *data, size_t len); + +#ifdef __cplusplus +} +#endif + +#endif /* INV_STORAGE_MANAGER_H__ */ diff --git a/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h new file mode 100644 index 0000000..5a53213 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/accel_auto_cal.h @@ -0,0 +1,38 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id$ + * + ******************************************************************************/ + +#ifndef MLDMP_ACCEL_AUTO_CALIBRATION_H__ +#define MLDMP_ACCEL_AUTO_CALIBRATION_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_in_use_auto_calibration(void); +inv_error_t inv_disable_in_use_auto_calibration(void); +inv_error_t inv_stop_in_use_auto_calibration(void); +inv_error_t inv_start_in_use_auto_calibration(void); +inv_error_t inv_in_use_auto_calibration_is_enabled(unsigned char *is_enabled); +inv_error_t inv_init_in_use_auto_calibration(void); +void inv_init_accel_maxmin(void); +void inv_record_good_accel_maxmin(void); +int inv_get_accel_bias_stage(); + +#ifdef __cplusplus +} +#endif + +#endif // MLDMP_ACCEL_AUTO_CALIBRATION_H__ + diff --git a/60xx/libsensors_iio/software/core/mpl/authenticate.h b/60xx/libsensors_iio/software/core/mpl/authenticate.h new file mode 100644 index 0000000..d7c803b --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/authenticate.h @@ -0,0 +1,21 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id$ + * + ******************************************************************************/ + +#ifndef MLDMP_AUTHENTICATE_H__ +#define MLDMP_AUTHENTICATE_H__ + +#include "invensense.h" + +inv_error_t inv_check_key(void); + +#endif /* MLDMP_AUTHENTICATE_H__ */ diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so new file mode 100644 index 0000000..fbd346f Binary files /dev/null and b/60xx/libsensors_iio/software/core/mpl/build/android/libmplmpu.so differ diff --git a/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk new file mode 100644 index 0000000..2e15205 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/build/android/shared.mk @@ -0,0 +1,88 @@ +MPL_LIB_NAME ?= mplmpu +LIBRARY = $(LIB_PREFIX)$(MPL_LIB_NAME).$(SHARED_LIB_EXT) + +MK_NAME = $(notdir $(CURDIR)/$(firstword $(MAKEFILE_LIST))) + +CROSS ?= $(ANDROID_ROOT)/prebuilt/linux-x86/toolchain/arm-eabi-4.4.0/bin/arm-eabi- +COMP ?= $(CROSS)gcc +LINK ?= $(CROSS)gcc + +OBJFOLDER = $(CURDIR)/obj + +INV_ROOT = ../../../../.. +MLLITE_DIR = $(INV_ROOT)/software/core/mllite +MPL_DIR = $(INV_ROOT)/software/core/mpl + +include $(INV_ROOT)/software/build/android/common.mk + +CFLAGS += $(CMDLINE_CFLAGS) +CFLAGS += $(ANDROID_COMPILE) +CFLAGS += -Wall +CFLAGS += -fpic +CFLAGS += -nostdlib +CFLAGS += -DNDEBUG +CFLAGS += -D_REENTRANT +CFLAGS += -DLINUX +CFLAGS += -DANDROID +CFLAGS += -mthumb-interwork +CFLAGS += -fno-exceptions +CFLAGS += -ffunction-sections +CFLAGS += -funwind-tables +CFLAGS += -fstack-protector +CFLAGS += -fno-short-enums +CFLAGS += -fmessage-length=0 +CFLAGS += $(INV_INCLUDES) +CFLAGS += $(INV_DEFINES) + +LLINK = -lc +LLINK += -lm +LLINK += -lutils +LLINK += -lcutils +LLINK += -lgcc +LLINK += -ldl + +LFLAGS += $(CMDLINE_LFLAGS) +LFLAGS += -shared +LFLAGS += -Wl,-soname,$(LIBRARY) +LFLAGS += -Wl,-shared,-Bsymbolic +LFLAGS += $(ANDROID_LINK) +LFLAGS += -Wl,-rpath,$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/obj/lib:$(ANDROID_ROOT)/out/target/product/$(PRODUCT)/system/lib + +#################################################################################################### +## sources + +INV_LIBS = $(MLLITE_DIR)/build/$(TARGET)/$(LIB_PREFIX)$(MLLITE_LIB_NAME).$(SHARED_LIB_EXT) + +#INV_SOURCES, VPATH provided by Makefile.filelist +include ../filelist.mk + +INV_OBJS := $(addsuffix .o,$(INV_SOURCES)) +INV_OBJS_DST = $(addprefix $(OBJFOLDER)/,$(addsuffix .o, $(notdir $(INV_SOURCES)))) + +#################################################################################################### +## rules + +.PHONY: all mpl clean cleanall + +all: mpl + +mpl: $(LIBRARY) $(MK_NAME) + +$(LIBRARY) : $(OBJFOLDER) $(INV_OBJS_DST) $(MK_NAME) + @$(call echo_in_colors, "\n\n") + mkdir obj + +$(INV_OBJS_DST) : $(OBJFOLDER)/%.c.o : %.c $(MK_NAME) + @$(call echo_in_colors, "\n\n") + $(COMP) $(ANDROID_INCLUDES) $(KERNEL_INCLUDES) $(CFLAGS) -o $@ -c $< + +clean : + rm -fR $(OBJFOLDER) + +cleanall : + rm -fR $(LIBRARY) $(OBJFOLDER) + diff --git a/60xx/libsensors_iio/software/core/mpl/build/filelist.mk b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk new file mode 100644 index 0000000..e18003a --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/build/filelist.mk @@ -0,0 +1,46 @@ +#### filelist.mk for mpl #### + +# headers only +HEADERS := $(MPL_DIR)/mpu.h + +# headers for sources +HEADERS := $(MPL_DIR)/fast_no_motion.h +HEADERS += $(MPL_DIR)/fusion_9axis.h +HEADERS += $(MPL_DIR)/motion_no_motion.h +HEADERS += $(MPL_DIR)/no_gyro_fusion.h +HEADERS += $(MPL_DIR)/quaternion_supervisor.h +HEADERS += $(MPL_DIR)/gyro_tc.h +HEADERS += $(MPL_DIR)/authenticate.h +HEADERS += $(MPL_DIR)/accel_auto_cal.h +HEADERS += $(MPL_DIR)/accel_auto_cal_protected.h +HEADERS += $(MPL_DIR)/compass_vec_cal.h +HEADERS += $(MPL_DIR)/compass_vec_cal_protected.h +HEADERS += $(MPL_DIR)/mag_disturb.h +HEADERS += $(MPL_DIR)/mag_disturb_protected.h +HEADERS += $(MPL_DIR)/compass_bias_w_gyro.h +HEADERS += $(MPL_DIR)/heading_from_gyro.h +HEADERS += $(MPL_DIR)/compass_fit.h +HEADERS += $(MPL_DIR)/quat_accuracy_monitor.h +#HEADERS += $(MPL_DIR)/auto_calibration.h + +# sources +SOURCES := $(MPL_DIR)/fast_no_motion.c +SOURCES += $(MPL_DIR)/fusion_9axis.c +SOURCES += $(MPL_DIR)/motion_no_motion.c +SOURCES += $(MPL_DIR)/no_gyro_fusion.c +SOURCES += $(MPL_DIR)/quaternion_supervisor.c +SOURCES += $(MPL_DIR)/gyro_tc.c +SOURCES += $(MPL_DIR)/authenticate.c +SOURCES += $(MPL_DIR)/accel_auto_cal.c +SOURCES += $(MPL_DIR)/compass_vec_cal.c +SOURCES += $(MPL_DIR)/mag_disturb.c +SOURCES += $(MPL_DIR)/compass_bias_w_gyro.c +SOURCES += $(MPL_DIR)/heading_from_gyro.c +SOURCES += $(MPL_DIR)/compass_fit.c +SOURCES += $(MPL_DIR)/quat_accuracy_monitor.c +#SOURCES += $(MPL_DIR)/auto_calibration.c + +INV_SOURCES += $(SOURCES) + +VPATH = $(MPL_DIR) + diff --git a/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h new file mode 100644 index 0000000..4f01fc2 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/compass_bias_w_gyro.h @@ -0,0 +1,31 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_COMPASSBIASWGYRO_H__ +#define MLDMP_COMPASSBIASWGYRO_H__ +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_compass_bias_w_gyro(); + inv_error_t inv_disable_compass_bias_w_gyro(); + void inv_init_compass_bias_w_gyro(); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_COMPASSBIASWGYRO_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/compass_fit.h b/60xx/libsensors_iio/software/core/mpl/compass_fit.h new file mode 100644 index 0000000..be3cce8 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/compass_fit.h @@ -0,0 +1,28 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +#ifndef INV_MLDMP_COMPASSFIT_H__ +#define INV_MLDMP_COMPASSFIT_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void inv_init_compass_fit(void); +inv_error_t inv_enable_compass_fit(void); +inv_error_t inv_disable_compass_fit(void); +inv_error_t inv_start_compass_fit(void); +inv_error_t inv_stop_compass_fit(void); + +#ifdef __cplusplus +} +#endif + + +#endif // INV_MLDMP_COMPASSFIT_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h new file mode 100644 index 0000000..a3e044c --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/compass_vec_cal.h @@ -0,0 +1,34 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +#ifndef COMPASS_ONLY_CAL_H__ +#define COMPASS_ONLY_CAL_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_vector_compass_cal(); +inv_error_t inv_disable_vector_compass_cal(); +inv_error_t inv_start_vector_compass_cal(void); +inv_error_t inv_stop_vector_compass_cal(void); +void inv_vector_compass_cal_sensitivity(float sens); +inv_error_t inv_init_vector_compass_cal(); + +#ifdef __cplusplus +} +#endif + +#endif // COMPASS_ONLY_CAL_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h new file mode 100644 index 0000000..c553926 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/fast_no_motion.h @@ -0,0 +1,46 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_FAST_NO_MOTION_H__ +#define MLDMP_FAST_NO_MOTION_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_fast_nomot(void); + inv_error_t inv_disable_fast_nomot(void); + inv_error_t inv_start_fast_nomot(void); + inv_error_t inv_stop_fast_nomot(void); + inv_error_t inv_init_fast_nomot(void); + void inv_set_default_number_of_samples(int N); + inv_error_t inv_fast_nomot_is_enabled(unsigned char *is_enabled); + inv_error_t inv_update_fast_nomot(long *gyro); + + void inv_get_fast_nomot_accel_param(long *cntr, long long *param); + void inv_get_fast_nomot_compass_param(long *cntr, long long *param); + void inv_set_fast_nomot_accel_threshold(long long thresh); + void inv_set_fast_nomot_compass_threshold(long long thresh); + void int_set_fast_nomot_gyro_threshold(long long thresh); + + void inv_fnm_debug_print(void); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_FAST_NO_MOTION_H__ + diff --git a/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h new file mode 100644 index 0000000..1ba1ebb --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/fusion_9axis.h @@ -0,0 +1,37 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_FUSION9AXIS_H__ +#define MLDMP_FUSION9AXIS_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + void inv_init_9x_fusion(void); + inv_error_t inv_9x_fusion_state_change(unsigned char newState); + inv_error_t inv_enable_9x_sensor_fusion(void); + inv_error_t inv_disable_9x_sensor_fusion(void); + inv_error_t inv_start_9x_sensor_fusion(void); + inv_error_t inv_stop_9x_sensor_fusion(void); + inv_error_t inv_9x_fusion_set_mag_fb(double fb); + inv_error_t inv_9x_fusion_enable_jitter_reduction(int en); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_FUSION9AXIS_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/gyro_tc.h b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h new file mode 100644 index 0000000..69918d5 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/gyro_tc.h @@ -0,0 +1,43 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef _GYRO_TC_H_ +#define _GYRO_TC_H_ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_gyro_tc(void); +inv_error_t inv_disable_gyro_tc(void); +inv_error_t inv_start_gyro_tc(void); +inv_error_t inv_stop_gyro_tc(void); + +inv_error_t inv_get_gyro_ts(long *data); +inv_error_t inv_set_gyro_ts(long *data); + +inv_error_t inv_init_gyro_ts(void); + +inv_error_t inv_set_gtc_max_temp(long data); +inv_error_t inv_set_gtc_min_temp(long data); + +inv_error_t inv_print_gtc_data(void); + +#ifdef __cplusplus +} +#endif + +#endif /* _GYRO_TC_H */ + diff --git a/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h new file mode 100644 index 0000000..09ecc42 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/heading_from_gyro.h @@ -0,0 +1,33 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef _HEADING_FROM_GYRO_H_ +#define _HEADING_FROM_GYRO_H_ +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_heading_from_gyro(void); + inv_error_t inv_disable_heading_from_gyro(void); + void inv_init_heading_from_gyro(void); + inv_error_t inv_start_heading_from_gyro(void); + inv_error_t inv_stop_heading_from_gyro(void); + +#ifdef __cplusplus +} +#endif + + +#endif /* _HEADING_FROM_GYRO_H_ */ diff --git a/60xx/libsensors_iio/software/core/mpl/inv_math.h b/60xx/libsensors_iio/software/core/mpl/inv_math.h new file mode 100644 index 0000000..6620bbf --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/inv_math.h @@ -0,0 +1,8 @@ +/* math.h has many functions and defines that are not consistent across +* platforms. This address that */ + +#ifdef _WINDOWS +#define _USE_MATH_DEFINES +#endif + +#include diff --git a/60xx/libsensors_iio/software/core/mpl/invensense_adv.h b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h new file mode 100644 index 0000000..12932c9 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/invensense_adv.h @@ -0,0 +1,31 @@ +/* + $License: + Copyright (C) 2012 InvenSense Corporation, All Rights Reserved. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +/* + Main header file for Invensense's Advanced library. +*/ + +#include "accel_auto_cal.h" +#include "compass_bias_w_gyro.h" +#include "compass_fit.h" +#include "compass_vec_cal.h" +#include "fast_no_motion.h" +#include "fusion_9axis.h" +#include "gyro_tc.h" +#include "heading_from_gyro.h" +#include "mag_disturb.h" +#include "motion_no_motion.h" +#include "no_gyro_fusion.h" +#include "quaternion_supervisor.h" +#include "mag_disturb.h" +#include "quat_accuracy_monitor.h" +#include "shake.h" diff --git a/60xx/libsensors_iio/software/core/mpl/mag_disturb.h b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h new file mode 100644 index 0000000..38df919 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/mag_disturb.h @@ -0,0 +1,37 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +#ifndef MLDMP_MAGDISTURB_H__ +#define MLDMP_MAGDISTURB_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + int inv_check_magnetic_disturbance(unsigned long delta_time, const long *quat, + const long *compass, const long *gravity); + + void inv_track_dip_angle(int mode, float currdip); + + inv_error_t inv_enable_magnetic_disturbance(void); + inv_error_t inv_disable_magnetic_disturbance(void); + int inv_get_magnetic_disturbance_state(); + inv_error_t inv_set_magnetic_disturbance(int time_ms); + inv_error_t inv_disable_dip_tracking(void); + inv_error_t inv_enable_dip_tracking(void); + inv_error_t inv_init_magnetic_disturbance(void); + + float Mag3ofNormalizedLong(const long *x); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_MAGDISTURB_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h new file mode 100644 index 0000000..01cf1c0 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/motion_no_motion.h @@ -0,0 +1,28 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_MOTION_NO_MOTION_H__ +#define INV_MOTION_NO_MOTION_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_motion_no_motion(void); +inv_error_t inv_disable_motion_no_motion(void); +inv_error_t inv_init_motion_no_motion(void); +inv_error_t inv_start_motion_no_motion(void); +inv_error_t inv_stop_motion_no_motion(void); + +inv_error_t inv_set_no_motion_time(long time_ms); + +#ifdef __cplusplus +} +#endif + +#endif // INV_MOTION_NO_MOTION_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h new file mode 100644 index 0000000..38d5690 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/no_gyro_fusion.h @@ -0,0 +1,34 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef MLDMP_NOGYROFUSION_H__ +#define MLDMP_NOGYROFUSION_H__ +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_no_gyro_fusion(void); + inv_error_t inv_disable_no_gyro_fusion(void); + inv_error_t inv_start_no_gyro_fusion(void); + inv_error_t inv_start_no_gyro_fusion(void); + inv_error_t inv_init_no_gyro_fusion(void); + +#ifdef __cplusplus +} +#endif + + +#endif // MLDMP_NOGYROFUSION_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h new file mode 100644 index 0000000..3c6a2c1 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/quat_accuracy_monitor.h @@ -0,0 +1,71 @@ +/* + quat_accuracy_monitor.h + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +#ifndef QUAT_ACCURARCY_MONITOR_H__ +#define QUAT_ACCURARCY_MONITOR_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif +enum accuracy_signal_type_e { + TYPE_NAV_QUAT, + TYPE_GAM_QUAT, + TYPE_NAV_QUAT_ADVANCED, + TYPE_GAM_QUAT_ADVANCED, + TYPE_NAV_QUAT_BASIC, + TYPE_GAM_QUAT_BASIC, + TYPE_MAG, + TYPE_GYRO, + TYPE_ACCEL, +}; + +inv_error_t inv_init_quat_accuracy_monitor(void); + +void set_accuracy_threshold(enum accuracy_signal_type_e type, double threshold); +double get_accuracy_threshold(enum accuracy_signal_type_e type); +void set_accuracy_weight(enum accuracy_signal_type_e type, int weight); +int get_accuracy_weight(enum accuracy_signal_type_e type); + +int8_t get_accuracy_accuracy(enum accuracy_signal_type_e type); + +void inv_reset_quat_accuracy(void); +double get_6axis_correction_term(void); +double get_9axis_correction_term(void); +int get_9axis_accuracy_state(); + +void set_6axis_error_average(double value); +double get_6axis_error_bound(void); +double get_compass_correction(void); +double get_9axis_error_bound(void); + +float get_confidence_interval(void); +void set_compass_uncertainty(float value); + +inv_error_t inv_enable_quat_accuracy_monitor(void); +inv_error_t inv_disable_quat_accuracy_monitor(void); +inv_error_t inv_start_quat_accuracy_monitor(void); +inv_error_t inv_stop_quat_accuracy_monitor(void); + +double get_compassNgravity(void); +double get_init_compassNgravity(void); + +float inv_heading_accuracy_check(float orient[3], float *heading, int8_t *accuracy); + +#ifdef __cplusplus +} +#endif + +#endif // QUAT_ACCURARCY_MONITOR_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h new file mode 100644 index 0000000..4fa36b0 --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/quaternion_supervisor.h @@ -0,0 +1,27 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_QUATERNION_SUPERVISOR_H__ +#define INV_QUATERNION_SUPERVISOR_H__ + +#include "mltypes.h" + + +#ifdef __cplusplus +extern "C" { +#endif + +inv_error_t inv_enable_quaternion(void); +inv_error_t inv_disable_quaternion(void); +inv_error_t inv_init_quaternion(void); +inv_error_t inv_start_quaternion(void); +void inv_set_quaternion(long *quat); + +#ifdef __cplusplus +} +#endif + +#endif // INV_QUATERNION_SUPERVISOR_H__ diff --git a/60xx/libsensors_iio/software/core/mpl/shake.h b/60xx/libsensors_iio/software/core/mpl/shake.h new file mode 100644 index 0000000..67acb7b --- /dev/null +++ b/60xx/libsensors_iio/software/core/mpl/shake.h @@ -0,0 +1,94 @@ +/* + $License: + Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved. + See included License.txt for License information. + $ + */ +#ifndef INV_SHAKE_H__ +#define INV_SHAKE_H__ + +#include "mltypes.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + #define STATE_ZERO 0 + #define STATE_INIT_1 1 + #define STATE_INIT_2 2 + #define STATE_DETECT 3 + + struct t_shake_config_params { + long shake_time_min_ms; + long shake_time_max_ms; + long shake_time_min; + long shake_time_max; + unsigned char shake_time_set; + long shake_time_saved; + float shake_deriv_thr; + int zero_cross_thr; + float accel_delta_min; + float accel_delta_max; + unsigned char interp_enable; + }; + + struct t_shake_state_params { + unsigned char state; + float accel_peak_high; + float accel_peak_low; + float accel_range; + int num_zero_cross; + short curr_shake_time; + int deriv_major_change; + int deriv_major_sign; + float accel_buffer[200]; + float delta_buffer[200]; + }; + + struct t_shake_data_params { + float accel_prev; + float accel_curr; + float delta_prev; + float delta_curr; + float delta_prev_buffer; + }; + + struct t_shake_results { + //unsigned char shake_int; + int shake_number; + }; + + struct t_shake_cb { + void (*shake_callback)(struct t_shake_results *shake_results); + }; + + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + inv_error_t inv_enable_shake(void); + inv_error_t inv_disable_shake(void); + inv_error_t inv_init_shake(void); + inv_error_t inv_start_shake(void); + int inv_set_shake_cb(void (*callback)(struct t_shake_results *shake_results)); + void inv_config_shake_time_params(long sample_time_ms); + void inv_set_shake_accel_delta_min(float accel_g); + void inv_set_shake_accel_delta_max(float accel_g); + void inv_set_shake_zero_cross_thresh(int num_zero_cross); + void inv_set_shake_deriv_thresh(float shake_deriv_thresh); + void inv_set_shake_time_min_ms(long time_ms); + void inv_set_shake_time_max_ms(long time_ms); + void inv_enable_shake_data_interpolation(unsigned char en); + + + +#ifdef __cplusplus +} +#endif + +#endif // INV_SHAKE__ \ No newline at end of file diff --git a/60xx/mlsdk/Android.mk b/60xx/mlsdk/Android.mk new file mode 100644 index 0000000..929a776 --- /dev/null +++ b/60xx/mlsdk/Android.mk @@ -0,0 +1,101 @@ +LOCAL_PATH := $(call my-dir) + +ifneq ($(BOARD_USES_GENERIC_INVENSENSE),false) + +include $(CLEAR_VARS) +LOCAL_MODULE_TAGS := optional + +LOCAL_MODULE := libmlplatform +#modify these to point to the mpl source installation +MLSDK_ROOT = . +MLPLATFORM_DIR = $(MLSDK_ROOT)/platform/linux + +LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID +LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050 +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR) +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/kernel +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite + +ML_SOURCES := \ + $(MLPLATFORM_DIR)/mlos_linux.c \ + $(MLPLATFORM_DIR)/mlsl_linux_mpu.c + +LOCAL_SRC_FILES := $(ML_SOURCES) + +LOCAL_SHARED_LIBRARIES := liblog libm libutils libcutils +include $(BUILD_SHARED_LIBRARY) + +include $(CLEAR_VARS) +LOCAL_MODULE := libmllite +LOCAL_MODULE_TAGS := optional +#modify these to point to the mpl source installation +MLSDK_ROOT = . +MLPLATFORM_DIR = $(MLSDK_ROOT)/platform +MLLITE_DIR = $(MLSDK_ROOT)/mllite +MPL_DIR = $(MLSDK_ROOT)/mldmp + +LOCAL_CFLAGS += -DNDEBUG +LOCAL_CFLAGS += -D_REENTRANT -DLINUX -DANDROID +LOCAL_CFLAGS += -DCONFIG_MPU_SENSORS_MPU3050 +LOCAL_CFLAGS += -DUNICODE -D_UNICODE -DSK_RELEASE +LOCAL_CFLAGS += -DI2CDEV=\"/dev/mpu\" +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MPL_DIR) -I$(LOCAL_PATH)/$(MLLITE_DIR) -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/include +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlutils -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mlapps/common +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/platform/include/linux +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLSDK_ROOT)/mllite/akmd +LOCAL_CFLAGS += -I$(LOCAL_PATH)/$(MLPLATFORM_DIR)/linux + +# optionally apply the compass filter. this is set in +# BoardConfig.mk +ifeq ($(BOARD_INVENSENSE_APPLY_COMPASS_NOISE_FILTER),true) +LOCAL_CFLAGS += -DAPPLY_COMPASS_FILTER +endif + +ML_SOURCES = \ + $(MLLITE_DIR)/accel.c \ + $(MLLITE_DIR)/compass.c \ + $(MLLITE_DIR)/pressure.c \ + $(MLLITE_DIR)/mldl_cfg_mpu.c \ + $(MLLITE_DIR)/dmpDefault.c \ + $(MLLITE_DIR)/ml.c \ + $(MLLITE_DIR)/mlarray.c \ + $(MLLITE_DIR)/mlarray_legacy.c \ + $(MLLITE_DIR)/mlFIFO.c \ + $(MLLITE_DIR)/mlFIFOHW.c \ + $(MLLITE_DIR)/mlMathFunc.c \ + $(MLLITE_DIR)/ml_stored_data.c \ + $(MLLITE_DIR)/mlcontrol.c \ + $(MLLITE_DIR)/mldl.c \ + $(MLLITE_DIR)/mldmp.c \ + $(MLLITE_DIR)/mlstates.c \ + $(MLLITE_DIR)/mlsupervisor.c \ + $(MLLITE_DIR)/mlBiasNoMotion.c \ + $(MLLITE_DIR)/mlSetGyroBias.c \ + \ + $(MLLITE_DIR)/ml_mputest.c \ + $(MLSDK_ROOT)/mlutils/mputest.c \ + $(MLSDK_ROOT)/mlutils/checksum.c + + +ifeq ($(HARDWARE),M_HW) + ML_SOURCES += $(MLLITE_DIR)/accel/mantis.c +endif + +LOCAL_SRC_FILES := $(ML_SOURCES) +LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform +include $(BUILD_SHARED_LIBRARY) + +#This makes an .so from our .a +#include $(CLEAR_VARS) +#LOCAL_MODULE := libmpl +#LOCAL_MODULE_TAGS := optional +#LOCAL_SRC_FILES := mlsdk/mldmp/mpl/android/libmpl.a +#LOCAL_SHARED_LIBRARIES := libm libutils libcutils liblog libmlplatform libmllite +#LOCAL_WHOLE_STATIC_LIBRARIES := libmpl +#LOCAL_PREBUILT_LIBS := mlsdk/mldmp/mpl/android/libmpl.a +#include $(BUILD_SHARED_LIBRARY) +#include $(BUILD_MULTI_PREBUILT) + +endif diff --git a/60xx/mlsdk/mllite/accel.c b/60xx/mlsdk/mllite/accel.c new file mode 100644 index 0000000..60b8d6c --- /dev/null +++ b/60xx/mlsdk/mllite/accel.c @@ -0,0 +1,189 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: accel.c 4595 2011-01-25 01:43:03Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup ACCELDL + * @brief Motion Library - Accel Driver Layer. + * Provides the interface to setup and handle an accel + * connected to either the primary or the seconday I2C interface + * of the gyroscope. + * + * @{ + * @file accel.c + * @brief Accel setup and handling methods. +**/ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include + +#include "ml.h" +#include "mlinclude.h" +#include "dmpKey.h" +#include "mlFIFO.h" +#include "mldl.h" +#include "mldl_cfg.h" +#include "mlMathFunc.h" +#include "mlsl.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-accel" + +#define ACCEL_DEBUG 0 + +/* --------------------- */ +/* - Global Variables. - */ +/* --------------------- */ + +/* --------------------- */ +/* - Static Variables. - */ +/* --------------------- */ + +/* --------------- */ +/* - Prototypes. - */ +/* --------------- */ + +/* -------------- */ +/* - Externs. - */ +/* -------------- */ + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Used to determine if an accel is configured and + * used by the MPL. + * @return INV_SUCCESS if the accel is present. + */ +unsigned char inv_accel_present(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->accel && + NULL != mldl_cfg->accel->resume && + mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) + return TRUE; + else + return FALSE; +} + +/** + * @brief Query the accel slave address. + * @return The 7-bit accel slave address. + */ +unsigned char inv_get_slave_addr(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->pdata) + return mldl_cfg->pdata->accel.address; + else + return 0; +} + +/** + * @brief Get the ID of the accel in use. + * @return ID of the accel in use. + */ +unsigned short inv_get_accel_id(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->accel) { + return mldl_cfg->accel->id; + } + return ID_INVALID; +} + +/** + * @brief Get a sample of accel data from the device. + * @param data + * the buffer to store the accel raw data for + * X, Y, and Z axes. + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t inv_get_accel_data(long *data) +{ + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + inv_error_t result; + unsigned char raw_data[2 * ACCEL_NUM_AXES]; + long tmp[ACCEL_NUM_AXES]; + int ii; + signed char *mtx = mldl_cfg->pdata->accel.orientation; + char accelId = mldl_cfg->accel->id; + + if (NULL == data) + return INV_ERROR_INVALID_PARAMETER; + + if (mldl_cfg->accel->read_len > sizeof(raw_data)) + return INV_ERROR_ASSERTION_FAILURE; + + result = (inv_error_t) inv_mpu_read_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), + raw_data); + if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { + return result; + } + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { + if (EXT_SLAVE_LITTLE_ENDIAN == mldl_cfg->accel->endian) { + tmp[ii] = (long)((signed char)raw_data[2 * ii + 1]) * 256; + tmp[ii] += (long)((unsigned char)raw_data[2 * ii]); + } else if ((EXT_SLAVE_BIG_ENDIAN == mldl_cfg->accel->endian) || + (EXT_SLAVE_FS16_BIG_ENDIAN == mldl_cfg->accel->endian)) { + tmp[ii] = (long)((signed char)raw_data[2 * ii]) * 256; + tmp[ii] += (long)((unsigned char)raw_data[2 * ii + 1]); + if (accelId == ACCEL_ID_KXSD9) { + tmp[ii] = (long)((short)(((unsigned short)tmp[ii]) + + ((unsigned short)0x8000))); + } + } else if (EXT_SLAVE_FS8_BIG_ENDIAN == mldl_cfg->accel->endian) { + tmp[ii] = (long)((signed char)raw_data[ii]) * 256; + } else { + result = INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + } + + for (ii = 0; ii < ARRAY_SIZE(tmp); ii++) { + data[ii] = ((long)tmp[0] * mtx[3 * ii] + + (long)tmp[1] * mtx[3 * ii + 1] + + (long)tmp[2] * mtx[3 * ii + 2]); + } + + //MPL_LOGI("ACCEL: %8ld, %8ld, %8ld\n", data[0], data[1], data[2]); + return result; +} + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/accel.h b/60xx/mlsdk/mllite/accel.h new file mode 100644 index 0000000..d3fbc6a --- /dev/null +++ b/60xx/mlsdk/mllite/accel.h @@ -0,0 +1,57 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: accel.h 4580 2011-01-22 03:19:23Z prao $ + * + *******************************************************************************/ + +#ifndef ACCEL_H +#define ACCEL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#include "mpu.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "accel_legacy.h" +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + + unsigned char inv_accel_present(void); + unsigned char inv_get_slave_addr(void); + inv_error_t inv_get_accel_data(long *data); + unsigned short inv_get_accel_id(void); + +#ifdef __cplusplus +} +#endif +#endif // ACCEL_H diff --git a/60xx/mlsdk/mllite/compass.c b/60xx/mlsdk/mllite/compass.c new file mode 100644 index 0000000..798cb9f --- /dev/null +++ b/60xx/mlsdk/mllite/compass.c @@ -0,0 +1,579 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: compass.c 5641 2011-06-14 02:10:02Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup COMPASSDL + * @brief Motion Library - Compass Driver Layer. + * Provides the interface to setup and handle an compass + * connected to either the primary or the seconday I2C interface + * of the gyroscope. + * + * @{ + * @file compass.c + * @brief Compass setup and handling methods. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include +#include +#include "compass.h" + +#include "ml.h" +#include "mlinclude.h" +#include "dmpKey.h" +#include "mlFIFO.h" +#include "mldl.h" +#include "mldl_cfg.h" +#include "mlMathFunc.h" +#include "mlsl.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-compass" + +#define COMPASS_DEBUG 0 + +/* --------------------- */ +/* - Global Variables. - */ +/* --------------------- */ + +/* --------------------- */ +/* - Static Variables. - */ +/* --------------------- */ + +/* --------------- */ +/* - Prototypes. - */ +/* --------------- */ + +/* -------------- */ +/* - Externs. - */ +/* -------------- */ + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +static float square(float data) +{ + return data * data; +} + +static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise) +{ + int i; + + adap_filter->num = 0; + adap_filter->index = 0; + adap_filter->noise = noise; + adap_filter->len = len; + + for (i = 0; i < adap_filter->len; ++i) { + adap_filter->sequence[i] = 0; + } +} + +static int cmpfloat(const void *p1, const void *p2) +{ + return *(float*)p1 - *(float*)p2; +} + + +static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in) +{ + float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN]; + int i; + + if (adap_filter->len <= 1) { + return in; + } + if (adap_filter->num < adap_filter->len) { + adap_filter->sequence[adap_filter->index++] = in; + adap_filter->num++; + return in; + } + if (adap_filter->len <= adap_filter->index) { + adap_filter->index = 0; + } + adap_filter->sequence[adap_filter->index++] = in; + + avg = 0; + for (i = 0; i < adap_filter->len; i++) { + avg += adap_filter->sequence[i]; + } + avg /= adap_filter->len; + + memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float)); + qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat); + median = sorted[adap_filter->len/2]; + + sum = 0; + for (i = 0; i < adap_filter->len; i++) { + sum += square(avg - adap_filter->sequence[i]); + } + sum /= adap_filter->len; + + if (sum <= adap_filter->noise) { + return median; + } + + return ((in - avg) * (sum - adap_filter->noise) / sum + avg); +} + +static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold) +{ + thresh_filter->threshold = threshold; + thresh_filter->last = 0; +} + +static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in) +{ + if (in < thresh_filter->last - thresh_filter->threshold + || thresh_filter->last + thresh_filter->threshold < in) { + thresh_filter->last = in; + return in; + } + else { + return thresh_filter->last; + } +} + +static int init(yas_filter_handle_t *t) +{ + float noise[] = { + YAS_DEFAULT_FILTER_NOISE, + YAS_DEFAULT_FILTER_NOISE, + YAS_DEFAULT_FILTER_NOISE, + }; + int i; + + if (t == NULL) { + return -1; + } + + for (i = 0; i < 3; i++) { + adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]); + thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH); + } + + return 0; +} + +static int update(yas_filter_handle_t *t, float *input, float *output) +{ + int i; + + if (t == NULL || input == NULL || output == NULL) { + return -1; + } + + for (i = 0; i < 3; i++) { + output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]); + output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]); + } + + return 0; +} + +int yas_filter_init(yas_filter_if_s *f) +{ + if (f == NULL) { + return -1; + } + f->init = init; + f->update = update; + + return 0; +} + +/** + * @brief Used to determine if a compass is + * configured and used by the MPL. + * @return INV_SUCCESS if the compass is present. + */ +unsigned char inv_compass_present(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->compass && + NULL != mldl_cfg->compass->resume && + mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS) + return TRUE; + else + return FALSE; +} + +/** + * @brief Query the compass slave address. + * @return The 7-bit compass slave address. + */ +unsigned char inv_get_compass_slave_addr(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->pdata) + return mldl_cfg->pdata->compass.address; + else + return 0; +} + +/** + * @brief Get the ID of the compass in use. + * @return ID of the compass in use. + */ +unsigned short inv_get_compass_id(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->compass) { + return mldl_cfg->compass->id; + } + return ID_INVALID; +} + +/** + * @brief Get a sample of compass data from the device. + * @param data + * the buffer to store the compass raw data for + * X, Y, and Z axes. + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t inv_get_compass_data(long *data) +{ + inv_error_t result; + int ii; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + unsigned char *tmp = inv_obj.compass_raw_data; + + if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION); + return INV_ERROR_INVALID_CONFIGURATION; + } + + if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY || + !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { + /*--- read the compass sensor data. + The compass read function may return an INV_ERROR_COMPASS_* errors + when the data is not ready (read/refresh frequency mismatch) or + the internal data sampling timing of the device was not respected. + Returning the error code will make the sensor fusion supervisor + ignore this compass data sample. ---*/ + result = (inv_error_t) inv_mpu_read_compass(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), + tmp); + if (result) { + if (COMPASS_DEBUG) { + MPL_LOGV("inv_mpu_read_compass returned %d\n", result); + } + return result; + } + for (ii = 0; ii < 3; ii++) { + if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian) + data[ii] = + ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1]; + else + data[ii] = + ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii]; + } + + inv_obj.compass_overunder = (int)tmp[6]; + + } else { +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + result = inv_get_external_sensor_data(data, 3); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#if defined CONFIG_MPU_SENSORS_MPU6050A2 + { + static unsigned char first = TRUE; + // one-off write to AKM + if (first) { + unsigned char regs[] = { + // beginning Mantis register for one-off slave R/W + MPUREG_I2C_SLV4_ADDR, + // the slave to write to + mldl_cfg->pdata->compass.address, + // the register to write to + /*mldl_cfg->compass->trigger->reg */ 0x0A, + // the value to write + /*mldl_cfg->compass->trigger->value */ 0x01, + // enable the write + 0xC0 + }; + result = + inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr, + ARRAY_SIZE(regs), regs); + first = FALSE; + } else { + unsigned char regs[] = { + MPUREG_I2C_SLV4_CTRL, + 0xC0 + }; + result = + inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr, + ARRAY_SIZE(regs), regs); + } + } +#endif +#else + return INV_ERROR_INVALID_CONFIGURATION; +#endif // CONFIG_MPU_SENSORS_xxxx + } + data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]); + data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]); + data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]); + + return INV_SUCCESS; +} + +/** + * @brief Sets the compass bias. + * @param bias + * Compass bias, length 3. Scale is micro Tesla's * 2^16. + * Frame is mount frame which may be different from body frame. + * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. + */ +inv_error_t inv_set_compass_bias(long *bias) +{ + inv_error_t result = INV_SUCCESS; + long biasC[3]; + struct mldl_cfg *mldlCfg = inv_get_dl_config(); + + inv_obj.compass_bias[0] = bias[0]; + inv_obj.compass_bias[1] = bias[1]; + inv_obj.compass_bias[2] = bias[2]; + + // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame + biasC[0] = + (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) + + inv_obj.init_compass_bias[0] * (1L << 16); + biasC[1] = + (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) + + inv_obj.init_compass_bias[1] * (1L << 16); + biasC[2] = + (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) + + inv_obj.init_compass_bias[2] * (1L << 16); + + if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) { + unsigned char reg[4]; + long biasB[3]; + signed char *orC = mldlCfg->pdata->compass.orientation; + + // Now transform to body frame + biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2]; + biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5]; + biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8]; + + result = + inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4, + inv_int32_to_big8(biasB[0], reg)); + result = + inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4, + inv_int32_to_big8(biasB[1], reg)); + result = + inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4, + inv_int32_to_big8(biasB[2], reg)); + } + return result; +} + +/** + * @brief Write a single register on the compass slave device, regardless + * of the bus it is connected to and the MPU's configuration. + * @param reg + * the register to write to on the slave compass device. + * @param val + * the value to write. + * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. + */ +inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val) +{ + struct ext_slave_config config; + unsigned char data[2]; + inv_error_t result; + + data[0] = reg; + data[1] = val; + + config.key = MPU_SLAVE_WRITE_REGISTERS; + config.len = 2; + config.apply = TRUE; + config.data = data; + + result = inv_mpu_config_compass(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * @brief Read values from the compass slave device registers, regardless + * of the bus it is connected to and the MPU's configuration. + * @param reg + * the register to read from on the slave compass device. + * @param val + * a buffer of 3 elements to store the values read from the + * compass device. + * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. + */ +inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val) +{ + struct ext_slave_config config; + unsigned char data[2]; + inv_error_t result; + + data[0] = reg; + + config.key = MPU_SLAVE_READ_REGISTERS; + config.len = 2; + config.apply = TRUE; + config.data = data; + + result = inv_mpu_get_compass_config(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + *val = data[1]; + return result; +} + +/** + * @brief Read values from the compass slave device scale registers, regardless + * of the bus it is connected to and the MPU's configuration. + * @param reg + * the register to read from on the slave compass device. + * @param val + * a buffer of 3 elements to store the values read from the + * compass device. + * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise. + */ +inv_error_t inv_compass_read_scale(long *val) +{ + struct ext_slave_config config; + unsigned char data[3]; + inv_error_t result; + + config.key = MPU_SLAVE_READ_SCALE; + config.len = 3; + config.apply = TRUE; + config.data = data; + + result = inv_mpu_get_compass_config(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + val[0] = ((data[0] - 128) << 22) + (1L << 30); + val[1] = ((data[1] - 128) << 22) + (1L << 30); + val[2] = ((data[2] - 128) << 22) + (1L << 30); + return result; +} + +inv_error_t inv_set_compass_offset(void) +{ + struct ext_slave_config config; + unsigned char data[3]; + inv_error_t result; + + config.key = MPU_SLAVE_OFFSET_VALS; + config.len = 3; + config.apply = TRUE; + config.data = data; + + if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) { + /* push stored values */ + data[0] = (char)inv_obj.compass_offsets[0]; + data[1] = (char)inv_obj.compass_offsets[1]; + data[2] = (char)inv_obj.compass_offsets[2]; + MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]); + result = inv_mpu_config_compass(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + } else { + /* compute new values and store them */ + result = inv_mpu_get_compass_config(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]); + if(result == INV_SUCCESS) { + inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1; + inv_obj.compass_offsets[0] = data[0]; + inv_obj.compass_offsets[1] = data[1]; + inv_obj.compass_offsets[2] = data[2]; + } + } + + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +inv_error_t inv_compass_check_range(void) +{ + struct ext_slave_config config; + unsigned char data[3]; + inv_error_t result; + + config.key = MPU_SLAVE_RANGE_CHECK; + config.len = 3; + config.apply = TRUE; + config.data = data; + + result = inv_mpu_get_compass_config(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if(data[0] || data[1] || data[2]) { + /* some value clipped */ + return INV_ERROR_COMPASS_DATA_ERROR; + } + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/compass.h b/60xx/mlsdk/mllite/compass.h new file mode 100644 index 0000000..f0bdb58 --- /dev/null +++ b/60xx/mlsdk/mllite/compass.h @@ -0,0 +1,92 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: compass.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#ifndef COMPASS_H +#define COMPASS_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#include "mpu.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "compass_legacy.h" +#endif + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + +#define YAS_MAX_FILTER_LEN (20) +#define YAS_DEFAULT_FILTER_LEN (20) +#define YAS_DEFAULT_FILTER_THRESH (300) /* 300 nT */ +#define YAS_DEFAULT_FILTER_NOISE (2000 * 2000) /* standard deviation 2000 nT */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + +struct yas_adaptive_filter { + int num; + int index; + int len; + float noise; + float sequence[YAS_MAX_FILTER_LEN]; +}; + +struct yas_thresh_filter { + float threshold; + float last; +}; + +typedef struct { + struct yas_adaptive_filter adap_filter[3]; + struct yas_thresh_filter thresh_filter[3]; +} yas_filter_handle_t; + +typedef struct { + int (*init)(yas_filter_handle_t *t); + int (*update)(yas_filter_handle_t *t, float *input, float *output); +} yas_filter_if_s; + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + + unsigned char inv_compass_present(void); + unsigned char inv_get_compass_slave_addr(void); + inv_error_t inv_get_compass_data(long *data); + inv_error_t inv_set_compass_bias(long *bias); + unsigned short inv_get_compass_id(void); + inv_error_t inv_set_compass_offset(void); + inv_error_t inv_compass_check_range(void); + inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val); + inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val); + inv_error_t inv_compass_read_scale(long *val); + + int yas_filter_init(yas_filter_if_s *f); + +#ifdef __cplusplus +} +#endif +#endif // COMPASS_H diff --git a/60xx/mlsdk/mllite/dmpDefault.c b/60xx/mlsdk/mllite/dmpDefault.c new file mode 100644 index 0000000..6620d14 --- /dev/null +++ b/60xx/mlsdk/mllite/dmpDefault.c @@ -0,0 +1,417 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/***************************************************************************** * + * $Id: dmpDefault.c 5627 2011-06-10 22:34:18Z nroyer $ + ******************************************************************************/ + +/* WARNING: autogenerated code, do not modify */ +/** + * @defgroup DMPDEFAULT + * @brief Data and configuration for MLDmpDefaultOpen. + * + * @{ + * @file inv_setup_dmp.c + * @brief Data and configuration for MLDmpDefaultOpen. + */ + +#include "mltypes.h" +#include "dmpDefault.h" +#include "dmpKey.h" +#include "dmpmap.h" +#include "ml.h" +#include "mpu.h" +#include "mldl.h" +#include "mldl_cfg.h" + +#define CFG_25 703 +#define CFG_24 699 +#define CFG_26 707 +#define CFG_21 802 +#define CFG_20 645 +#define CFG_23 814 +#define CFG_TAP4 808 +#define CFG_TAP5 809 +#define CFG_TAP6 810 +#define CFG_1 783 +#define CFG_TAP0 802 +#define CFG_TAP1 804 +#define CFG_TAP2 805 +#define CFG_TAP3 806 +#define FCFG_AZ 878 +#define CFG_ORIENT_IRQ_1 715 +#define CFG_ORIENT_IRQ_2 738 +#define CFG_ORIENT_IRQ_3 743 +#define CFG_TAP_QUANTIZE 647 +#define FCFG_3 936 +#define CFG_TAP_CLEAR_STICKY 817 +#define FCFG_1 868 +#define CFG_ACCEL_FILTER 968 +#define FCFG_2 872 +#define CFG_3D 521 +#define CFG_3B 517 +#define CFG_3C 519 +#define FCFG_5 942 +#define FCFG_4 857 +#define FCFG_FSCALE 877 +#define CFG_TAP_JERK 639 +#define FCFG_6 996 +#define CFG_12 797 +#define FCFG_7 930 +#define CFG_14 790 +#define CFG_15 790 +#define CFG_16 815 +#define CFG_18 551 +#define CFG_6 823 +#define CFG_7 564 +#define CFG_4 526 +#define CFG_5 749 +#define CFG_3 515 +#define CFG_GYRO_SOURCE 777 +#define CFG_8 772 +#define CFG_9 778 +#define CFG_ORIENT_2 733 +#define CFG_ORIENT_1 713 +#define FCFG_ACCEL_INPUT 904 +#define CFG_TAP7 811 +#define CFG_TAP_SAVE_ACCB 687 +#define FCFG_ACCEL_INIT 831 + + +#define D_0_22 (22) +#define D_0_24 (24) +#define D_0_36 (36) +#define D_0_52 (52) +#define D_0_96 (96) +#define D_0_104 (104) +#define D_0_108 (108) +#define D_0_163 (163) +#define D_0_188 (188) +#define D_0_192 (192) +#define D_0_224 (224) +#define D_0_228 (228) +#define D_0_232 (232) +#define D_0_236 (236) + +#define D_1_2 (256 + 2) +#define D_1_4 (256 + 4) +#define D_1_8 (256 + 8) +#define D_1_10 (256 + 10) +#define D_1_24 (256 + 24) +#define D_1_28 (256 + 28) +#define D_1_92 (256 + 92) +#define D_1_96 (256 + 96) +#define D_1_98 (256 + 98) +#define D_1_106 (256 + 106) +#define D_1_108 (256 + 108) +#define D_1_112 (256 + 112) +#define D_1_128 (256 + 144) +#define D_1_152 (256 + 12) +#define D_1_168 (256 + 168) +#define D_1_175 (256 + 175) +#define D_1_178 (256 + 178) +#define D_1_236 (256 + 236) +#define D_1_244 (256 + 244) + + +static const tKeyLabel dmpTConfig[] = { + {KEY_CFG_25, CFG_25}, + {KEY_CFG_24, CFG_24}, + {KEY_CFG_26, CFG_26}, + {KEY_CFG_21, CFG_21}, + {KEY_CFG_20, CFG_20}, + {KEY_CFG_23, CFG_23}, + {KEY_CFG_TAP4, CFG_TAP4}, + {KEY_CFG_TAP5, CFG_TAP5}, + {KEY_CFG_TAP6, CFG_TAP6}, + {KEY_CFG_1, CFG_1}, + {KEY_CFG_TAP0, CFG_TAP0}, + {KEY_CFG_TAP1, CFG_TAP1}, + {KEY_CFG_TAP2, CFG_TAP2}, + {KEY_CFG_TAP3, CFG_TAP3}, + {KEY_FCFG_AZ, FCFG_AZ}, + {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1}, + {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2}, + {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3}, + {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE}, + {KEY_FCFG_3, FCFG_3}, + {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY}, + {KEY_FCFG_1, FCFG_1}, + //{KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER}, + {KEY_FCFG_2, FCFG_2}, + {KEY_CFG_3D, CFG_3D}, + {KEY_CFG_3B, CFG_3B}, + {KEY_CFG_3C, CFG_3C}, + {KEY_FCFG_5, FCFG_5}, + {KEY_FCFG_4, FCFG_4}, + {KEY_FCFG_FSCALE, FCFG_FSCALE}, + {KEY_CFG_TAP_JERK, CFG_TAP_JERK}, + {KEY_FCFG_6, FCFG_6}, + {KEY_CFG_12, CFG_12}, + {KEY_FCFG_7, FCFG_7}, + {KEY_CFG_14, CFG_14}, + {KEY_CFG_15, CFG_15}, + {KEY_CFG_16, CFG_16}, + {KEY_CFG_18, CFG_18}, + {KEY_CFG_6, CFG_6}, + {KEY_CFG_7, CFG_7}, + {KEY_CFG_4, CFG_4}, + {KEY_CFG_5, CFG_5}, + {KEY_CFG_3, CFG_3}, + {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE}, + {KEY_CFG_8, CFG_8}, + {KEY_CFG_9, CFG_9}, + {KEY_CFG_ORIENT_2, CFG_ORIENT_2}, + {KEY_CFG_ORIENT_1, CFG_ORIENT_1}, + {KEY_FCFG_ACCEL_INPUT, FCFG_ACCEL_INPUT}, + {KEY_CFG_TAP7, CFG_TAP7}, + {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB}, + {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT}, + + {KEY_D_0_22, D_0_22}, + {KEY_D_0_24, D_0_24}, + {KEY_D_0_36, D_0_36}, + {KEY_D_0_52, D_0_52}, + {KEY_D_0_96, D_0_96}, + {KEY_D_0_104, D_0_104}, + {KEY_D_0_108, D_0_108}, + {KEY_D_0_163, D_0_163}, + {KEY_D_0_188, D_0_188}, + {KEY_D_0_192, D_0_192}, + {KEY_D_0_224, D_0_224}, + {KEY_D_0_228, D_0_228}, + {KEY_D_0_232, D_0_232}, + {KEY_D_0_236, D_0_236}, + + {KEY_DMP_PREVPTAT, DMP_PREVPTAT}, + {KEY_D_1_2, D_1_2}, + {KEY_D_1_4, D_1_4}, + {KEY_D_1_8, D_1_8}, + {KEY_D_1_10, D_1_10}, + {KEY_D_1_24, D_1_24}, + {KEY_D_1_28, D_1_28}, + {KEY_D_1_92, D_1_92}, + {KEY_D_1_96, D_1_96}, + {KEY_D_1_98, D_1_98}, + {KEY_D_1_106, D_1_106}, + {KEY_D_1_108, D_1_108}, + {KEY_D_1_112, D_1_112}, + {KEY_D_1_128, D_1_128}, + {KEY_D_1_152, D_1_152}, + {KEY_D_1_168, D_1_168}, + {KEY_D_1_175, D_1_175}, + {KEY_D_1_178, D_1_178}, + {KEY_D_1_236, D_1_236}, + {KEY_D_1_244, D_1_244}, + + {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN}, + {KEY_DMP_TAP_THR_X, DMP_TAP_THX}, + {KEY_DMP_TAP_THR_Y, DMP_TAP_THY}, + {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ}, + {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y}, + {KEY_DMP_SH_TH_X, DMP_SH_TH_X}, + {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z}, + {KEY_DMP_ORIENT, DMP_ORIENT} +}; + +#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) +static const unsigned short sConfig = 0x013f; +#define SCD (1024) +static const unsigned char dmpMemory[SCD] = { + 0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x5a, 0xd6, 0x96, 0x06, 0x3f, 0xa3, 0x00, 0x00, + 0x20, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x77, 0x8e, 0x00, 0x01, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01, + 0x02, 0x00, 0x00, 0x01, 0x04, 0x00, 0x00, 0x03, 0x06, 0x00, 0x00, 0x05, 0x01, 0xe9, 0xa2, 0x0f, + 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x3e, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f, + 0xfc, 0xba, 0xfa, 0x00, 0x01, 0x00, 0x80, 0x00, 0x02, 0x01, 0x80, 0x00, 0x03, 0x02, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0xb4, 0x8b, 0x00, 0x00, 0x7a, 0xdf, 0x00, 0x02, 0x5b, 0x2f, + 0x00, 0x7d, 0x32, 0xba, 0x00, 0x0a, 0x1e, 0xd1, 0x00, 0x3a, 0xe8, 0x25, 0x00, 0x00, 0x00, 0x00, + 0x3f, 0xd7, 0x96, 0x08, 0xff, 0xb3, 0x39, 0xf5, 0xfe, 0x11, 0x1b, 0x62, 0xfb, 0xf4, 0xb4, 0x52, + 0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc, + 0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4, + 0x0d, 0x68, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10, + + 0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8, + 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x3e, 0x80, 0x00, 0x00, 0x12, 0x82, 0x2d, 0x90, + 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x02, 0xff, 0xff, 0x00, 0x05, 0x02, 0x00, 0x00, 0x0c, + 0x00, 0x03, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x03, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0x00, 0xff, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, + 0xff, 0xec, 0x3f, 0xc8, 0xff, 0xee, 0x00, 0x00, 0xff, 0xfe, 0x40, 0x00, 0xff, 0xff, 0xff, 0xc8, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xff, 0xff, 0xff, 0xff, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x33, 0x00, 0x00, 0x03, 0x65, 0x00, 0x00, 0x00, 0x99, 0x00, 0x00, 0x02, 0xf5, + + 0x9e, 0xc5, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, 0xc1, 0x83, 0x06, 0x26, + 0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xc4, 0xad, 0x00, 0x2c, 0x54, 0x7c, 0xf9, 0xc5, 0xa3, + 0xc1, 0xc3, 0x8f, 0x96, 0x19, 0xa6, 0x81, 0xda, 0x0c, 0xd9, 0x2e, 0xd8, 0xa3, 0x86, 0x31, 0x81, + 0xa6, 0xd9, 0x30, 0x26, 0xd8, 0xd8, 0xfa, 0xc1, 0x8c, 0xc2, 0x99, 0xc5, 0xa3, 0x2d, 0x55, 0x7d, + 0x81, 0x91, 0xac, 0x38, 0xad, 0x3a, 0xc3, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, + 0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9, + 0x04, 0xae, 0xd8, 0x79, 0xd9, 0x04, 0xd8, 0x81, 0xfb, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81, + 0xad, 0xd9, 0x01, 0xd8, 0xfa, 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad, + 0xad, 0xad, 0xad, 0xfb, 0x2a, 0xd8, 0xd8, 0xf9, 0xc0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xfb, + 0xac, 0x2e, 0x2e, 0xf9, 0xc1, 0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30, + 0x18, 0xa8, 0x98, 0x81, 0x28, 0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xfa, + 0xc0, 0x89, 0xac, 0x91, 0x2c, 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8, + 0xd8, 0x79, 0xd9, 0xd8, 0xd8, 0xf9, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9, + 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xc1, 0x83, 0x93, 0x35, 0x3d, 0x80, + 0x25, 0xda, 0xd8, 0xd8, 0x85, 0x69, 0xda, 0xd8, 0xd8, 0xf9, 0xc2, 0x93, 0x81, 0xa3, 0x28, 0x34, + 0x3c, 0xfb, 0x91, 0xab, 0x8b, 0x18, 0xa3, 0x09, 0xd9, 0xab, 0x97, 0x0a, 0x91, 0x3c, 0xc0, 0x87, + + 0x9c, 0xc5, 0xa3, 0xdd, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x95, 0xf9, 0xa3, 0xa3, 0xa3, 0x9d, 0xf9, + 0xa3, 0xa3, 0xa3, 0xa3, 0xf9, 0x90, 0xa3, 0xa3, 0xa3, 0xa3, 0x91, 0xc3, 0x99, 0xf9, 0xa3, 0xa3, + 0xa3, 0x98, 0xf9, 0xa3, 0xa3, 0xa3, 0xa3, 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xfb, 0x9b, 0xa3, 0xa3, + 0xdc, 0xc5, 0xa7, 0xf9, 0x26, 0x26, 0x26, 0xd8, 0xd8, 0xff, 0xd8, 0xd8, 0xd8, 0xd8, 0xd8, 0xc1, + 0xc2, 0xc4, 0x81, 0xa0, 0x90, 0xfa, 0x2c, 0x80, 0x74, 0xfb, 0x70, 0xfa, 0x7c, 0xc0, 0x86, 0x98, + 0xa8, 0xf9, 0xc9, 0x88, 0xa1, 0xfa, 0x0e, 0x97, 0x80, 0xf9, 0xa9, 0x2e, 0x2e, 0x2e, 0xaa, 0x2e, + 0x2e, 0x2e, 0xfa, 0xaa, 0xc9, 0x2c, 0xcb, 0xa9, 0x4c, 0xcd, 0x6c, 0xf9, 0x89, 0xa5, 0xca, 0xcd, + 0xcf, 0xc3, 0x9e, 0xa9, 0x3e, 0x5e, 0x7e, 0x85, 0xa5, 0x1a, 0x3e, 0x5e, 0xc2, 0xa5, 0x99, 0xfb, + 0x08, 0x34, 0x5c, 0xf9, 0xa9, 0xc9, 0xcb, 0xcd, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, + 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0x97, 0xa9, + 0xf9, 0x89, 0x26, 0x46, 0x66, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xaa, 0x98, 0x82, 0x87, 0x2d, + 0x35, 0x3d, 0xc5, 0xa3, 0xc2, 0xc1, 0x97, 0x80, 0x4a, 0x4e, 0x4e, 0xa3, 0xfa, 0x48, 0xcd, 0xc9, + 0xf9, 0xc4, 0xa9, 0x99, 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xc5, 0xa3, 0x2d, 0x55, 0x7d, 0xc3, 0x93, + 0xa3, 0x0e, 0x16, 0x1e, 0xa9, 0x2c, 0x54, 0x7c, 0xc0, 0xc2, 0x83, 0x97, 0xaf, 0x08, 0xc4, 0xa8, + 0x11, 0xc1, 0x8f, 0xc5, 0xaf, 0x98, 0xf8, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xf9, 0xa3, 0x29, + 0x55, 0x7d, 0xaf, 0x83, 0xc3, 0x93, 0xaf, 0xf8, 0x00, 0x28, 0x50, 0xc4, 0xc2, 0xc0, 0xf9, 0x97, +}; +static tKeyLabel keys[NUM_KEYS]; + +static unsigned short inv_setup_dmpGetAddress(unsigned short key) +{ + static int isSorted = 0; + if ( !isSorted ) { + int kk; + for (kk=0; kk= NUM_KEYS ) + return 0xffff; + return keys[ key ].addr; +} + + +/** + * @brief + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t inv_setup_dmp(void) +{ + inv_error_t result; + inv_set_get_address( inv_setup_dmpGetAddress ); + + result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_full_scale(2000.f); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_load_dmp(dmpMemory, SCD, sConfig); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_ignore_system_suspend(FALSE); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (inv_accel_present()) + { + struct ext_slave_config config; + long odr; + config.key = MPU_SLAVE_CONFIG_ODR_SUSPEND; + config.len = sizeof(long); + config.apply = FALSE; + config.data = &odr; + + odr = 0; + result = inv_mpu_config_accel(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), + &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + config.key = MPU_SLAVE_CONFIG_ODR_RESUME; + odr = 200000; + result = inv_mpu_config_accel(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), + &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + config.key = MPU_SLAVE_CONFIG_IRQ_SUSPEND; + odr = MPU_SLAVE_IRQ_TYPE_NONE; + result = inv_mpu_config_accel(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), + &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; + odr = MPU_SLAVE_IRQ_TYPE_NONE; + result = inv_mpu_config_accel(inv_get_dl_config(), + inv_get_serial_handle(), + inv_get_serial_handle(), + &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + } + + return result; +} +/** + * @} + */ + diff --git a/60xx/mlsdk/mllite/dmpDefault.h b/60xx/mlsdk/mllite/dmpDefault.h new file mode 100644 index 0000000..0670977 --- /dev/null +++ b/60xx/mlsdk/mllite/dmpDefault.h @@ -0,0 +1,42 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#ifndef ML_DMPDEFAULT_H__ +#define ML_DMPDEFAULT_H__ + +/** + * @defgroup DEFAULT + * @brief Default DMP assembly listing header file + * + * @{ + * @file inv_setup_dmp.c + * @brief Default DMP assembly listing header file + */ + + +#ifdef __cplusplus +extern "C" +{ +#endif + inv_error_t inv_setup_dmp(void); + +#ifdef __cplusplus +} +#endif + + +#endif // ML_DMPDEFAULT_H__ diff --git a/60xx/mlsdk/mllite/dmpDefaultMantis.c b/60xx/mlsdk/mllite/dmpDefaultMantis.c new file mode 100644 index 0000000..f35aaca --- /dev/null +++ b/60xx/mlsdk/mllite/dmpDefaultMantis.c @@ -0,0 +1,467 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/* WARNING: autogenerated code, do not modify */ +/** + * @defgroup DMPDEFAULT + * @brief + * + * @{ + * @file inv_setup_dmpMantis.c + * @brief + * + * + */ + +#include "mltypes.h" +#include "dmpDefault.h" +#include "dmpKey.h" +#include "dmpmap.h" +#include "mldl.h" + +#define CFG_25 1786 +#define CFG_24 1782 +#define CFG_26 1790 +#define CFG_21 1899 +#define CFG_20 1728 +#define CFG_23 1911 +#define CFG_TAP4 1905 +#define CFG_TAP5 1906 +#define CFG_TAP6 1907 +#define CFG_1 1865 +#define CFG_TAP0 1899 +#define CFG_TAP1 1901 +#define CFG_TAP2 1902 +#define CFG_TAP_SAVE_ACCB 1770 +#define CFG_ORIENT_IRQ_1 1798 +#define CFG_ORIENT_IRQ_2 1821 +#define CFG_ORIENT_IRQ_3 1826 +#define FCFG_MAG_VAL 774 +#define CFG_TAP_QUANTIZE 1730 +#define FCFG_3 936 +#define FCFG_1 891 +#define CFG_ACCEL_FILTER 1076 +#define FCFG_2 895 +#define CFG_3D 1629 +#define FCFG_7 902 +#define CFG_3C 1627 +#define FCFG_5 1030 +#define FCFG_4 880 +#define CFG_3B 1625 +#define CFG_TAP_JERK 1722 +#define FCFG_6 954 +#define CFG_12 1894 +#define CFG_TAP7 1908 +#define CFG_14 1871 +#define CFG_15 1876 +#define CFG_16 1912 +#define CFG_TAP_CLEAR_STICKY 1914 +#define CFG_6 1920 +#define CFG_7 1014 +#define CFG_4 1634 +#define CFG_5 1831 +#define CFG_3 1623 +#define CFG_GYRO_SOURCE 1859 +#define CFG_TAP3 1903 +#define CFG_EXTERNAL 1884 +#define CFG_8 1854 +#define CFG_9 1860 +#define CFG_ORIENT_2 1816 +#define CFG_ORIENT_1 1796 +#define CFG_MOTION_BIAS 1023 +#define FCFG_MAG_MOV 791 +#define TEMPLABEL 1491 +#define FCFG_ACCEL_INIT 847 + + +#define D_0_22 (22+512) +#define D_0_24 (24+512) + +#define D_0_36 (36) +#define D_0_52 (52) +#define D_0_96 (96) +#define D_0_104 (104) +#define D_0_108 (108) +#define D_0_163 (163) +#define D_0_188 (188) +#define D_0_192 (192) +#define D_0_224 (224) +#define D_0_228 (228) +#define D_0_232 (232) +#define D_0_236 (236) + +#define D_1_2 (256 + 2) +#define D_1_4 (256 + 4) +#define D_1_8 (256 + 8) +#define D_1_10 (256 + 10) +#define D_1_24 (256 + 24) +#define D_1_28 (256 + 28) +#define D_1_92 (256 + 92) +#define D_1_96 (256 + 96) +#define D_1_98 (256 + 98) +#define D_1_106 (256 + 106) +#define D_1_108 (256 + 108) +#define D_1_112 (256 + 112) +#define D_1_128 (256 + 144) +#define D_1_152 (256 + 12) +#define D_1_168 (256 + 168) +#define D_1_175 (256 + 175) +#define D_1_178 (256 + 178) +#define D_1_236 (256 + 236) +#define D_1_244 (256 + 244) +#define D_2_12 (512+12) +#define D_2_96 (512+96) +#define D_2_108 (512+108) +#define D_2_244 (512+244) +#define D_2_248 (512+248) +#define D_2_252 (512+252) + +#define CPASS_BIAS_X (35*16+4) +#define CPASS_BIAS_Y (35*16+8) +#define CPASS_BIAS_Z (35*16+12) +#define CPASS_MTX_00 (36*16) +#define CPASS_MTX_01 (36*16+4) +#define CPASS_MTX_02 (36*16+8) +#define CPASS_MTX_10 (36*16+12) +#define CPASS_MTX_11 (37*16) +#define CPASS_MTX_12 (37*16+4) +#define CPASS_MTX_20 (37*16+8) +#define CPASS_MTX_21 (37*16+12) +#define CPASS_MTX_22 (43*16+12) +#define D_ACT0 (40*16) +#define D_ACSX (40*16+4) +#define D_ACSY (40*16+8) +#define D_ACSZ (40*16+12) + +static const tKeyLabel dmpTConfig[] = { + {KEY_CFG_25, CFG_25}, + {KEY_CFG_24, CFG_24}, + {KEY_CFG_26, CFG_26}, + {KEY_CFG_21, CFG_21}, + {KEY_CFG_20, CFG_20}, + {KEY_CFG_23, CFG_23}, + {KEY_CFG_TAP4, CFG_TAP4}, + {KEY_CFG_TAP5, CFG_TAP5}, + {KEY_CFG_TAP6, CFG_TAP6}, + {KEY_CFG_1, CFG_1}, + {KEY_CFG_TAP0, CFG_TAP0}, + {KEY_CFG_TAP1, CFG_TAP1}, + {KEY_CFG_TAP2, CFG_TAP2}, + {KEY_CFG_TAP_SAVE_ACCB, CFG_TAP_SAVE_ACCB}, + {KEY_CFG_ORIENT_IRQ_1, CFG_ORIENT_IRQ_1}, + {KEY_CFG_ORIENT_IRQ_2, CFG_ORIENT_IRQ_2}, + {KEY_CFG_ORIENT_IRQ_3, CFG_ORIENT_IRQ_3}, + {KEY_FCFG_MAG_VAL, FCFG_MAG_VAL}, + {KEY_CFG_TAP_QUANTIZE, CFG_TAP_QUANTIZE}, + {KEY_FCFG_3, FCFG_3}, + {KEY_FCFG_1, FCFG_1}, +// {KEY_CFG_ACCEL_FILTER, CFG_ACCEL_FILTER}, + {KEY_FCFG_2, FCFG_2}, + {KEY_CFG_3D, CFG_3D}, + {KEY_FCFG_7, FCFG_7}, + {KEY_CFG_3C, CFG_3C}, + {KEY_FCFG_5, FCFG_5}, + {KEY_FCFG_4, FCFG_4}, + {KEY_CFG_3B, CFG_3B}, + {KEY_CFG_TAP_JERK, CFG_TAP_JERK}, + {KEY_FCFG_6, FCFG_6}, + {KEY_CFG_12, CFG_12}, + {KEY_CFG_TAP7, CFG_TAP7}, + {KEY_CFG_14, CFG_14}, + {KEY_CFG_15, CFG_15}, + {KEY_CFG_16, CFG_16}, + {KEY_CFG_TAP_CLEAR_STICKY, CFG_TAP_CLEAR_STICKY}, + {KEY_CFG_6, CFG_6}, + {KEY_CFG_7, CFG_7}, + {KEY_CFG_4, CFG_4}, + {KEY_CFG_5, CFG_5}, + {KEY_CFG_3, CFG_3}, + {KEY_CFG_GYRO_SOURCE, CFG_GYRO_SOURCE}, + {KEY_CFG_TAP3, CFG_TAP3}, + {KEY_CFG_EXTERNAL, CFG_EXTERNAL}, + {KEY_CFG_8, CFG_8}, + {KEY_CFG_9, CFG_9}, + {KEY_CFG_ORIENT_2, CFG_ORIENT_2}, + {KEY_CFG_ORIENT_1, CFG_ORIENT_1}, + {KEY_CFG_MOTION_BIAS, CFG_MOTION_BIAS}, + {KEY_FCFG_MAG_MOV, FCFG_MAG_MOV}, +// {KEY_TEMPLABEL, TEMPLABEL}, + {KEY_FCFG_ACCEL_INIT, FCFG_ACCEL_INIT}, + {KEY_D_0_22, D_0_22}, + {KEY_D_0_24, D_0_24}, + {KEY_D_0_36, D_0_36}, + {KEY_D_0_52, D_0_52}, + {KEY_D_0_96, D_0_96}, + {KEY_D_0_104, D_0_104}, + {KEY_D_0_108, D_0_108}, + {KEY_D_0_163, D_0_163}, + {KEY_D_0_188, D_0_188}, + {KEY_D_0_192, D_0_192}, + {KEY_D_0_224, D_0_224}, + {KEY_D_0_228, D_0_228}, + {KEY_D_0_232, D_0_232}, + {KEY_D_0_236, D_0_236}, + + {KEY_DMP_PREVPTAT, DMP_PREVPTAT}, + {KEY_D_1_2, D_1_2}, + {KEY_D_1_4, D_1_4}, + {KEY_D_1_8, D_1_8}, + {KEY_D_1_10, D_1_10}, + {KEY_D_1_24, D_1_24}, + {KEY_D_1_28, D_1_28}, + {KEY_D_1_92, D_1_92}, + {KEY_D_1_96, D_1_96}, + {KEY_D_1_98, D_1_98}, + {KEY_D_1_106, D_1_106}, + {KEY_D_1_108, D_1_108}, + {KEY_D_1_112, D_1_112}, + {KEY_D_1_128, D_1_128}, + {KEY_D_1_152, D_1_152}, + {KEY_D_1_168, D_1_168}, + {KEY_D_1_175, D_1_175}, + {KEY_D_1_178, D_1_178}, + {KEY_D_1_236, D_1_236}, + {KEY_D_1_244, D_1_244}, + + {KEY_D_2_12, D_2_12}, + {KEY_D_2_96, D_2_96}, + {KEY_D_2_108, D_2_108}, + {KEY_D_2_244, D_2_244}, + {KEY_D_2_248, D_2_248}, + {KEY_D_2_252, D_2_252}, + + {KEY_DMP_TAPW_MIN, DMP_TAPW_MIN}, + {KEY_DMP_TAP_THR_X, DMP_TAP_THX}, + {KEY_DMP_TAP_THR_Y, DMP_TAP_THY}, + {KEY_DMP_TAP_THR_Z, DMP_TAP_THZ}, + {KEY_DMP_SH_TH_Y, DMP_SH_TH_Y}, + {KEY_DMP_SH_TH_X, DMP_SH_TH_X}, + {KEY_DMP_SH_TH_Z, DMP_SH_TH_Z}, + {KEY_DMP_ORIENT, DMP_ORIENT}, + + {KEY_CPASS_BIAS_X, CPASS_BIAS_X}, + {KEY_CPASS_BIAS_Y, CPASS_BIAS_Y}, + {KEY_CPASS_BIAS_Z, CPASS_BIAS_Z}, + {KEY_CPASS_MTX_00, CPASS_MTX_00}, + {KEY_CPASS_MTX_01, CPASS_MTX_01}, + {KEY_CPASS_MTX_02, CPASS_MTX_02}, + {KEY_CPASS_MTX_10, CPASS_MTX_10}, + {KEY_CPASS_MTX_11, CPASS_MTX_11}, + {KEY_CPASS_MTX_12, CPASS_MTX_12}, + {KEY_CPASS_MTX_20, CPASS_MTX_20}, + {KEY_CPASS_MTX_21, CPASS_MTX_21}, + {KEY_CPASS_MTX_22, CPASS_MTX_22}, + {KEY_D_ACT0, D_ACT0}, + {KEY_D_ACSX, D_ACSX}, + {KEY_D_ACSY, D_ACSY}, + {KEY_D_ACSZ, D_ACSZ} +}; +#define NUM_LOCAL_KEYS (sizeof(dmpTConfig)/sizeof(dmpTConfig[0])) + +#define DMP_CODE_SIZE 1923 + +static const unsigned char dmpMemory[DMP_CODE_SIZE] = { + 0xfb, 0x00, 0x00, 0x3e, 0x00, 0x0b, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0xfa, 0x80, 0x00, 0x0b, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xff, 0xff, 0x45, 0x81, 0xff, 0xff, 0xfa, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xe8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7f, 0xff, 0xff, 0xfe, 0x80, 0x01, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3e, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xca, 0xe3, 0x09, 0x3e, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xff, 0x00, 0x00, 0x00, 0x00, 0x0b, 0x2a, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xfd, 0x87, 0x26, 0x50, 0xfd, 0x80, 0x00, 0x00, 0x00, 0x1f, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6f, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5e, 0xc0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xfb, 0x8c, 0x6f, 0x5d, 0xfd, 0x5d, 0x08, 0xd9, 0x00, 0x7c, 0x73, 0x3b, 0x00, 0x6c, 0x12, 0xcc, + 0x32, 0x00, 0x13, 0x9d, 0x32, 0x00, 0xd0, 0xd6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xf4, + 0xff, 0xe6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xd6, 0x00, 0x00, 0x27, 0x10, + 0xfb, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xfa, 0x36, 0xff, 0xbc, 0x30, 0x8e, 0x00, 0x05, 0xfb, 0xf0, 0xff, 0xd9, 0x5b, 0xc8, + 0xff, 0xd0, 0x9a, 0xbe, 0x00, 0x00, 0x10, 0xa9, 0xff, 0xf4, 0x1e, 0xb2, 0x00, 0xce, 0xbb, 0xf7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0c, + 0xff, 0xc2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xcf, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3f, 0x68, 0xb6, 0x79, 0x35, 0x28, 0xbc, 0xc6, 0x7e, 0xd1, 0x6c, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xb2, 0x6a, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3f, 0xf0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x25, 0x4d, 0x00, 0x2f, 0x70, 0x6d, 0x00, 0x00, 0x05, 0xae, 0x00, 0x0c, 0x02, 0xd0, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xff, 0xef, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xff, 0xef, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x1b, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + 0xd8, 0xdc, 0xba, 0xa2, 0xf1, 0xde, 0xb2, 0xb8, 0xb4, 0xa8, 0x81, 0x91, 0xf7, 0x4a, 0x90, 0x7f, + 0x91, 0x6a, 0xf3, 0xf9, 0xdb, 0xa8, 0xf9, 0xb0, 0xba, 0xa0, 0x80, 0xf2, 0xce, 0x81, 0xf3, 0xc2, + 0xf1, 0xc1, 0xf2, 0xc3, 0xf3, 0xcc, 0xa2, 0xb2, 0x80, 0xf1, 0xc6, 0xd8, 0x80, 0xba, 0xa7, 0xdf, + 0xdf, 0xdf, 0xf2, 0xa7, 0xc3, 0xcb, 0xc5, 0xb6, 0xf0, 0x87, 0xa2, 0x94, 0x24, 0x48, 0x70, 0x3c, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9b, 0x78, 0xa2, 0xf1, 0x83, 0x92, 0x2d, 0x55, 0x7d, 0xd8, 0xb1, + 0xb4, 0xb8, 0xa1, 0xd0, 0x91, 0x80, 0xf2, 0x70, 0xf3, 0x70, 0xf2, 0x7c, 0x80, 0xa8, 0xf1, 0x01, + 0xb0, 0x98, 0x87, 0xd9, 0x43, 0xd8, 0x86, 0xc9, 0x88, 0xba, 0xa1, 0xf2, 0x0e, 0xb8, 0x97, 0x80, + 0xf1, 0xa9, 0xdf, 0xdf, 0xdf, 0xaa, 0xdf, 0xdf, 0xdf, 0xf2, 0xaa, 0xc5, 0xcd, 0xc7, 0xa9, 0xc1, + 0xc9, 0xc3, 0x97, 0xf1, 0xa9, 0x89, 0x26, 0x46, 0x66, 0xb0, 0xb4, 0xba, 0x80, 0xac, 0xde, 0xf2, + 0xca, 0xf1, 0xb2, 0x8c, 0x02, 0xa9, 0xb6, 0x98, 0x00, 0x89, 0x0e, 0x16, 0x1e, 0xb8, 0xa9, 0xb4, + 0x99, 0x2c, 0x54, 0x7c, 0xb0, 0x8a, 0xa8, 0x96, 0x36, 0x56, 0x76, 0xf1, 0xb9, 0xaf, 0xb4, 0xb0, + 0x83, 0xc0, 0xb8, 0xa8, 0x97, 0x11, 0xb1, 0x8f, 0x98, 0xb9, 0xaf, 0xf0, 0x24, 0x08, 0x44, 0x10, + 0x64, 0x18, 0xf1, 0xa3, 0x29, 0x55, 0x7d, 0xaf, 0x83, 0xb5, 0x93, 0xaf, 0xf0, 0x00, 0x28, 0x50, + 0xf1, 0xa3, 0x86, 0x9f, 0x61, 0xa6, 0xda, 0xde, 0xdf, 0xd9, 0xfa, 0xa3, 0x86, 0x96, 0xdb, 0x31, + 0xa6, 0xd9, 0xf8, 0xdf, 0xba, 0xa6, 0x8f, 0xc2, 0xc5, 0xc7, 0xb2, 0x8c, 0xc1, 0xb8, 0xa2, 0xdf, + 0xdf, 0xdf, 0xa3, 0xdf, 0xdf, 0xdf, 0xd8, 0xd8, 0xf1, 0xb8, 0xa8, 0xb2, 0x86, 0xb4, 0x98, 0x0d, + 0x35, 0x5d, 0xb8, 0xaa, 0x98, 0xb0, 0x87, 0x2d, 0x35, 0x3d, 0xb2, 0xb6, 0xba, 0xaf, 0x8c, 0x96, + 0x19, 0x8f, 0x9f, 0xa7, 0x0e, 0x16, 0x1e, 0xb4, 0x9a, 0xb8, 0xaa, 0x87, 0x2c, 0x54, 0x7c, 0xb9, + 0xa3, 0xde, 0xdf, 0xdf, 0xa3, 0xb1, 0x80, 0xf2, 0xc4, 0xcd, 0xc9, 0xf1, 0xb8, 0xa9, 0xb4, 0x99, + 0x83, 0x0d, 0x35, 0x5d, 0x89, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0xb5, 0x93, 0xa3, 0x0e, 0x16, 0x1e, + 0xa9, 0x2c, 0x54, 0x7c, 0xb8, 0xb4, 0xb0, 0xf1, 0x97, 0x83, 0xa8, 0x11, 0x84, 0xa5, 0x09, 0x98, + 0xa3, 0x83, 0xf0, 0xda, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xd8, 0xf1, 0xa5, 0x29, 0x55, 0x7d, + 0xa5, 0x85, 0x95, 0x02, 0x1a, 0x2e, 0x3a, 0x56, 0x5a, 0x40, 0x48, 0xf9, 0xf3, 0xa3, 0xd9, 0xf8, + 0xf0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xa8, 0xf1, 0x11, 0xf0, 0x98, + 0xa2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xda, 0xf3, 0xde, 0xd8, 0x83, 0xa5, 0x94, 0x01, 0xd9, + 0xa3, 0x02, 0xf1, 0xa2, 0xc3, 0xc5, 0xc7, 0xd8, 0xf1, 0x84, 0x92, 0xa2, 0x4d, 0xda, 0x2a, 0xd8, + 0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, + 0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0x93, 0xa3, 0x4d, 0xda, 0x2a, 0xd8, + 0x48, 0x69, 0xd9, 0x2a, 0xd8, 0x68, 0x55, 0xda, 0x32, 0xd8, 0x50, 0x71, 0xd9, 0x32, 0xd8, 0x70, + 0x5d, 0xda, 0x3a, 0xd8, 0x58, 0x79, 0xd9, 0x3a, 0xd8, 0x78, 0xa8, 0x8a, 0x9a, 0xf0, 0x28, 0x50, + 0x78, 0x9e, 0xf3, 0x88, 0x18, 0xf1, 0x9f, 0x1d, 0x98, 0xa8, 0xd9, 0x08, 0xd8, 0xc8, 0x9f, 0x12, + 0x9e, 0xf3, 0x15, 0xa8, 0xda, 0x12, 0x10, 0xd8, 0xf1, 0xaf, 0xc8, 0x97, 0x87, 0x34, 0xb5, 0xb9, + 0x94, 0xa4, 0x21, 0xf3, 0xd9, 0x22, 0xd8, 0xf2, 0x2d, 0xf3, 0xd9, 0x2a, 0xd8, 0xf2, 0x35, 0xf3, + 0xd9, 0x32, 0xd8, 0x81, 0xa4, 0x60, 0x60, 0x61, 0xd9, 0x61, 0xd8, 0x6c, 0x68, 0x69, 0xd9, 0x69, + 0xd8, 0x74, 0x70, 0x71, 0xd9, 0x71, 0xd8, 0xb1, 0xa3, 0x84, 0x19, 0x3d, 0x5d, 0xa3, 0x83, 0x1a, + 0x3e, 0x5e, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xb8, 0xb0, 0xaf, 0x8f, 0x94, 0xf2, 0xda, 0x3e, + 0xd8, 0xb4, 0x9a, 0xa8, 0x87, 0x29, 0xda, 0xf8, 0xd8, 0x87, 0x9a, 0x35, 0xda, 0xf8, 0xd8, 0x87, + 0x9a, 0x3d, 0xda, 0xf8, 0xd8, 0xb1, 0xb9, 0xa4, 0x98, 0x85, 0x02, 0x2e, 0x56, 0xa5, 0x81, 0x00, + 0x0c, 0x14, 0xa3, 0x97, 0xb0, 0x8a, 0xf1, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, + 0xd9, 0x68, 0xd8, 0xb1, 0x84, 0x0d, 0xda, 0x0e, 0xd8, 0xa3, 0x29, 0x83, 0xda, 0x2c, 0x0e, 0xd8, + 0xa3, 0x84, 0x49, 0x83, 0xda, 0x2c, 0x4c, 0x0e, 0xd8, 0xb8, 0xb0, 0xa8, 0x8a, 0x9a, 0xf5, 0x20, + 0xaa, 0xda, 0xdf, 0xd8, 0xa8, 0x40, 0xaa, 0xd0, 0xda, 0xde, 0xd8, 0xa8, 0x60, 0xaa, 0xda, 0xd0, + 0xdf, 0xd8, 0xf1, 0x97, 0x86, 0xa8, 0x31, 0x9b, 0x06, 0x99, 0x07, 0xab, 0x97, 0x28, 0x88, 0x9b, + 0xf0, 0x0c, 0x20, 0x14, 0x40, 0xb8, 0xb0, 0xb4, 0xa8, 0x8c, 0x9c, 0xf0, 0x04, 0x28, 0x51, 0x79, + 0x1d, 0x30, 0x14, 0x38, 0xb2, 0x82, 0xab, 0xd0, 0x98, 0x2c, 0x50, 0x50, 0x78, 0x78, 0x9b, 0xf1, + 0x1a, 0xb0, 0xf0, 0x8a, 0x9c, 0xa8, 0x29, 0x51, 0x79, 0x8b, 0x29, 0x51, 0x79, 0x8a, 0x24, 0x70, + 0x59, 0x8b, 0x20, 0x58, 0x71, 0x8a, 0x44, 0x69, 0x38, 0x8b, 0x39, 0x40, 0x68, 0x8a, 0x64, 0x48, + 0x31, 0x8b, 0x30, 0x49, 0x60, 0xa5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, + 0x49, 0x30, 0x19, 0xf1, 0xac, 0x00, 0x2c, 0x54, 0x7c, 0xf0, 0x8c, 0xa8, 0x04, 0x28, 0x50, 0x78, + 0xf1, 0x88, 0x97, 0x26, 0xa8, 0x59, 0x98, 0xac, 0x8c, 0x02, 0x26, 0x46, 0x66, 0xf0, 0x89, 0x9c, + 0xa8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xa9, 0x88, 0x09, + 0x20, 0x59, 0x70, 0xab, 0x11, 0x38, 0x40, 0x69, 0xa8, 0x19, 0x31, 0x48, 0x60, 0x8c, 0xa8, 0x3c, + 0x41, 0x5c, 0x20, 0x7c, 0x00, 0xf1, 0x87, 0x98, 0x19, 0x86, 0xa8, 0x6e, 0x76, 0x7e, 0xa9, 0x99, + 0x88, 0x2d, 0x55, 0x7d, 0x9e, 0xb9, 0xa3, 0x8a, 0x22, 0x8a, 0x6e, 0x8a, 0x56, 0x8a, 0x5e, 0x9f, + 0xb1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0e, 0x2e, 0x4e, 0x6e, 0x9d, 0xb8, 0xad, 0x00, 0x2c, 0x54, + 0x7c, 0xf2, 0xb1, 0x8c, 0xb4, 0x99, 0xb9, 0xa3, 0x2d, 0x55, 0x7d, 0x81, 0x91, 0xac, 0x38, 0xad, + 0x3a, 0xb5, 0x83, 0x91, 0xac, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, 0xd8, 0x6d, 0xd9, 0x68, + 0xd8, 0x8c, 0x9d, 0xae, 0x29, 0xd9, 0x04, 0xae, 0xd8, 0x51, 0xd9, 0x04, 0xae, 0xd8, 0x79, 0xd9, + 0x04, 0xd8, 0x81, 0xf3, 0x9d, 0xad, 0x00, 0x8d, 0xae, 0x19, 0x81, 0xad, 0xd9, 0x01, 0xd8, 0xf2, + 0xae, 0xda, 0x26, 0xd8, 0x8e, 0x91, 0x29, 0x83, 0xa7, 0xd9, 0xad, 0xad, 0xad, 0xad, 0xf3, 0x2a, + 0xd8, 0xd8, 0xf1, 0xb0, 0xac, 0x89, 0x91, 0x3e, 0x5e, 0x76, 0xf3, 0xac, 0x2e, 0x2e, 0xf1, 0xb1, + 0x8c, 0x5a, 0x9c, 0xac, 0x2c, 0x28, 0x28, 0x28, 0x9c, 0xac, 0x30, 0x18, 0xa8, 0x98, 0x81, 0x28, + 0x34, 0x3c, 0x97, 0x24, 0xa7, 0x28, 0x34, 0x3c, 0x9c, 0x24, 0xf2, 0xb0, 0x89, 0xac, 0x91, 0x2c, + 0x4c, 0x6c, 0x8a, 0x9b, 0x2d, 0xd9, 0xd8, 0xd8, 0x51, 0xd9, 0xd8, 0xd8, 0x79, 0xd9, 0xd8, 0xd8, + 0xf1, 0x9e, 0x88, 0xa3, 0x31, 0xda, 0xd8, 0xd8, 0x91, 0x2d, 0xd9, 0x28, 0xd8, 0x4d, 0xd9, 0x48, + 0xd8, 0x6d, 0xd9, 0x68, 0xd8, 0xb1, 0x83, 0x93, 0x35, 0x3d, 0x80, 0x25, 0xda, 0xd8, 0xd8, 0x85, + 0x69, 0xda, 0xd8, 0xd8, 0xb4, 0x93, 0x81, 0xa3, 0x28, 0x34, 0x3c, 0xf3, 0xab, 0x8b, 0xf8, 0xa3, + 0x91, 0xb6, 0x09, 0xb4, 0xd9, 0xab, 0xde, 0xfa, 0xb0, 0x87, 0x9c, 0xb9, 0xa3, 0xdd, 0xf1, 0xa3, + 0xa3, 0xa3, 0xa3, 0x95, 0xf1, 0xa3, 0xa3, 0xa3, 0x9d, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, 0xf2, 0xa3, + 0xb4, 0x90, 0x80, 0xf2, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xa3, 0xb2, 0xa3, 0xa3, 0xa3, 0xa3, + 0xa3, 0xa3, 0xb0, 0x87, 0xb5, 0x99, 0xf1, 0xa3, 0xa3, 0xa3, 0x98, 0xf1, 0xa3, 0xa3, 0xa3, 0xa3, + 0x97, 0xa3, 0xa3, 0xa3, 0xa3, 0xf3, 0x9b, 0xa3, 0xa3, 0xdc, 0xb9, 0xa7, 0xf1, 0x26, 0x26, 0x26, + 0xd8, 0xd8, 0xff +}; + +static unsigned short sStartAddress = 0x0300; + + +static tKeyLabel keys[NUM_KEYS]; + +static unsigned short inv_setup_dmpGetAddress( unsigned short key ) +{ + static int isSorted = 0; + if ( !isSorted ) { + int kk; + for (kk=0; kk= NUM_KEYS ) + return 0xffff; + return keys[ key ].addr; +} + +/** + * @brief + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t inv_setup_dmp(void) + +{ + inv_error_t result; + + inv_set_get_address(inv_setup_dmpGetAddress); + result = inv_clock_source(MPU_CLK_SEL_PLLGYROZ); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_dl_cfg_sampling(MPU_FILTER_42HZ, 4); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_full_scale(2000.f); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_load_dmp(dmpMemory, DMP_CODE_SIZE, sStartAddress); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_external_sync(MPU_EXT_SYNC_TEMP); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} +/** + *@} + */ diff --git a/60xx/mlsdk/mllite/dmpKey.h b/60xx/mlsdk/mllite/dmpKey.h new file mode 100644 index 0000000..f152644 --- /dev/null +++ b/60xx/mlsdk/mllite/dmpKey.h @@ -0,0 +1,432 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#ifndef DMPKEY_H__ +#define DMPKEY_H__ + +#define KEY_CFG_25 0 +#define KEY_CFG_24 (KEY_CFG_25+1) +#define KEY_CFG_26 (KEY_CFG_24+1) +#define KEY_CFG_21 (KEY_CFG_26+1) +#define KEY_CFG_20 (KEY_CFG_21+1) +#define KEY_CFG_TAP4 (KEY_CFG_20+1) +#define KEY_CFG_TAP5 (KEY_CFG_TAP4+1) +#define KEY_CFG_TAP6 (KEY_CFG_TAP5+1) +#define KEY_CFG_TAP7 (KEY_CFG_TAP6+1) +#define KEY_CFG_TAP0 (KEY_CFG_TAP7+1) +#define KEY_CFG_TAP1 (KEY_CFG_TAP0+1) +#define KEY_CFG_TAP2 (KEY_CFG_TAP1+1) +#define KEY_CFG_TAP3 (KEY_CFG_TAP2+1) +#define KEY_CFG_TAP_QUANTIZE (KEY_CFG_TAP3+1) +#define KEY_CFG_TAP_JERK (KEY_CFG_TAP_QUANTIZE+1) +#define KEY_CFG_TAP_SAVE_ACCB (KEY_CFG_TAP_JERK+1) +#define KEY_CFG_TAP_CLEAR_STICKY (KEY_CFG_TAP_SAVE_ACCB+1) +#define KEY_FCFG_ACCEL_INPUT (KEY_CFG_TAP_CLEAR_STICKY +1) +#define KEY_FCFG_ACCEL_INIT (KEY_FCFG_ACCEL_INPUT+1) +#define KEY_CFG_23 (KEY_FCFG_ACCEL_INIT+1) +#define KEY_FCFG_1 (KEY_CFG_23+1) +#define KEY_FCFG_3 (KEY_FCFG_1+1) +#define KEY_FCFG_2 (KEY_FCFG_3+1) +#define KEY_CFG_3D (KEY_FCFG_2+1) +#define KEY_CFG_3B (KEY_CFG_3D+1) +#define KEY_CFG_3C (KEY_CFG_3B+1) +#define KEY_FCFG_5 (KEY_CFG_3C+1) +#define KEY_FCFG_4 (KEY_FCFG_5+1) +#define KEY_FCFG_7 (KEY_FCFG_4+1) +#define KEY_FCFG_FSCALE (KEY_FCFG_7+1) +#define KEY_FCFG_AZ (KEY_FCFG_FSCALE+1) +#define KEY_FCFG_6 (KEY_FCFG_AZ+1) +#define KEY_FCFG_LSB4 (KEY_FCFG_6+1) +#define KEY_CFG_12 (KEY_FCFG_LSB4+1) +#define KEY_CFG_14 (KEY_CFG_12+1) +#define KEY_CFG_15 (KEY_CFG_14+1) +#define KEY_CFG_16 (KEY_CFG_15+1) +#define KEY_CFG_18 (KEY_CFG_16+1) +#define KEY_CFG_6 (KEY_CFG_18 + 1) +#define KEY_CFG_7 (KEY_CFG_6+1) +#define KEY_CFG_4 (KEY_CFG_7+1) +#define KEY_CFG_5 (KEY_CFG_4+1) +#define KEY_CFG_2 (KEY_CFG_5+1) +#define KEY_CFG_3 (KEY_CFG_2+1) +#define KEY_CFG_1 (KEY_CFG_3+1) +#define KEY_CFG_EXTERNAL (KEY_CFG_1+1) +#define KEY_CFG_8 (KEY_CFG_EXTERNAL+1) +#define KEY_CFG_9 (KEY_CFG_8+1) +#define KEY_CFG_ORIENT_3 (KEY_CFG_9 + 1) +#define KEY_CFG_ORIENT_2 (KEY_CFG_ORIENT_3 + 1) +#define KEY_CFG_ORIENT_1 (KEY_CFG_ORIENT_2 + 1) +#define KEY_CFG_GYRO_SOURCE (KEY_CFG_ORIENT_1 + 1) +#define KEY_CFG_ORIENT_IRQ_1 (KEY_CFG_GYRO_SOURCE + 1) +#define KEY_CFG_ORIENT_IRQ_2 (KEY_CFG_ORIENT_IRQ_1 + 1) +#define KEY_CFG_ORIENT_IRQ_3 (KEY_CFG_ORIENT_IRQ_2 + 1) +#define KEY_FCFG_MAG_VAL (KEY_CFG_ORIENT_IRQ_3 + 1) +#define KEY_FCFG_MAG_MOV (KEY_FCFG_MAG_VAL + 1) + +#define KEY_D_0_22 (KEY_FCFG_MAG_MOV + 1) +#define KEY_D_0_24 (KEY_D_0_22+1) +#define KEY_D_0_36 (KEY_D_0_24+1) +#define KEY_D_0_52 (KEY_D_0_36+1) +#define KEY_D_0_96 (KEY_D_0_52+1) +#define KEY_D_0_104 (KEY_D_0_96+1) +#define KEY_D_0_108 (KEY_D_0_104+1) +#define KEY_D_0_163 (KEY_D_0_108+1) +#define KEY_D_0_188 (KEY_D_0_163+1) +#define KEY_D_0_192 (KEY_D_0_188+1) +#define KEY_D_0_224 (KEY_D_0_192+1) +#define KEY_D_0_228 (KEY_D_0_224+1) +#define KEY_D_0_232 (KEY_D_0_228+1) +#define KEY_D_0_236 (KEY_D_0_232+1) + +#define KEY_DMP_PREVPTAT (KEY_D_0_236+1) +#define KEY_D_1_2 (KEY_DMP_PREVPTAT+1) +#define KEY_D_1_4 (KEY_D_1_2 + 1) +#define KEY_D_1_8 (KEY_D_1_4 + 1) +#define KEY_D_1_10 (KEY_D_1_8+1) +#define KEY_D_1_24 (KEY_D_1_10+1) +#define KEY_D_1_28 (KEY_D_1_24+1) +#define KEY_D_1_92 (KEY_D_1_28+1) +#define KEY_D_1_96 (KEY_D_1_92+1) +#define KEY_D_1_98 (KEY_D_1_96+1) +#define KEY_D_1_106 (KEY_D_1_98+1) +#define KEY_D_1_108 (KEY_D_1_106+1) +#define KEY_D_1_112 (KEY_D_1_108+1) +#define KEY_D_1_128 (KEY_D_1_112+1) +#define KEY_D_1_152 (KEY_D_1_128+1) +#define KEY_D_1_168 (KEY_D_1_152+1) +#define KEY_D_1_175 (KEY_D_1_168+1) +#define KEY_D_1_178 (KEY_D_1_175+1) +#define KEY_D_1_179 (KEY_D_1_178+1) +#define KEY_D_1_236 (KEY_D_1_179+1) +#define KEY_D_1_244 (KEY_D_1_236+1) +#define KEY_D_2_12 (KEY_D_1_244+1) +#define KEY_D_2_96 (KEY_D_2_12+1) +#define KEY_D_2_108 (KEY_D_2_96+1) +#define KEY_D_2_244 (KEY_D_2_108+1) +#define KEY_D_2_248 (KEY_D_2_244+1) +#define KEY_D_2_252 (KEY_D_2_248+1) + +// Compass Keys +#define KEY_CPASS_BIAS_X (KEY_D_2_252+1) +#define KEY_CPASS_BIAS_Y (KEY_CPASS_BIAS_X+1) +#define KEY_CPASS_BIAS_Z (KEY_CPASS_BIAS_Y+1) +#define KEY_CPASS_MTX_00 (KEY_CPASS_BIAS_Z+1) +#define KEY_CPASS_MTX_01 (KEY_CPASS_MTX_00+1) +#define KEY_CPASS_MTX_02 (KEY_CPASS_MTX_01+1) +#define KEY_CPASS_MTX_10 (KEY_CPASS_MTX_02+1) +#define KEY_CPASS_MTX_11 (KEY_CPASS_MTX_10+1) +#define KEY_CPASS_MTX_12 (KEY_CPASS_MTX_11+1) +#define KEY_CPASS_MTX_20 (KEY_CPASS_MTX_12+1) +#define KEY_CPASS_MTX_21 (KEY_CPASS_MTX_20+1) +#define KEY_CPASS_MTX_22 (KEY_CPASS_MTX_21+1) + +// Mantis Keys +#define KEY_CFG_MOTION_BIAS (KEY_CPASS_MTX_22+1) + +#define KEY_DMP_TAPW_MIN (KEY_CFG_MOTION_BIAS+1) +#define KEY_DMP_TAP_THR_X (KEY_DMP_TAPW_MIN+1) +#define KEY_DMP_TAP_THR_Y (KEY_DMP_TAP_THR_X+1) +#define KEY_DMP_TAP_THR_Z (KEY_DMP_TAP_THR_Y+1) +#define KEY_DMP_SH_TH_Y (KEY_DMP_TAP_THR_Z+1) +#define KEY_DMP_SH_TH_X (KEY_DMP_SH_TH_Y+1) +#define KEY_DMP_SH_TH_Z (KEY_DMP_SH_TH_X+1) +#define KEY_DMP_ORIENT (KEY_DMP_SH_TH_Z+1) +#define KEY_D_ACT0 (KEY_DMP_ORIENT+1) +#define KEY_D_ACSX (KEY_D_ACT0+1) +#define KEY_D_ACSY (KEY_D_ACSX+1) +#define KEY_D_ACSZ (KEY_D_ACSY+1) + +// Pedometer Standalone only keys +#define KEY_D_PEDSTD_BP_B (KEY_D_ACSZ+1) +#define KEY_D_PEDSTD_HP_A (KEY_D_PEDSTD_BP_B+1) +#define KEY_D_PEDSTD_HP_B (KEY_D_PEDSTD_HP_A+1) +#define KEY_D_PEDSTD_BP_A4 (KEY_D_PEDSTD_HP_B+1) +#define KEY_D_PEDSTD_BP_A3 (KEY_D_PEDSTD_BP_A4+1) +#define KEY_D_PEDSTD_BP_A2 (KEY_D_PEDSTD_BP_A3+1) +#define KEY_D_PEDSTD_BP_A1 (KEY_D_PEDSTD_BP_A2+1) +#define KEY_D_PEDSTD_INT_THRSH (KEY_D_PEDSTD_BP_A1+1) +#define KEY_D_PEDSTD_CLIP (KEY_D_PEDSTD_INT_THRSH+1) +#define KEY_D_PEDSTD_SB (KEY_D_PEDSTD_CLIP+1) +#define KEY_D_PEDSTD_SB_TIME (KEY_D_PEDSTD_SB+1) +#define KEY_D_PEDSTD_PEAKTHRSH (KEY_D_PEDSTD_SB_TIME+1) +#define KEY_D_PEDSTD_TIML (KEY_D_PEDSTD_PEAKTHRSH+1) +#define KEY_D_PEDSTD_TIMH (KEY_D_PEDSTD_TIML+1) +#define KEY_D_PEDSTD_PEAK (KEY_D_PEDSTD_TIMH+1) +#define KEY_D_PEDSTD_TIMECTR (KEY_D_PEDSTD_PEAK+1) +#define KEY_D_PEDSTD_STEPCTR (KEY_D_PEDSTD_TIMECTR+1) +#define KEY_D_PEDSTD_WALKTIME (KEY_D_PEDSTD_STEPCTR+1) + +// EIS Keys +#define KEY_P_EIS_FIFO_FOOTER (KEY_D_PEDSTD_WALKTIME+1) +#define KEY_P_EIS_FIFO_YSHIFT (KEY_P_EIS_FIFO_FOOTER+1) +#define KEY_P_EIS_DATA_RATE (KEY_P_EIS_FIFO_YSHIFT+1) +#define KEY_P_EIS_FIFO_XSHIFT (KEY_P_EIS_DATA_RATE+1) +#define KEY_P_EIS_FIFO_SYNC (KEY_P_EIS_FIFO_XSHIFT+1) +#define KEY_P_EIS_FIFO_ZSHIFT (KEY_P_EIS_FIFO_SYNC+1) +#define KEY_P_EIS_FIFO_READY (KEY_P_EIS_FIFO_ZSHIFT+1) +#define KEY_DMP_FOOTER (KEY_P_EIS_FIFO_READY+1) +#define KEY_DMP_INTX_HC (KEY_DMP_FOOTER+1) +#define KEY_DMP_INTX_PH (KEY_DMP_INTX_HC+1) +#define KEY_DMP_INTX_SH (KEY_DMP_INTX_PH+1) +#define KEY_DMP_AINV_SH (KEY_DMP_INTX_SH +1) +#define KEY_DMP_A_INV_XH (KEY_DMP_AINV_SH+1) +#define KEY_DMP_AINV_PH (KEY_DMP_A_INV_XH+1) +#define KEY_DMP_CTHX_H (KEY_DMP_AINV_PH+1) +#define KEY_DMP_CTHY_H (KEY_DMP_CTHX_H+1) +#define KEY_DMP_CTHZ_H (KEY_DMP_CTHY_H+1) +#define KEY_DMP_NCTHX_H (KEY_DMP_CTHZ_H+1) +#define KEY_DMP_NCTHY_H (KEY_DMP_NCTHX_H+1) +#define KEY_DMP_NCTHZ_H (KEY_DMP_NCTHY_H+1) +#define KEY_DMP_CTSQ_XH (KEY_DMP_NCTHZ_H+1) +#define KEY_DMP_CTSQ_YH (KEY_DMP_CTSQ_XH+1) +#define KEY_DMP_CTSQ_ZH (KEY_DMP_CTSQ_YH+1) +#define KEY_DMP_INTX_H (KEY_DMP_CTSQ_ZH+1) +#define KEY_DMP_INTY_H (KEY_DMP_INTX_H+1) +#define KEY_DMP_INTZ_H (KEY_DMP_INTY_H+1) +#define KEY_DMP_HPX_H (KEY_DMP_INTZ_H+1) +#define KEY_DMP_HPY_H (KEY_DMP_HPX_H+1) +#define KEY_DMP_HPZ_H (KEY_DMP_HPY_H+1) + +// Stream Keys +#define KEY_STREAM_P_GYRO_Z (KEY_DMP_HPZ_H + 1) +#define KEY_STREAM_P_GYRO_Y (KEY_STREAM_P_GYRO_Z+1) +#define KEY_STREAM_P_GYRO_X (KEY_STREAM_P_GYRO_Y+1) +#define KEY_STREAM_P_TEMP (KEY_STREAM_P_GYRO_X+1) +#define KEY_STREAM_P_AUX_Y (KEY_STREAM_P_TEMP+1) +#define KEY_STREAM_P_AUX_X (KEY_STREAM_P_AUX_Y+1) +#define KEY_STREAM_P_AUX_Z (KEY_STREAM_P_AUX_X+1) +#define KEY_STREAM_P_ACCEL_Y (KEY_STREAM_P_AUX_Z+1) +#define KEY_STREAM_P_ACCEL_X (KEY_STREAM_P_ACCEL_Y+1) +#define KEY_STREAM_P_FOOTER (KEY_STREAM_P_ACCEL_X+1) +#define KEY_STREAM_P_ACCEL_Z (KEY_STREAM_P_FOOTER+1) + +#define NUM_KEYS (KEY_STREAM_P_ACCEL_Z+1) + + typedef struct { + unsigned short key; + unsigned short addr; + } tKeyLabel; + +#define DINA0A 0x0a +#define DINA22 0x22 +#define DINA42 0x42 +#define DINA5A 0x5a + +#define DINA06 0x06 +#define DINA0E 0x0e +#define DINA16 0x16 +#define DINA1E 0x1e +#define DINA26 0x26 +#define DINA2E 0x2e +#define DINA36 0x36 +#define DINA3E 0x3e +#define DINA46 0x46 +#define DINA4E 0x4e +#define DINA56 0x56 +#define DINA5E 0x5e +#define DINA66 0x66 +#define DINA6E 0x6e +#define DINA76 0x76 +#define DINA7E 0x7e + +#define DINA00 0x00 +#define DINA08 0x08 +#define DINA10 0x10 +#define DINA18 0x18 +#define DINA20 0x20 +#define DINA28 0x28 +#define DINA30 0x30 +#define DINA38 0x38 +#define DINA40 0x40 +#define DINA48 0x48 +#define DINA50 0x50 +#define DINA58 0x58 +#define DINA60 0x60 +#define DINA68 0x68 +#define DINA70 0x70 +#define DINA78 0x78 + +#define DINA04 0x04 +#define DINA0C 0x0c +#define DINA14 0x14 +#define DINA1C 0x1C +#define DINA24 0x24 +#define DINA2C 0x2c +#define DINA34 0x34 +#define DINA3C 0x3c +#define DINA44 0x44 +#define DINA4C 0x4c +#define DINA54 0x54 +#define DINA5C 0x5c +#define DINA64 0x64 +#define DINA6C 0x6c +#define DINA74 0x74 +#define DINA7C 0x7c + +#define DINA01 0x01 +#define DINA09 0x09 +#define DINA11 0x11 +#define DINA19 0x19 +#define DINA21 0x21 +#define DINA29 0x29 +#define DINA31 0x31 +#define DINA39 0x39 +#define DINA41 0x41 +#define DINA49 0x49 +#define DINA51 0x51 +#define DINA59 0x59 +#define DINA61 0x61 +#define DINA69 0x69 +#define DINA71 0x71 +#define DINA79 0x79 + +#define DINA25 0x25 +#define DINA2D 0x2d +#define DINA35 0x35 +#define DINA3D 0x3d +#define DINA4D 0x4d +#define DINA55 0x55 +#define DINA5D 0x5D +#define DINA6D 0x6d +#define DINA75 0x75 +#define DINA7D 0x7d + +#define DINC00 0x00 +#define DINC01 0x01 +#define DINC02 0x02 +#define DINC03 0x03 +#define DINC08 0x08 +#define DINC09 0x09 +#define DINC0A 0x0a +#define DINC0B 0x0b +#define DINC10 0x10 +#define DINC11 0x11 +#define DINC12 0x12 +#define DINC13 0x13 +#define DINC18 0x18 +#define DINC19 0x19 +#define DINC1A 0x1a +#define DINC1B 0x1b + +#define DINC20 0x20 +#define DINC21 0x21 +#define DINC22 0x22 +#define DINC23 0x23 +#define DINC28 0x28 +#define DINC29 0x29 +#define DINC2A 0x2a +#define DINC2B 0x2b +#define DINC30 0x30 +#define DINC31 0x31 +#define DINC32 0x32 +#define DINC33 0x33 +#define DINC38 0x38 +#define DINC39 0x39 +#define DINC3A 0x3a +#define DINC3B 0x3b + +#define DINC40 0x40 +#define DINC41 0x41 +#define DINC42 0x42 +#define DINC43 0x43 +#define DINC48 0x48 +#define DINC49 0x49 +#define DINC4A 0x4a +#define DINC4B 0x4b +#define DINC50 0x50 +#define DINC51 0x51 +#define DINC52 0x52 +#define DINC53 0x53 +#define DINC58 0x58 +#define DINC59 0x59 +#define DINC5A 0x5a +#define DINC5B 0x5b + +#define DINC60 0x60 +#define DINC61 0x61 +#define DINC62 0x62 +#define DINC63 0x63 +#define DINC68 0x68 +#define DINC69 0x69 +#define DINC6A 0x6a +#define DINC6B 0x6b +#define DINC70 0x70 +#define DINC71 0x71 +#define DINC72 0x72 +#define DINC73 0x73 +#define DINC78 0x78 +#define DINC79 0x79 +#define DINC7A 0x7a +#define DINC7B 0x7b + +#if defined CONFIG_MPU_SENSORS_MPU3050 +#define DINA80 0x80 +#define DINA90 0x90 +#define DINAA0 0xa0 +#define DINAC9 0xc9 +#define DINACB 0xcb +#define DINACD 0xcd +#define DINACF 0xcf +#define DINAC8 0xc8 +#define DINACA 0xca +#define DINACC 0xcc +#define DINACE 0xce +#define DINAD8 0xd8 +#define DINADD 0xdd +#define DINAF8 0xf8 +#define DINAFE 0xfe +#define DINAC0 0xc0 +#define DINAC1 0xc1 +#define DINAC2 0xc2 +#define DINAC3 0xc3 +#define DINAC4 0xc4 +#define DINAC5 0xc5 +#elif defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + +#define DINA80 0x80 +#define DINA90 0x90 +#define DINAA0 0xa0 +#define DINAC9 0xc9 +#define DINACB 0xcb +#define DINACD 0xcd +#define DINACF 0xcf +#define DINAC8 0xc8 +#define DINACA 0xca +#define DINACC 0xcc +#define DINACE 0xce +#define DINAD8 0xd8 +#define DINADD 0xdd +#define DINAF8 0xf0 +#define DINAFE 0xfe + +#define DINBF8 0xf8 +#define DINAC0 0xb0 +#define DINAC1 0xb1 +#define DINAC2 0xb4 +#define DINAC3 0xb5 +#define DINAC4 0xb8 +#define DINAC5 0xb9 +#define DINBC0 0xc0 +#define DINBC2 0xc2 +#define DINBC4 0xc4 +#define DINBC6 0xc6 +#else +#error No CONFIG_MPU_SENSORS_xxxx has been defined. +#endif + + +#endif // DMPKEY_H__ diff --git a/60xx/mlsdk/mllite/dmpconfig.txt b/60xx/mlsdk/mllite/dmpconfig.txt new file mode 100644 index 0000000..4643ed5 --- /dev/null +++ b/60xx/mlsdk/mllite/dmpconfig.txt @@ -0,0 +1,3 @@ +major version = 1 +minor version = 0 +startAddr = 0 diff --git a/60xx/mlsdk/mllite/dmpmap.h b/60xx/mlsdk/mllite/dmpmap.h new file mode 100644 index 0000000..cb53c7c --- /dev/null +++ b/60xx/mlsdk/mllite/dmpmap.h @@ -0,0 +1,276 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#ifndef DMPMAP_H +#define DMPMAP_H + +#ifdef __cplusplus +extern "C" +{ +#endif + +#define DMP_PTAT 0 +#define DMP_XGYR 2 +#define DMP_YGYR 4 +#define DMP_ZGYR 6 +#define DMP_XACC 8 +#define DMP_YACC 10 +#define DMP_ZACC 12 +#define DMP_ADC1 14 +#define DMP_ADC2 16 +#define DMP_ADC3 18 +#define DMP_BIASUNC 20 +#define DMP_FIFORT 22 +#define DMP_INVGSFH 24 +#define DMP_INVGSFL 26 +#define DMP_1H 28 +#define DMP_1L 30 +#define DMP_BLPFSTCH 32 +#define DMP_BLPFSTCL 34 +#define DMP_BLPFSXH 36 +#define DMP_BLPFSXL 38 +#define DMP_BLPFSYH 40 +#define DMP_BLPFSYL 42 +#define DMP_BLPFSZH 44 +#define DMP_BLPFSZL 46 +#define DMP_BLPFMTC 48 +#define DMP_SMC 50 +#define DMP_BLPFMXH 52 +#define DMP_BLPFMXL 54 +#define DMP_BLPFMYH 56 +#define DMP_BLPFMYL 58 +#define DMP_BLPFMZH 60 +#define DMP_BLPFMZL 62 +#define DMP_BLPFC 64 +#define DMP_SMCTH 66 +#define DMP_0H2 68 +#define DMP_0L2 70 +#define DMP_BERR2H 72 +#define DMP_BERR2L 74 +#define DMP_BERR2NH 76 +#define DMP_SMCINC 78 +#define DMP_ANGVBXH 80 +#define DMP_ANGVBXL 82 +#define DMP_ANGVBYH 84 +#define DMP_ANGVBYL 86 +#define DMP_ANGVBZH 88 +#define DMP_ANGVBZL 90 +#define DMP_BERR1H 92 +#define DMP_BERR1L 94 +#define DMP_ATCH 96 +#define DMP_BIASUNCSF 98 +#define DMP_ACT2H 100 +#define DMP_ACT2L 102 +#define DMP_GSFH 104 +#define DMP_GSFL 106 +#define DMP_GH 108 +#define DMP_GL 110 +#define DMP_0_5H 112 +#define DMP_0_5L 114 +#define DMP_0_0H 116 +#define DMP_0_0L 118 +#define DMP_1_0H 120 +#define DMP_1_0L 122 +#define DMP_1_5H 124 +#define DMP_1_5L 126 +#define DMP_TMP1AH 128 +#define DMP_TMP1AL 130 +#define DMP_TMP2AH 132 +#define DMP_TMP2AL 134 +#define DMP_TMP3AH 136 +#define DMP_TMP3AL 138 +#define DMP_TMP4AH 140 +#define DMP_TMP4AL 142 +#define DMP_XACCW 144 +#define DMP_TMP5 146 +#define DMP_XACCB 148 +#define DMP_TMP8 150 +#define DMP_YACCB 152 +#define DMP_TMP9 154 +#define DMP_ZACCB 156 +#define DMP_TMP10 158 +#define DMP_DZH 160 +#define DMP_DZL 162 +#define DMP_XGCH 164 +#define DMP_XGCL 166 +#define DMP_YGCH 168 +#define DMP_YGCL 170 +#define DMP_ZGCH 172 +#define DMP_ZGCL 174 +#define DMP_YACCW 176 +#define DMP_TMP7 178 +#define DMP_AFB1H 180 +#define DMP_AFB1L 182 +#define DMP_AFB2H 184 +#define DMP_AFB2L 186 +#define DMP_MAGFBH 188 +#define DMP_MAGFBL 190 +#define DMP_QT1H 192 +#define DMP_QT1L 194 +#define DMP_QT2H 196 +#define DMP_QT2L 198 +#define DMP_QT3H 200 +#define DMP_QT3L 202 +#define DMP_QT4H 204 +#define DMP_QT4L 206 +#define DMP_CTRL1H 208 +#define DMP_CTRL1L 210 +#define DMP_CTRL2H 212 +#define DMP_CTRL2L 214 +#define DMP_CTRL3H 216 +#define DMP_CTRL3L 218 +#define DMP_CTRL4H 220 +#define DMP_CTRL4L 222 +#define DMP_CTRLS1 224 +#define DMP_CTRLSF1 226 +#define DMP_CTRLS2 228 +#define DMP_CTRLSF2 230 +#define DMP_CTRLS3 232 +#define DMP_CTRLSFNLL 234 +#define DMP_CTRLS4 236 +#define DMP_CTRLSFNL2 238 +#define DMP_CTRLSFNL 240 +#define DMP_TMP30 242 +#define DMP_CTRLSFJT 244 +#define DMP_TMP31 246 +#define DMP_TMP11 248 +#define DMP_CTRLSF2_2 250 +#define DMP_TMP12 252 +#define DMP_CTRLSF1_2 254 +#define DMP_PREVPTAT 256 +#define DMP_ACCZB 258 +#define DMP_ACCXB 264 +#define DMP_ACCYB 266 +#define DMP_1HB 272 +#define DMP_1LB 274 +#define DMP_0H 276 +#define DMP_0L 278 +#define DMP_ASR22H 280 +#define DMP_ASR22L 282 +#define DMP_ASR6H 284 +#define DMP_ASR6L 286 +#define DMP_TMP13 288 +#define DMP_TMP14 290 +#define DMP_FINTXH 292 +#define DMP_FINTXL 294 +#define DMP_FINTYH 296 +#define DMP_FINTYL 298 +#define DMP_FINTZH 300 +#define DMP_FINTZL 302 +#define DMP_TMP1BH 304 +#define DMP_TMP1BL 306 +#define DMP_TMP2BH 308 +#define DMP_TMP2BL 310 +#define DMP_TMP3BH 312 +#define DMP_TMP3BL 314 +#define DMP_TMP4BH 316 +#define DMP_TMP4BL 318 +#define DMP_STXG 320 +#define DMP_ZCTXG 322 +#define DMP_STYG 324 +#define DMP_ZCTYG 326 +#define DMP_STZG 328 +#define DMP_ZCTZG 330 +#define DMP_CTRLSFJT2 332 +#define DMP_CTRLSFJTCNT 334 +#define DMP_PVXG 336 +#define DMP_TMP15 338 +#define DMP_PVYG 340 +#define DMP_TMP16 342 +#define DMP_PVZG 344 +#define DMP_TMP17 346 +#define DMP_MNMFLAGH 352 +#define DMP_MNMFLAGL 354 +#define DMP_MNMTMH 356 +#define DMP_MNMTML 358 +#define DMP_MNMTMTHRH 360 +#define DMP_MNMTMTHRL 362 +#define DMP_MNMTHRH 364 +#define DMP_MNMTHRL 366 +#define DMP_ACCQD4H 368 +#define DMP_ACCQD4L 370 +#define DMP_ACCQD5H 372 +#define DMP_ACCQD5L 374 +#define DMP_ACCQD6H 376 +#define DMP_ACCQD6L 378 +#define DMP_ACCQD7H 380 +#define DMP_ACCQD7L 382 +#define DMP_ACCQD0H 384 +#define DMP_ACCQD0L 386 +#define DMP_ACCQD1H 388 +#define DMP_ACCQD1L 390 +#define DMP_ACCQD2H 392 +#define DMP_ACCQD2L 394 +#define DMP_ACCQD3H 396 +#define DMP_ACCQD3L 398 +#define DMP_XN2H 400 +#define DMP_XN2L 402 +#define DMP_XN1H 404 +#define DMP_XN1L 406 +#define DMP_YN2H 408 +#define DMP_YN2L 410 +#define DMP_YN1H 412 +#define DMP_YN1L 414 +#define DMP_YH 416 +#define DMP_YL 418 +#define DMP_B0H 420 +#define DMP_B0L 422 +#define DMP_A1H 424 +#define DMP_A1L 426 +#define DMP_A2H 428 +#define DMP_A2L 430 +#define DMP_SEM1 432 +#define DMP_FIFOCNT 434 +#define DMP_SH_TH_X 436 +#define DMP_PACKET 438 +#define DMP_SH_TH_Y 440 +#define DMP_FOOTER 442 +#define DMP_SH_TH_Z 444 +#define DMP_TEMP29 448 +#define DMP_TEMP30 450 +#define DMP_XACCB_PRE 452 +#define DMP_XACCB_PREL 454 +#define DMP_YACCB_PRE 456 +#define DMP_YACCB_PREL 458 +#define DMP_ZACCB_PRE 460 +#define DMP_ZACCB_PREL 462 +#define DMP_TMP22 464 +#define DMP_TAP_TIMER 466 +#define DMP_TAP_THX 468 +#define DMP_TAP_THY 472 +#define DMP_TAP_THZ 476 +#define DMP_TAPW_MIN 478 +#define DMP_TMP25 480 +#define DMP_TMP26 482 +#define DMP_TMP27 484 +#define DMP_TMP28 486 +#define DMP_ORIENT 488 +#define DMP_THRSH 490 +#define DMP_ENDIANH 492 +#define DMP_ENDIANL 494 +#define DMP_BLPFNMTCH 496 +#define DMP_BLPFNMTCL 498 +#define DMP_BLPFNMXH 500 +#define DMP_BLPFNMXL 502 +#define DMP_BLPFNMYH 504 +#define DMP_BLPFNMYL 506 +#define DMP_BLPFNMZH 508 +#define DMP_BLPFNMZL 510 +#ifdef __cplusplus +} +#endif +#endif // DMPMAP_H diff --git a/60xx/mlsdk/mllite/invensense.h b/60xx/mlsdk/mllite/invensense.h new file mode 100644 index 0000000..586dd25 --- /dev/null +++ b/60xx/mlsdk/mllite/invensense.h @@ -0,0 +1,24 @@ +/** Main header file for Invensense's basic library. + */ +#include "accel.h" +#include "compass.h" +#include "dmpDefault.h" +#include "dmpKey.h" +#include "dmpmap.h" +#include "ml.h" +#include "mlBiasNoMotion.h" +#include "mlFIFO.h" +#include "mlFIFOHW.h" +#include "mlMathFunc.h" +#include "mlSetGyroBias.h" +#include "ml_legacy.h" +#include "ml_mputest.h" +#include "ml_stored_data.h" +#include "mlcontrol.h" +#include "mldl.h" +#include "mldl_cfg.h" +#include "mldmp.h" +#include "mlinclude.h" +#include "mlstates.h" +#include "mlsupervisor.h" +#include "pressure.h" diff --git a/60xx/mlsdk/mllite/ml.c b/60xx/mlsdk/mllite/ml.c new file mode 100644 index 0000000..3328edd --- /dev/null +++ b/60xx/mlsdk/mllite/ml.c @@ -0,0 +1,1885 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/****************************************************************************** + * + * $Id: ml.c 5642 2011-06-14 02:26:20Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML + * @brief Motion Library APIs. + * The Motion Library processes gyroscopes, accelerometers, and + * compasses to provide a physical model of the movement for the + * sensors. + * The results of this processing may be used to control objects + * within a user interface environment, detect gestures, track 3D + * movement for gaming applications, and analyze the blur created + * due to hand movement while taking a picture. + * + * @{ + * @file ml.c + * @brief The Motion Library APIs. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include + +#include "ml.h" +#include "mldl.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "compass.h" +#include "dmpKey.h" +#include "dmpDefault.h" +#include "mlstates.h" +#include "mlFIFO.h" +#include "mlFIFOHW.h" +#include "mlMathFunc.h" +#include "mlsupervisor.h" +#include "mlmath.h" +#include "mlcontrol.h" +#include "mldl_cfg.h" +#include "mpu.h" +#include "accel.h" +#include "mlos.h" +#include "mlsl.h" +#include "mlos.h" +#include "mlBiasNoMotion.h" +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-ml" + +#define ML_MOT_TYPE_NONE 0 +#define ML_MOT_TYPE_NO_MOTION 1 +#define ML_MOT_TYPE_MOTION_DETECTED 2 + +#define ML_MOT_STATE_MOVING 0 +#define ML_MOT_STATE_NO_MOTION 1 +#define ML_MOT_STATE_BIAS_IN_PROG 2 + +#define _mlDebug(x) //{x} + +/* Global Variables */ +const unsigned char mlVer[] = { INV_VERSION }; + +struct inv_params_obj inv_params_obj = { + INV_BIAS_UPDATE_FUNC_DEFAULT, // bias_mode + INV_ORIENTATION_MASK_DEFAULT, // orientation_mask + INV_PROCESSED_DATA_CALLBACK_DEFAULT, // fifo_processed_func + INV_ORIENTATION_CALLBACK_DEFAULT, // orientation_cb_func + INV_MOTION_CALLBACK_DEFAULT, // motion_cb_func + INV_STATE_SERIAL_CLOSED // starting state +}; + +struct inv_obj_t inv_obj; +void *g_mlsl_handle; + +typedef struct { + // These describe callbacks happening everythime a DMP interrupt is processed + int_fast8_t numInterruptProcesses; + // Array of function pointers, function being void function taking void + inv_obj_func processInterruptCb[MAX_INTERRUPT_PROCESSES]; + +} tMLXCallbackInterrupt; // MLX_callback_t + +tMLXCallbackInterrupt mlxCallbackInterrupt; + +/* --------------- */ +/* - Functions. - */ +/* --------------- */ + +inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient); +inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient); +unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx); + +/** + * @brief Open serial connection with the MPU device. + * This is the entry point of the MPL and must be + * called prior to any other function call. + * + * @param port System handle for 'port' MPU device is found on. + * The significance of this parameter varies by + * platform. It is passed as 'port' to MLSLSerialOpen. + * + * @return INV_SUCCESS or error code. + */ +inv_error_t inv_serial_start(char const *port) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + + if (inv_get_state() >= INV_STATE_SERIAL_OPENED) + return INV_SUCCESS; + + result = inv_state_transition(INV_STATE_SERIAL_OPENED); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_serial_open(port, &g_mlsl_handle); + if (INV_SUCCESS != result) { + (void)inv_state_transition(INV_STATE_SERIAL_CLOSED); + } + + return result; +} + +/** + * @brief Close the serial communication. + * This function needs to be called explicitly to shut down + * the communication with the device. Calling inv_dmp_close() + * won't affect the established serial communication. + * @return INV_SUCCESS; non-zero error code otherwise. + */ +inv_error_t inv_serial_stop(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() == INV_STATE_SERIAL_CLOSED) + return INV_SUCCESS; + + result = inv_state_transition(INV_STATE_SERIAL_CLOSED); + if (INV_SUCCESS != result) { + MPL_LOGE("State Transition Failure in %s: %d\n", __func__, result); + } + result = inv_serial_close(g_mlsl_handle); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to close Serial Handle %s: %d\n", __func__, result); + } + return result; +} + +/** + * @brief Get the serial file handle to the device. + * @return The serial file handle. + */ +void *inv_get_serial_handle(void) +{ + INVENSENSE_FUNC_START; + return g_mlsl_handle; +} + +/** + * @brief apply the choosen orientation and full scale range + * for gyroscopes, accelerometer, and compass. + * @return INV_SUCCESS if successful, a non-zero code otherwise. + */ +inv_error_t inv_apply_calibration(void) +{ + INVENSENSE_FUNC_START; + signed char gyroCal[9] = { 0 }; + signed char accelCal[9] = { 0 }; + signed char magCal[9] = { 0 }; + float gyroScale = 2000.f; + float accelScale = 0.f; + float magScale = 0.f; + + inv_error_t result; + int ii; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + for (ii = 0; ii < 9; ii++) { + gyroCal[ii] = mldl_cfg->pdata->orientation[ii]; + if (NULL != mldl_cfg->accel){ + accelCal[ii] = mldl_cfg->pdata->accel.orientation[ii]; + } + if (NULL != mldl_cfg->compass){ + magCal[ii] = mldl_cfg->pdata->compass.orientation[ii]; + } + } + + switch (mldl_cfg->full_scale) { + case MPU_FS_250DPS: + gyroScale = 250.f; + break; + case MPU_FS_500DPS: + gyroScale = 500.f; + break; + case MPU_FS_1000DPS: + gyroScale = 1000.f; + break; + case MPU_FS_2000DPS: + gyroScale = 2000.f; + break; + default: + MPL_LOGE("Unrecognized full scale setting for gyros : %02X\n", + mldl_cfg->full_scale); + return INV_ERROR_INVALID_PARAMETER; + break; + } + + if (NULL != mldl_cfg->accel){ + RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->accel->range, accelScale); + inv_obj.accel_sens = (long)(accelScale * 65536L); + /* sensitivity adjustment, typically = 2 (for +/- 2 gee) */ +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + inv_obj.accel_sens /= 32768 / mldl_cfg->accel_sens_trim; +#else + inv_obj.accel_sens /= 2; +#endif + } + if (NULL != mldl_cfg->compass){ + RANGE_FIXEDPOINT_TO_FLOAT(mldl_cfg->compass->range, magScale); + inv_obj.compass_sens = (long)(magScale * 32768); + } + + if (inv_get_state() == INV_STATE_DMP_OPENED) { + result = inv_set_gyro_calibration(gyroScale, gyroCal); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to set Gyro Calibration\n"); + return result; + } + if (NULL != mldl_cfg->accel){ + result = inv_set_accel_calibration(accelScale, accelCal); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to set Accel Calibration\n"); + return result; + } + } + if (NULL != mldl_cfg->compass){ + result = inv_set_compass_calibration(magScale, magCal); + if (INV_SUCCESS != result) { + MPL_LOGE("Unable to set Mag Calibration\n"); + return result; + } + } + } + return INV_SUCCESS; +} + +/** + * @brief Setup the DMP to handle the accelerometer endianess. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_apply_endian_accel(void) +{ + INVENSENSE_FUNC_START; + unsigned char regs[4] = { 0 }; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + int endian = mldl_cfg->accel->endian; + + if (mldl_cfg->pdata->accel.bus != EXT_SLAVE_BUS_SECONDARY) { + endian = EXT_SLAVE_BIG_ENDIAN; + } + switch (endian) { + case EXT_SLAVE_FS8_BIG_ENDIAN: + case EXT_SLAVE_FS16_BIG_ENDIAN: + case EXT_SLAVE_LITTLE_ENDIAN: + regs[0] = 0; + regs[1] = 64; + regs[2] = 0; + regs[3] = 0; + break; + case EXT_SLAVE_BIG_ENDIAN: + default: + regs[0] = 0; + regs[1] = 0; + regs[2] = 64; + regs[3] = 0; + } + + return inv_set_mpu_memory(KEY_D_1_236, 4, regs); +} + +/** + * @internal + * @brief Initialize MLX data. This should be called to setup the mlx + * output buffers before any motion processing is done. + */ +void inv_init_ml(void) +{ + INVENSENSE_FUNC_START; + int ii; + long tmp[COMPASS_NUM_AXES]; + // Set all values to zero by default + memset(&inv_obj, 0, sizeof(struct inv_obj_t)); + memset(&mlxCallbackInterrupt, 0, sizeof(tMLXCallbackInterrupt)); + + inv_obj.compass_correction[0] = 1073741824L; + inv_obj.compass_correction_relative[0] = 1073741824L; + inv_obj.compass_disturb_correction[0] = 1073741824L; + inv_obj.compass_correction_offset[0] = 1073741824L; + inv_obj.relative_quat[0] = 1073741824L; + + //Not used with the ST accelerometer + inv_obj.no_motion_threshold = 20; // noMotionThreshold + //Not used with the ST accelerometer + inv_obj.motion_duration = 1536; // motionDuration + + inv_obj.motion_state = INV_MOTION; // Motion state + + inv_obj.bias_update_time = 8000; + inv_obj.bias_calc_time = 2000; + + inv_obj.internal_motion_state = ML_MOT_STATE_MOVING; + + inv_obj.start_time = inv_get_tick_count(); + + inv_obj.compass_cal[0] = 322122560L; + inv_obj.compass_cal[4] = 322122560L; + inv_obj.compass_cal[8] = 322122560L; + inv_obj.compass_sens = 322122560L; // Should only change when the sensor full-scale range (FSR) is changed. + + for (ii = 0; ii < COMPASS_NUM_AXES; ii++) { + inv_obj.compass_scale[ii] = 65536L; + inv_obj.compass_test_scale[ii] = 65536L; + inv_obj.compass_bias_error[ii] = P_INIT; + inv_obj.init_compass_bias[ii] = 0; + inv_obj.compass_asa[ii] = (1L << 30); + } + if (inv_compass_read_scale(tmp) == INV_SUCCESS) { + for (ii = 0; ii < COMPASS_NUM_AXES; ii++) + inv_obj.compass_asa[ii] = tmp[ii]; + } + inv_obj.got_no_motion_bias = 0; + inv_obj.got_compass_bias = 0; + inv_obj.compass_state = SF_UNCALIBRATED; + inv_obj.acc_state = SF_STARTUP_SETTLE; + + inv_obj.got_init_compass_bias = 0; + inv_obj.resetting_compass = 0; + + inv_obj.external_slave_callback = NULL; + inv_obj.compass_accuracy = 0; + + inv_obj.factory_temp_comp = 0; + inv_obj.got_coarse_heading = 0; + + inv_obj.compass_bias_ptr[0] = P_INIT; + inv_obj.compass_bias_ptr[4] = P_INIT; + inv_obj.compass_bias_ptr[8] = P_INIT; + + inv_obj.gyro_bias_err = 1310720; + + inv_obj.accel_lpf_gain = 1073744L; + inv_obj.no_motion_accel_threshold = 7000000L; +} + +/** + * @internal + * @brief This registers a function to be called for each time the DMP + * generates an an interrupt. + * It will be called after inv_register_fifo_rate_process() which gets called + * every time the FIFO data is processed. + * The FIFO does not have to be on for this callback. + * @param func Function to be called when a DMP interrupt occurs. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func) +{ + INVENSENSE_FUNC_START; + // Make sure we have not filled up our number of allowable callbacks + if (mlxCallbackInterrupt.numInterruptProcesses <= + MAX_INTERRUPT_PROCESSES - 1) { + int kk; + // Make sure we haven't registered this function already + for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { + if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { + return INV_ERROR_INVALID_PARAMETER; + } + } + // Add new callback + mlxCallbackInterrupt.processInterruptCb[mlxCallbackInterrupt. + numInterruptProcesses] = func; + mlxCallbackInterrupt.numInterruptProcesses++; + return INV_SUCCESS; + } + return INV_ERROR_MEMORY_EXAUSTED; +} + +/** + * @internal + * @brief This unregisters a function to be called for each DMP interrupt. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func) +{ + INVENSENSE_FUNC_START; + int kk, jj; + // Make sure we haven't registered this function already + for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { + if (mlxCallbackInterrupt.processInterruptCb[kk] == func) { + // FIXME, we may need a thread block here + for (jj = kk + 1; jj < mlxCallbackInterrupt.numInterruptProcesses; + ++jj) { + mlxCallbackInterrupt.processInterruptCb[jj - 1] = + mlxCallbackInterrupt.processInterruptCb[jj]; + } + mlxCallbackInterrupt.numInterruptProcesses--; + return INV_SUCCESS; + } + } + return INV_ERROR_INVALID_PARAMETER; +} + +/** + * @internal + * @brief Run the recorded interrupt process callbacks in the event + * of an interrupt. + */ +void inv_run_dmp_interupt_cb(void) +{ + int kk; + for (kk = 0; kk < mlxCallbackInterrupt.numInterruptProcesses; ++kk) { + if (mlxCallbackInterrupt.processInterruptCb[kk]) + mlxCallbackInterrupt.processInterruptCb[kk] (&inv_obj); + } +} + +/** @internal +* Resets the Motion/No Motion state which should be called at startup and resume. +*/ +inv_error_t inv_reset_motion(void) +{ + unsigned char regs[8]; + inv_error_t result; + + inv_obj.motion_state = INV_MOTION; + inv_obj.flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; + inv_obj.no_motion_accel_time = inv_get_tick_count(); +#if defined CONFIG_MPU_SENSORS_MPU3050 + regs[0] = DINAD8 + 2; + regs[1] = DINA0C; + regs[2] = DINAD8 + 1; + result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#else +#endif + regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); + regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); + result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + memset(regs, 0, 8); + result = inv_set_mpu_memory(KEY_D_1_96, 8, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = + inv_set_mpu_memory(KEY_D_0_96, 4, inv_int32_to_big8(0x40000000, regs)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + inv_set_motion_state(INV_MOTION); + return result; +} + +/** + * @internal + * @brief inv_start_bias_calc starts the bias calculation on the MPU. + */ +void inv_start_bias_calc(void) +{ + INVENSENSE_FUNC_START; + + inv_obj.suspend = 1; +} + +/** + * @internal + * @brief inv_stop_bias_calc stops the bias calculation on the MPU. + */ +void inv_stop_bias_calc(void) +{ + INVENSENSE_FUNC_START; + + inv_obj.suspend = 0; +} + +/** + * @brief inv_update_data fetches data from the fifo and updates the + * motion algorithms. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() must have been called. + * + * @note Motion algorithm data is constant between calls to inv_update_data + * + * @return + * - INV_SUCCESS + * - Non-zero error code + */ +inv_error_t inv_update_data(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + int_fast8_t got, ftry; + uint_fast8_t mpu_interrupt; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + // Set the maximum number of FIFO packets we want to process + if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { + ftry = 100; // Large enough to process all packets + } else { + ftry = 1; + } + + // Go and process at most ftry number of packets, probably less than ftry + result = inv_read_and_process_fifo(ftry, &got); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + // Process all interrupts + mpu_interrupt = inv_get_interrupt_trigger(INTSRC_AUX1); + if (mpu_interrupt) { + inv_clear_interrupt_trigger(INTSRC_AUX1); + } + // Check if interrupt was from MPU + mpu_interrupt = inv_get_interrupt_trigger(INTSRC_MPU); + if (mpu_interrupt) { + inv_clear_interrupt_trigger(INTSRC_MPU); + } + // Take care of the callbacks that want to be notified when there was an MPU interrupt + if (mpu_interrupt) { + inv_run_dmp_interupt_cb(); + } + + result = inv_get_fifo_status(); + return result; +} + +/** + * @brief inv_check_flag returns the value of a flag. + * inv_check_flag can be used to check a number of flags, + * allowing users to poll flags rather than register callback + * functions. If a flag is set to True when inv_check_flag is called, + * the flag is automatically reset. + * The flags are: + * - INV_RAW_DATA_READY + * Indicates that new raw data is available. + * - INV_PROCESSED_DATA_READY + * Indicates that new processed data is available. + * - INV_GOT_GESTURE + * Indicates that a gesture has been detected by the gesture engine. + * - INV_MOTION_STATE_CHANGE + * Indicates that a change has been made from motion to no motion, + * or vice versa. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() must have been called. + * + * @param flag The flag to check. + * + * @return TRUE or FALSE state of the flag + */ +int inv_check_flag(int flag) +{ + INVENSENSE_FUNC_START; + int flagReturn = inv_obj.flags[flag]; + + inv_obj.flags[flag] = 0; + return flagReturn; +} + +/** + * @brief Enable generation of the DMP interrupt when Motion or no-motion + * is detected + * @param on + * Boolean to turn the interrupt on or off. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_set_motion_interrupt(unsigned char on) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char regs[2] = { 0 }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (on) { + result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_obj.interrupt_sources |= INV_INT_MOTION; + } else { + inv_obj.interrupt_sources &= ~INV_INT_MOTION; + if (!inv_obj.interrupt_sources) { + result = inv_get_dl_cfg_int(0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + + if (on) { + regs[0] = DINAFE; + } else { + regs[0] = DINAD8; + } + result = inv_set_mpu_memory(KEY_CFG_7, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * Enable generation of the DMP interrupt when a FIFO packet is ready + * + * @param on Boolean to turn the interrupt on or off + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_set_fifo_interrupt(unsigned char on) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char regs[2] = { 0 }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (on) { + result = inv_get_dl_cfg_int(BIT_DMP_INT_EN); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_obj.interrupt_sources |= INV_INT_FIFO; + } else { + inv_obj.interrupt_sources &= ~INV_INT_FIFO; + if (!inv_obj.interrupt_sources) { + result = inv_get_dl_cfg_int(0); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + + if (on) { + regs[0] = DINAFE; + } else { + regs[0] = DINAD8; + } + result = inv_set_mpu_memory(KEY_CFG_6, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * @brief Get the current set of DMP interrupt sources. + * These interrupts are generated by the DMP and can be + * routed to the MPU interrupt line via internal + * settings. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @return Currently enabled interrupt sources. The possible interrupts are: + * - INV_INT_FIFO, + * - INV_INT_MOTION, + * - INV_INT_TAP + */ +int inv_get_interrupts(void) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return 0; // error + + return inv_obj.interrupt_sources; +} + +/** + * @brief Sets up the Accelerometer calibration and scale factor. + * + * Please refer to the provided "9-Axis Sensor Fusion Application + * Note" document provided. Section 5, "Sensor Mounting Orientation" + * offers a good coverage on the mounting matrices and explains how + * to use them. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * @pre inv_dmp_start() must NOT have been called. + * + * @see inv_set_gyro_calibration(). + * @see inv_set_compass_calibration(). + * + * @param[in] range + * The range of the accelerometers in g's. An accelerometer + * that has a range of +2g's to -2g's should pass in 2. + * @param[in] orientation + * A 9 element matrix that represents how the accelerometers + * are oriented with respect to the device they are mounted + * in and the reference axis system. + * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. + * This example corresponds to a 3 x 3 identity matrix. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_accel_calibration(float range, signed char *orientation) +{ + INVENSENSE_FUNC_START; + float cal[9]; + float scale = range / 32768.f; + int kk; + unsigned long sf; + inv_error_t result; + unsigned char regs[4] = { 0, 0, 0, 0 }; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + /* Apply zero g-offset values */ + if (ACCEL_ID_KXSD9 == mldl_cfg->accel->id) { + regs[0] = 0x80; + regs[1] = 0x0; + regs[2] = 0x80; + regs[3] = 0x0; + } + + if (inv_dmpkey_supported(KEY_D_1_152)) { + result = inv_set_mpu_memory(KEY_D_1_152, 4, ®s[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + if (scale == 0) { + inv_obj.accel_sens = 0; + } + + if (mldl_cfg->accel->id) { +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + unsigned char tmp[3] = { DINA0C, DINAC9, DINA2C }; +#else + unsigned char tmp[3] = { DINA4C, DINACD, DINA6C }; +#endif + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + unsigned char regs[3]; + unsigned short orient; + + for (kk = 0; kk < 9; ++kk) { + cal[kk] = scale * orientation[kk]; + inv_obj.accel_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); + } + + orient = inv_orientation_matrix_to_scalar(orientation); + regs[0] = tmp[orient & 3]; + regs[1] = tmp[(orient >> 3) & 3]; + regs[2] = tmp[(orient >> 6) & 3]; + result = inv_set_mpu_memory(KEY_FCFG_2, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + regs[0] = DINA26; + regs[1] = DINA46; +#if defined CONFIG_MPU_SENSORS_MPU3050 || defined CONFIG_MPU_SENSORS_MPU6050A2 + regs[2] = DINA66; +#else + if (MPL_PROD_KEY(mldl_cfg->product_id, mldl_cfg->product_revision) + == MPU_PRODUCT_KEY_B1_E1_5) + regs[2] = DINA76; + else + regs[2] = DINA66; +#endif + if (orient & 4) + regs[0] |= 1; + if (orient & 0x20) + regs[1] |= 1; + if (orient & 0x100) + regs[2] |= 1; + + result = inv_set_mpu_memory(KEY_FCFG_7, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (mldl_cfg->accel->id == ACCEL_ID_MMA845X) { + result = inv_freescale_sensor_fusion_16bit(orient); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else if (mldl_cfg->accel->id == ACCEL_ID_MMA8450) { + result = inv_freescale_sensor_fusion_8bit(orient); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + + if (inv_obj.accel_sens != 0) { + sf = (1073741824L / inv_obj.accel_sens); + } else { + sf = 0; + } + regs[0] = (unsigned char)((sf >> 8) & 0xff); + regs[1] = (unsigned char)(sf & 0xff); + result = inv_set_mpu_memory(KEY_D_0_108, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/** + * @brief Sets up the Gyro calibration and scale factor. + * + * Please refer to the provided "9-Axis Sensor Fusion Application + * Note" document provided. Section 5, "Sensor Mounting Orientation" + * offers a good coverage on the mounting matrices and explains + * how to use them. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * @pre inv_dmp_start() must have NOT been called. + * + * @see inv_set_accel_calibration(). + * @see inv_set_compass_calibration(). + * + * @param[in] range + * The range of the gyros in degrees per second. A gyro + * that has a range of +2000 dps to -2000 dps should pass in + * 2000. + * @param[in] orientation + * A 9 element matrix that represents how the gyro are oriented + * with respect to the device they are mounted in. A typical + * set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. This + * example corresponds to a 3 x 3 identity matrix. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_gyro_calibration(float range, signed char *orientation) +{ + INVENSENSE_FUNC_START; + + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + int kk; + float scale; + inv_error_t result; + + unsigned char regs[12] = { 0 }; + unsigned char maxVal = 0; + unsigned char tmpPtr = 0; + unsigned char tmpSign = 0; + unsigned char i = 0; + int sf = 0; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (mldl_cfg->gyro_sens_trim != 0) { + /* adjust the declared range to consider the gyro_sens_trim + of this part when different from the default (first dividend) */ + range *= (32768.f / 250) / mldl_cfg->gyro_sens_trim; + } + + scale = range / 32768.f; // inverse of sensitivity for the given full scale range + inv_obj.gyro_sens = (long)(range * 32768L); + + for (kk = 0; kk < 9; ++kk) { + inv_obj.gyro_cal[kk] = (long)(scale * orientation[kk] * (1L << 30)); + inv_obj.gyro_orient[kk] = (long)((double)orientation[kk] * (1L << 30)); + } + + { +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + unsigned char tmpD = DINA4C; + unsigned char tmpE = DINACD; + unsigned char tmpF = DINA6C; +#else + unsigned char tmpD = DINAC9; + unsigned char tmpE = DINA2C; + unsigned char tmpF = DINACB; +#endif + regs[3] = DINA36; + regs[4] = DINA56; + regs[5] = DINA76; + + for (i = 0; i < 3; i++) { + maxVal = 0; + tmpSign = 0; + if (inv_obj.gyro_orient[0 + 3 * i] < 0) + tmpSign = 1; + if (ABS(inv_obj.gyro_orient[1 + 3 * i]) > + ABS(inv_obj.gyro_orient[0 + 3 * i])) { + maxVal = 1; + if (inv_obj.gyro_orient[1 + 3 * i] < 0) + tmpSign = 1; + } + if (ABS(inv_obj.gyro_orient[2 + 3 * i]) > + ABS(inv_obj.gyro_orient[1 + 3 * i])) { + tmpSign = 0; + maxVal = 2; + if (inv_obj.gyro_orient[2 + 3 * i] < 0) + tmpSign = 1; + } + if (maxVal == 0) { + regs[tmpPtr++] = tmpD; + if (tmpSign) + regs[tmpPtr + 2] |= 0x01; + } else if (maxVal == 1) { + regs[tmpPtr++] = tmpE; + if (tmpSign) + regs[tmpPtr + 2] |= 0x01; + } else { + regs[tmpPtr++] = tmpF; + if (tmpSign) + regs[tmpPtr + 2] |= 0x01; + } + } + result = inv_set_mpu_memory(KEY_FCFG_1, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_mpu_memory(KEY_FCFG_3, 3, ®s[3]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + //sf = (gyroSens) * (0.5 * (pi/180) / 200.0) * 16384 + inv_obj.gyro_sf = + (long)(((long long)inv_obj.gyro_sens * 767603923LL) / 1073741824LL); + result = + inv_set_mpu_memory(KEY_D_0_104, 4, + inv_int32_to_big8(inv_obj.gyro_sf, regs)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (inv_obj.gyro_sens != 0) { + sf = (long)((long long)23832619764371LL / inv_obj.gyro_sens); + } else { + sf = 0; + } + + result = inv_set_mpu_memory(KEY_D_0_24, 4, inv_int32_to_big8(sf, regs)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + return INV_SUCCESS; +} + +/** + * @brief Sets up the Compass calibration and scale factor. + * + * Please refer to the provided "9-Axis Sensor Fusion Application + * Note" document provided. Section 5, "Sensor Mounting Orientation" + * offers a good coverage on the mounting matrices and explains + * how to use them. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * @pre inv_dmp_start() must have NOT been called. + * + * @see inv_set_gyro_calibration(). + * @see inv_set_accel_calibration(). + * + * @param[in] range + * The range of the compass. + * @param[in] orientation + * A 9 element matrix that represents how the compass is + * oriented with respect to the device they are mounted in. + * A typical set of values are {1, 0, 0, 0, 1, 0, 0, 0, 1}. + * This example corresponds to a 3 x 3 identity matrix. + * The matrix describes how to go from the chip mounting to + * the body of the device. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_compass_calibration(float range, signed char *orientation) +{ + INVENSENSE_FUNC_START; + float cal[9]; + float scale = range / 32768.f; + int kk; + unsigned short compassId = 0; + + compassId = inv_get_compass_id(); + + if ((compassId == COMPASS_ID_YAS529) || (compassId == COMPASS_ID_HMC5883) + || (compassId == COMPASS_ID_LSM303M)) { + scale /= 32.0f; + } + + for (kk = 0; kk < 9; ++kk) { + cal[kk] = scale * orientation[kk]; + inv_obj.compass_cal[kk] = (long)(cal[kk] * (float)(1L << 30)); + } + + inv_obj.compass_sens = (long)(scale * 1073741824L); + + if (inv_dmpkey_supported(KEY_CPASS_MTX_00)) { + unsigned char reg0[4] = { 0, 0, 0, 0 }; + unsigned char regp[4] = { 64, 0, 0, 0 }; + unsigned char regn[4] = { 64 + 128, 0, 0, 0 }; + unsigned char *reg; + int_fast8_t kk; + unsigned short keyList[9] = + { KEY_CPASS_MTX_00, KEY_CPASS_MTX_01, KEY_CPASS_MTX_02, + KEY_CPASS_MTX_10, KEY_CPASS_MTX_11, KEY_CPASS_MTX_12, + KEY_CPASS_MTX_20, KEY_CPASS_MTX_21, KEY_CPASS_MTX_22 + }; + + for (kk = 0; kk < 9; ++kk) { + + if (orientation[kk] == 1) + reg = regp; + else if (orientation[kk] == -1) + reg = regn; + else + reg = reg0; + inv_set_mpu_memory(keyList[kk], 4, reg); + } + } + + return INV_SUCCESS; +} + +/** +* @internal +* @brief Sets the Gyro Dead Zone based upon LPF filter settings and Control setup. +*/ +inv_error_t inv_set_dead_zone(void) +{ + unsigned char reg; + inv_error_t result; + extern struct control_params cntrl_params; + + if (cntrl_params.functions & INV_DEAD_ZONE) { + reg = 0x08; + } else { +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + reg = 0; +#else + if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { + reg = 0x2; + } else { + reg = 0; + } +#endif + } + + result = inv_set_mpu_memory(KEY_D_0_163, 1, ®); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * @brief inv_set_bias_update is used to register which algorithms will be + * used to automatically reset the gyroscope bias. + * The engine INV_BIAS_UPDATE must be enabled for these algorithms to + * run. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must NOT have been called. + * + * @param function A function or bitwise OR of functions that determine + * how the gyroscope bias will be automatically updated. + * Functions include: + * - INV_NONE or 0, + * - INV_BIAS_FROM_NO_MOTION, + * - INV_BIAS_FROM_GRAVITY, + * - INV_BIAS_FROM_TEMPERATURE, + \ifnot UMPL + * - INV_BIAS_FROM_LPF, + * - INV_MAG_BIAS_FROM_MOTION, + * - INV_MAG_BIAS_FROM_GYRO, + * - INV_ALL. + * \endif + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_bias_update(unsigned short function) +{ + INVENSENSE_FUNC_START; + unsigned char regs[4]; + long tmp[3] = { 0, 0, 0 }; + inv_error_t result = INV_SUCCESS; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + /* do not allow progressive no motion bias tracker to run - + it's not fully debugged */ + function &= ~INV_PROGRESSIVE_NO_MOTION; // FIXME, workaround + MPL_LOGV("forcing disable of PROGRESSIVE_NO_MOTION bias tracker\n"); + + // You must use EnableFastNoMotion() to get this feature + function &= ~INV_BIAS_FROM_FAST_NO_MOTION; + + // You must use DisableFastNoMotion() to turn this feature off + if (inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) + function |= INV_BIAS_FROM_FAST_NO_MOTION; + + /*--- remove magnetic components from bias tracking + if there is no compass ---*/ + if (!inv_compass_present()) { + function &= ~(INV_MAG_BIAS_FROM_GYRO | INV_MAG_BIAS_FROM_MOTION); + } else { + function &= ~(INV_BIAS_FROM_LPF); + } + + // Enable function sets bias from no motion + inv_params_obj.bias_mode = function & (~INV_BIAS_FROM_NO_MOTION); + + if (function & INV_BIAS_FROM_NO_MOTION) { + inv_enable_bias_no_motion(); + } else { + inv_disable_bias_no_motion(); + } + + if (inv_params_obj.bias_mode & INV_BIAS_FROM_LPF) { + regs[0] = DINA80 + 2; + regs[1] = DINA2D; + regs[2] = DINA55; + regs[3] = DINA7D; + } else { + regs[0] = DINA80 + 7; + regs[1] = DINA2D; + regs[2] = DINA35; + regs[3] = DINA3D; + } + result = inv_set_mpu_memory(KEY_FCFG_5, 4, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_dead_zone(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if ((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) && + !inv_compass_present()) { + result = inv_set_gyro_data_source(INV_GYRO_FROM_QUATERNION); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else { + result = inv_set_gyro_data_source(INV_GYRO_FROM_RAW); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + inv_obj.factory_temp_comp = 0; // FIXME, workaround + if ((mldl_cfg->offset_tc[0] != 0) || + (mldl_cfg->offset_tc[1] != 0) || (mldl_cfg->offset_tc[2] != 0)) { + inv_obj.factory_temp_comp = 1; + } + + if (inv_obj.factory_temp_comp == 0) { + if (inv_params_obj.bias_mode & INV_BIAS_FROM_TEMPERATURE) { + result = inv_set_gyro_temp_slope(inv_obj.temp_slope); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else { + result = inv_set_gyro_temp_slope(tmp); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } else { + inv_params_obj.bias_mode &= ~INV_LEARN_BIAS_FROM_TEMPERATURE; + MPL_LOGV("factory temperature compensation coefficients available - " + "disabling INV_LEARN_BIAS_FROM_TEMPERATURE\n"); + } + + /*---- hard requirement for using bias tracking BIAS_FROM_GRAVITY, relying on + compass and accel data, is to have accelerometer data delivered in the + FIFO ----*/ + if (((inv_params_obj.bias_mode & INV_BIAS_FROM_GRAVITY) + && inv_compass_present()) + || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) + || (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION)) { + inv_send_accel(INV_ALL, INV_32_BIT); + inv_send_gyro(INV_ALL, INV_32_BIT); + } + + return result; +} + +/** + * @brief inv_get_motion_state is used to determine if the device is in + * a 'motion' or 'no motion' state. + * inv_get_motion_state returns INV_MOTION of the device is moving, + * or INV_NO_MOTION if the device is not moving. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must have been called. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +int inv_get_motion_state(void) +{ + INVENSENSE_FUNC_START; + return inv_obj.motion_state; +} + +/** + * @brief inv_set_no_motion_thresh is used to set the threshold for + * detecting INV_NO_MOTION + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param thresh A threshold scaled in degrees per second. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_no_motion_thresh(float thresh) +{ + inv_error_t result = INV_SUCCESS; + unsigned char regs[4] = { 0 }; + long tmp; + INVENSENSE_FUNC_START; + + tmp = (long)(thresh * thresh * 2.045f); + if (tmp < 0) { + return INV_ERROR; + } else if (tmp > 8180000L) { + return INV_ERROR; + } + inv_obj.no_motion_threshold = tmp; + + regs[0] = (unsigned char)(tmp >> 24); + regs[1] = (unsigned char)((tmp >> 16) & 0xff); + regs[2] = (unsigned char)((tmp >> 8) & 0xff); + regs[3] = (unsigned char)(tmp & 0xff); + + result = inv_set_mpu_memory(KEY_D_1_108, 4, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_reset_motion(); + return result; +} + +/** + * @brief inv_set_no_motion_threshAccel is used to set the threshold for + * detecting INV_NO_MOTION with accelerometers when Gyros have + * been turned off + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param thresh A threshold in g's scaled by 2^32 + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_no_motion_threshAccel(long thresh) +{ + INVENSENSE_FUNC_START; + + inv_obj.no_motion_accel_threshold = thresh; + + return INV_SUCCESS; +} + +/** + * @brief inv_set_no_motion_time is used to set the time required for + * detecting INV_NO_MOTION + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param time A time in seconds. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_no_motion_time(float time) +{ + inv_error_t result = INV_SUCCESS; + unsigned char regs[2] = { 0 }; + long tmp; + + INVENSENSE_FUNC_START; + + tmp = (long)(time * 200); + if (tmp < 0) { + return INV_ERROR; + } else if (tmp > 65535L) { + return INV_ERROR; + } + inv_obj.motion_duration = (unsigned short)tmp; + + regs[0] = (unsigned char)((inv_obj.motion_duration >> 8) & 0xff); + regs[1] = (unsigned char)(inv_obj.motion_duration & 0xff); + result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_reset_motion(); + return result; +} + +/** + * @brief inv_get_version is used to get the ML version. + * + * @pre inv_get_version can be called at any time. + * + * @param version inv_get_version writes the ML version + * string pointer to version. + * + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_get_version(unsigned char **version) +{ + INVENSENSE_FUNC_START; + + *version = (unsigned char *)mlVer; //fixme we are wiping const + + return INV_SUCCESS; +} + +/** + * @brief Check for the presence of the gyro sensor. + * + * This is not a physical check but a logical check and the value can change + * dynamically based on calls to inv_set_mpu_sensors(). + * + * @return TRUE if the gyro is enabled FALSE otherwise. + */ +int inv_get_gyro_present(void) +{ + return inv_get_dl_config()->requested_sensors & (INV_X_GYRO | INV_Y_GYRO | + INV_Z_GYRO); +} + +static unsigned short inv_row_2_scale(const signed char *row) +{ + unsigned short b; + + if (row[0] > 0) + b = 0; + else if (row[0] < 0) + b = 4; + else if (row[1] > 0) + b = 1; + else if (row[1] < 0) + b = 5; + else if (row[2] > 0) + b = 2; + else if (row[2] < 0) + b = 6; + else + b = 7; // error + return b; +} + +unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx) +{ + unsigned short scalar; + /* + XYZ 010_001_000 Identity Matrix + XZY 001_010_000 + YXZ 010_000_001 + YZX 000_010_001 + ZXY 001_000_010 + ZYX 000_001_010 + */ + + scalar = inv_row_2_scale(mtx); + scalar |= inv_row_2_scale(mtx + 3) << 3; + scalar |= inv_row_2_scale(mtx + 6) << 6; + + return scalar; +} + +/* Setups up the Freescale 16-bit accel for Sensor Fusion +* @param[in] orient A scalar representation of the orientation +* that describes how to go from the Chip Orientation +* to the Board Orientation often times called the Body Orientation in Invensense Documentation. +* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. +*/ +inv_error_t inv_freescale_sensor_fusion_16bit(unsigned short orient) +{ + unsigned char rr[3]; + inv_error_t result; + + orient = orient & 0xdb; + switch (orient) { + default: + // Typically 0x88 + rr[0] = DINACC; + rr[1] = DINACF; + rr[2] = DINA0E; + break; + case 0x50: + rr[0] = DINACE; + rr[1] = DINA0E; + rr[2] = DINACD; + break; + case 0x81: + rr[0] = DINACE; + rr[1] = DINACB; + rr[2] = DINA0E; + break; + case 0x11: + rr[0] = DINACC; + rr[1] = DINA0E; + rr[2] = DINACB; + break; + case 0x42: + rr[0] = DINA0A; + rr[1] = DINACF; + rr[2] = DINACB; + break; + case 0x0a: + rr[0] = DINA0A; + rr[1] = DINACB; + rr[2] = DINACD; + break; + } + result = inv_set_mpu_memory(KEY_FCFG_AZ, 3, rr); + return result; +} + +/* Setups up the Freescale 8-bit accel for Sensor Fusion +* @param[in] orient A scalar representation of the orientation +* that describes how to go from the Chip Orientation +* to the Board Orientation often times called the Body Orientation in Invensense Documentation. +* inv_orientation_matrix_to_scalar() will turn the transformation matrix into this scalar. +*/ +inv_error_t inv_freescale_sensor_fusion_8bit(unsigned short orient) +{ + unsigned char regs[27]; + inv_error_t result; + uint_fast8_t kk; + + orient = orient & 0xdb; + kk = 0; + + regs[kk++] = DINAC3; + regs[kk++] = DINA90 + 14; + regs[kk++] = DINAA0 + 9; + regs[kk++] = DINA3E; + regs[kk++] = DINA5E; + regs[kk++] = DINA7E; + + regs[kk++] = DINAC2; + regs[kk++] = DINAA0 + 9; + regs[kk++] = DINA90 + 9; + regs[kk++] = DINAF8 + 2; + + switch (orient) { + default: + // Typically 0x88 + regs[kk++] = DINACB; + + regs[kk++] = DINA54; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + + regs[kk++] = DINACD; + break; + case 0x50: + regs[kk++] = DINACB; + + regs[kk++] = DINACF; + + regs[kk++] = DINA7C; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + break; + case 0x81: + regs[kk++] = DINA2C; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + + regs[kk++] = DINACD; + + regs[kk++] = DINACB; + break; + case 0x11: + regs[kk++] = DINA2C; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINA28; + regs[kk++] = DINACB; + regs[kk++] = DINACF; + break; + case 0x42: + regs[kk++] = DINACF; + regs[kk++] = DINACD; + + regs[kk++] = DINA7C; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + regs[kk++] = DINA78; + break; + case 0x0a: + regs[kk++] = DINACD; + + regs[kk++] = DINA54; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + regs[kk++] = DINA50; + + regs[kk++] = DINACF; + break; + } + + regs[kk++] = DINA90 + 7; + regs[kk++] = DINAF8 + 3; + regs[kk++] = DINAA0 + 9; + regs[kk++] = DINA0E; + regs[kk++] = DINA0E; + regs[kk++] = DINA0E; + + regs[kk++] = DINAF8 + 1; // filler + + result = inv_set_mpu_memory(KEY_FCFG_FSCALE, kk, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/** + * Controlls each sensor and each axis when the motion processing unit is + * running. When it is not running, simply records the state for later. + * + * NOTE: In this version only full sensors controll is allowed. Independent + * axis control will return an error. + * + * @param sensors Bit field of each axis desired to be turned on or off + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_set_mpu_sensors(unsigned long sensors) +{ + INVENSENSE_FUNC_START; + unsigned char state = inv_get_state(); + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + inv_error_t result = INV_SUCCESS; + unsigned short fifoRate; + + if (state < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (((sensors & INV_THREE_AXIS_ACCEL) != INV_THREE_AXIS_ACCEL) && + ((sensors & INV_THREE_AXIS_ACCEL) != 0)) { + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + if (((sensors & INV_THREE_AXIS_ACCEL) != 0) && + (mldl_cfg->pdata->accel.get_slave_descr == 0)) { + return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; + } + + if (((sensors & INV_THREE_AXIS_COMPASS) != INV_THREE_AXIS_COMPASS) && + ((sensors & INV_THREE_AXIS_COMPASS) != 0)) { + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + if (((sensors & INV_THREE_AXIS_COMPASS) != 0) && + (mldl_cfg->pdata->compass.get_slave_descr == 0)) { + return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; + } + + if (((sensors & INV_THREE_AXIS_PRESSURE) != INV_THREE_AXIS_PRESSURE) && + ((sensors & INV_THREE_AXIS_PRESSURE) != 0)) { + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + } + if (((sensors & INV_THREE_AXIS_PRESSURE) != 0) && + (mldl_cfg->pdata->pressure.get_slave_descr == 0)) { + return INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED; + } + + /* DMP was off, and is turning on */ + if (sensors & INV_DMP_PROCESSOR && + !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) { + struct ext_slave_config config; + long odr; + config.key = MPU_SLAVE_CONFIG_ODR_RESUME; + config.len = sizeof(long); + config.apply = (state == INV_STATE_DMP_STARTED); + config.data = &odr; + + odr = (inv_mpu_get_sampling_rate_hz(mldl_cfg) * 1000); + result = inv_mpu_config_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; + odr = MPU_SLAVE_IRQ_TYPE_NONE; + result = inv_mpu_config_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_init_fifo_hardare(); + } + + if (inv_obj.mode_change_func) { + result = inv_obj.mode_change_func(mldl_cfg->requested_sensors, sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + /* Get the fifo rate before changing sensors so if we need to match it */ + fifoRate = inv_get_fifo_rate(); + mldl_cfg->requested_sensors = sensors; + + /* inv_dmp_start will turn the sensors on */ + if (state == INV_STATE_DMP_STARTED) { + result = inv_dl_start(sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_reset_motion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_dl_stop(~sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + inv_set_fifo_rate(fifoRate); + + if (!(sensors & INV_DMP_PROCESSOR) && (sensors & INV_THREE_AXIS_ACCEL)) { + struct ext_slave_config config; + long data; + + config.len = sizeof(long); + config.key = MPU_SLAVE_CONFIG_IRQ_RESUME; + config.apply = (state == INV_STATE_DMP_STARTED); + config.data = &data; + data = MPU_SLAVE_IRQ_TYPE_DATA_READY; + result = inv_mpu_config_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + return result; +} + +void inv_set_mode_change(inv_error_t(*mode_change_func) + (unsigned long, unsigned long)) +{ + inv_obj.mode_change_func = mode_change_func; +} + +/** +* Mantis setup +*/ +#ifdef CONFIG_MPU_SENSORS_MPU6050B1 +inv_error_t inv_set_mpu_6050_config() +{ + long temp; + inv_error_t result; + unsigned char big8[4]; + unsigned char atc[4]; + long s[3], s2[3]; + int kk; + struct mldl_cfg *mldlCfg = inv_get_dl_config(); + + result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), + 0x0d, 4, atc); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + temp = atc[3] & 0x3f; + if (temp >= 32) + temp = temp - 64; + temp = (temp << 21) | 0x100000; + temp += (1L << 29); + temp = -temp; + result = inv_set_mpu_memory(KEY_D_ACT0, 4, inv_int32_to_big8(temp, big8)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + for (kk = 0; kk < 3; ++kk) { + s[kk] = atc[kk] & 0x3f; + if (s[kk] > 32) + s[kk] = s[kk] - 64; + s[kk] *= 2516582L; + } + + for (kk = 0; kk < 3; ++kk) { + s2[kk] = mldlCfg->pdata->orientation[kk * 3] * s[0] + + mldlCfg->pdata->orientation[kk * 3 + 1] * s[1] + + mldlCfg->pdata->orientation[kk * 3 + 2] * s[2]; + } + result = inv_set_mpu_memory(KEY_D_ACSX, 4, inv_int32_to_big8(s2[0], big8)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_mpu_memory(KEY_D_ACSY, 4, inv_int32_to_big8(s2[1], big8)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_mpu_memory(KEY_D_ACSZ, 4, inv_int32_to_big8(s2[2], big8)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} +#endif + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/ml.h b/60xx/mlsdk/mllite/ml.h new file mode 100644 index 0000000..838cd49 --- /dev/null +++ b/60xx/mlsdk/mllite/ml.h @@ -0,0 +1,596 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/****************************************************************************** + * + * $Id: ml.h 5653 2011-06-16 21:06:55Z nroyer $ + * + *****************************************************************************/ + +/** + * @defgroup ML + * @brief The Motion Library processes gyroscopes and accelerometers to + * provide a physical model of the movement of the sensors. + * The results of this processing may be used to control objects + * within a user interface environment, detect gestures, track 3D + * movement for gaming applications, and analyze the blur created + * due to hand movement while taking a picture. + * + * @{ + * @file ml.h + * @brief Header file for the Motion Library. +**/ + +#ifndef INV_H +#define INV_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#include "mldmp.h" +#include "mlsl.h" +#include "mpu.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "ml_legacy.h" +#endif + +/* ------------ */ +/* - Defines. - */ +/* ------------ */ + +/* - Module defines. - */ + +/*************************************************************************/ +/* Motion Library Vesion */ +/*************************************************************************/ + +#define INV_VERSION_MAJOR 4 +#define INV_VERSION_MINOR 0 +#define INV_VERSION_SUB_MINOR 0 + +#define INV_VERSION_MAJOR_STR "4" +#define INV_VERSION_MINOR_STR "0" +#define INV_VERSION_SUB_MINOR_STR "0" + +#define INV_VERSION_NONE "" +#define INV_VERSION_PROTOTYPE "ProtoA " +#define INV_VERSION_ENGINEERING "EngA " +#define INV_VERSION_PRE_ALPHA "Pre-Alpha " +#define INV_VERSION_ALPHA "Alpha " +#define INV_VERSION_BETA "Beta " +#define INV_VERSION_PRODUCTION "Production " + +#ifndef INV_VERSION_TYPE +#define INV_VERSION_TYPE INV_VERSION_NONE +#endif + +#define INV_VERSION "InvenSense MPL" " " \ + "v" INV_VERSION_MAJOR_STR "." INV_VERSION_MINOR_STR "." INV_VERSION_SUB_MINOR_STR " " \ + INV_VERSION_TYPE \ + __DATE__ " " __TIME__ + +/*************************************************************************/ +/* Motion processing engines */ +/*************************************************************************/ +#define INV_MOTION_DETECT (0x0004) +#define INV_BIAS_UPDATE (0x0008) +#define INV_GESTURE (0x0020) +#define INV_CONTROL (0x0040) +#define INV_ORIENTATION (0x0100) +#define INV_PEDOMETER (0x0200) +#define INV_BASIC (INV_MOTION_DETECT | INV_BIAS_UPDATE) + +/*************************************************************************/ +/* Data Source - Obsolete */ +/*************************************************************************/ +#define INV_DATA_FIFO (0x1) +#define INV_DATA_POLL (0x2) + +/*************************************************************************/ +/* Interrupt Source */ +/*************************************************************************/ +#define INV_INT_MOTION (0x01) +#define INV_INT_FIFO (0x02) +#define INV_INT_TAP (0x04) +#define INV_INT_ORIENTATION (0x08) +#define INV_INT_SHAKE_PITCH (0x10) +#define INV_INT_SHAKE_ROLL (0x20) +#define INV_INT_SHAKE_YAW (0x40) + +/*************************************************************************/ +/* Bias update functions */ +/*************************************************************************/ +#define INV_BIAS_FROM_NO_MOTION 0x0001 +#define INV_BIAS_FROM_GRAVITY 0x0002 +#define INV_BIAS_FROM_TEMPERATURE 0x0004 +#define INV_BIAS_FROM_LPF 0x0008 +#define INV_MAG_BIAS_FROM_MOTION 0x0010 +#define INV_MAG_BIAS_FROM_GYRO 0x0020 +#define INV_LEARN_BIAS_FROM_TEMPERATURE 0x0040 +#define INV_AUTO_RESET_MAG_BIAS 0x0080 +#define INV_REJECT_MAG_DISTURBANCE 0x0100 +#define INV_PROGRESSIVE_NO_MOTION 0x0200 +#define INV_BIAS_FROM_FAST_NO_MOTION 0x0400 + +/*************************************************************************/ +/* Euler angles and axis names */ +/*************************************************************************/ +#define INV_X_AXIS (0x01) +#define INV_Y_AXIS (0x02) +#define INV_Z_AXIS (0x04) + +#define INV_PITCH (INV_X_AXIS) +#define INV_ROLL (INV_Y_AXIS) +#define INV_YAW (INV_Z_AXIS) + +/*************************************************************************/ +/* Sensor types */ +/*************************************************************************/ +#define INV_GYROS 0x0001 +#define INV_ACCELS 0x0002 + +/*************************************************************************/ +/* Motion arrays */ +/*************************************************************************/ +#define INV_ROTATION_MATRIX 0x0003 +#define INV_QUATERNION 0x0004 +#define INV_EULER_ANGLES 0x0005 +#define INV_LINEAR_ACCELERATION 0x0006 +#define INV_LINEAR_ACCELERATION_WORLD 0x0007 +#define INV_GRAVITY 0x0008 +#define INV_ANGULAR_VELOCITY 0x0009 + +#define INV_GYRO_CALIBRATION_MATRIX 0x000B +#define INV_ACCEL_CALIBRATION_MATRIX 0x000C +#define INV_GYRO_BIAS 0x000D +#define INV_ACCEL_BIAS 0x000E +#define INV_GYRO_TEMP_SLOPE 0x000F + +#define INV_RAW_DATA 0x0011 +#define INV_DMP_TAP 0x0012 +#define INV_DMP_TAP2 0x0021 + +#define INV_EULER_ANGLES_X 0x0013 +#define INV_EULER_ANGLES_Y 0x0014 +#define INV_EULER_ANGLES_Z 0x0015 + +#define INV_BIAS_UNCERTAINTY 0x0016 +#define INV_DMP_PACKET_NUMBER 0x0017 +#define INV_FOOTER 0x0018 + +#define INV_CONTROL_DATA 0x0019 + +#define INV_MAGNETOMETER 0x001A +#define INV_PEDLBS 0x001B +#define INV_MAG_RAW_DATA 0x001C +#define INV_MAG_CALIBRATION_MATRIX 0x001D +#define INV_MAG_BIAS 0x001E +#define INV_HEADING 0x001F + +#define INV_MAG_BIAS_ERROR 0x0020 + +#define INV_PRESSURE 0x0021 +#define INV_LOCAL_FIELD 0x0022 +#define INV_MAG_SCALE 0x0023 + +#define INV_RELATIVE_QUATERNION 0x0024 + +#define SET_QUATERNION 0x0001 +#define SET_GYROS 0x0002 +#define SET_LINEAR_ACCELERATION 0x0004 +#define SET_GRAVITY 0x0008 +#define SET_ACCELS 0x0010 +#define SET_TAP 0x0020 +#define SET_PEDLBS 0x0040 +#define SET_LINEAR_ACCELERATION_WORLD 0x0080 +#define SET_CONTROL 0x0100 +#define SET_PACKET_NUMBER 0x4000 +#define SET_FOOTER 0x8000 + +/*************************************************************************/ +/* Integral reset options */ +/*************************************************************************/ +#define INV_NO_RESET 0x0000 +#define INV_RESET 0x0001 + +/*************************************************************************/ +/* Motion states */ +/*************************************************************************/ +#define INV_MOTION 0x0001 +#define INV_NO_MOTION 0x0002 + +/*************************************************************************/ +/* Orientation and Gesture states */ +/*************************************************************************/ +#define INV_STATE_IDLE (0) +#define INV_STATE_RUNNING (1) + +/*************************************************************************/ +/* Gyroscope Temperature Compensation bins */ +/*************************************************************************/ +#define BINS (25) +#define PTS_PER_BIN (5) +#define MIN_TEMP (-40) +#define MAX_TEMP (+85) +#define TEMP_PER_BIN ((MAX_TEMP - MIN_TEMP) / BINS) + +/*************************************************************************/ +/* Flags */ +/*************************************************************************/ +#define INV_RAW_DATA_READY 0x0001 +#define INV_PROCESSED_DATA_READY 0x0002 + +#define INV_GOT_GESTURE 0x0004 + +#define INV_MOTION_STATE_CHANGE 0x0006 +#define INV_COMPASS_OFFSET_VALID 0x0007 + +/*************************************************************************/ +/* General */ +/*************************************************************************/ +#define INV_NONE (0x0000) +#define INV_INVALID_FIFO_RATE (0xFFFF) + +/*************************************************************************/ +/* ML Params Structure Default Values */ +/*************************************************************************/ +#define INV_BIAS_UPDATE_FUNC_DEFAULT (INV_BIAS_FROM_NO_MOTION | INV_BIAS_FROM_GRAVITY) +#define INV_ORIENTATION_MASK_DEFAULT 0x3f +#define INV_PROCESSED_DATA_CALLBACK_DEFAULT 0 +#define INV_ORIENTATION_CALLBACK_DEFAULT 0 +#define INV_MOTION_CALLBACK_DEFAULT 0 + +/* ------------ */ +/* - Defines. - */ +/* ------------ */ +/* Priority for various features */ +#define INV_PRIORITY_BUS_ACCEL 100 +#define INV_PRIORITY_EXTERNAL_SLAVE_MAG 110 +#define INV_PRIORITY_FAST_NO_MOTION 120 +#define INV_PRIORITY_BIAS_NO_MOTION 125 +#define INV_PRIORITY_SET_GYRO_BIASES 150 +#define INV_PRIORITY_TEMP_COMP 175 +#define INV_PRIORITY_CONTROL 200 +#define INV_PRIORITY_EIS 300 +#define INV_PRIORITY_ORIENTATION 400 +#define INV_PRIORITY_PEDOMETER_FULLPOWER 500 +#define INV_PRIORITY_NAVIGATION_PEDOMETER 600 +#define INV_PRIORITY_GESTURE 700 +#define INV_PRIORITY_GLYPH 800 + +#define MAX_INTERRUPT_PROCESSES 5 +/* Number of quantized accel samples */ +#define INV_MAX_NUM_ACCEL_SAMPLES (8) + +#define PRECISION 10000.f +#define RANGE_FLOAT_TO_FIXEDPOINT(range, x) { \ + range.mantissa = (long)x; \ + range.fraction = (long)((float)(x-(long)x)*PRECISION); \ +} +#define RANGE_FIXEDPOINT_TO_FLOAT(range, x) { \ + x = (float)(range.mantissa); \ + x += ((float)range.fraction/PRECISION); \ +} + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + +struct inv_obj_t { + //Calibration parameters + /* Raw sensor orientation */ + long gyro_bias[3]; + long accel_bias[3]; + long compass_bias[3]; + + /* Cached values after orietnation is applied */ + long scaled_gyro_bias[3]; + long scaled_accel_bias[3]; + long scaled_compass_bias[3]; + + long compass_scale[3]; + long compass_test_bias[3]; + long compass_test_scale[3]; + long compass_asa[3]; + long compass_offsets[3]; + + long compass_bias_error[3]; + + long got_no_motion_bias; + long got_compass_bias; + long compass_state; + long large_field; + long acc_state; + + long factory_temp_comp; + long got_coarse_heading; + long gyro_temp_bias[3]; + long prog_no_motion_bias[3]; + + long accel_cal[9]; + // Deprecated, used gyro_orient + long gyro_cal[GYRO_NUM_AXES * GYRO_NUM_AXES]; + long gyro_orient[GYRO_NUM_AXES * GYRO_NUM_AXES]; + long accel_sens; + long compass_cal[9]; + long gyro_sens; + long gyro_sf; + long temp_slope[GYRO_NUM_AXES]; + long compass_sens; + long temp_offset[GYRO_NUM_AXES]; + + int cal_loaded_flag; + + /* temperature compensation */ + float x_gyro_coef[3]; + float y_gyro_coef[3]; + float z_gyro_coef[3]; + float x_gyro_temp_data[BINS][PTS_PER_BIN]; + float y_gyro_temp_data[BINS][PTS_PER_BIN]; + float z_gyro_temp_data[BINS][PTS_PER_BIN]; + float temp_data[BINS][PTS_PER_BIN]; + int temp_ptrs[BINS]; + long temp_valid_data[BINS]; + + long compass_correction[4]; + long compass_correction_relative[4]; + long compass_disturb_correction[4]; + long compass_correction_offset[4]; + long relative_quat[4]; + long local_field[3]; + long new_local_field; + long sync_grav_body[3]; + int gyro_bias_err; + + double compass_bias_ptr[9]; + double compass_bias_v[3]; + double compass_prev_m[36]; + double compass_prev_xty[6]; + + int compass_peaks[18]; + int all_sensors_no_motion; + + long init_compass_bias[3]; + + int got_init_compass_bias; + int resetting_compass; + + long ang_v_body[GYRO_NUM_AXES]; + unsigned char compass_raw_data[24]; /* Sensor data plus status etc */ + long compass_sensor_data[3]; /* Raw sensor data only */ + long compass_calibrated_data[3]; + long compass_test_calibrated_data[3]; + long pressure; + inv_error_t (*external_slave_callback)(struct inv_obj_t *); + int compass_accuracy; + int compass_overunder; + + unsigned short flags[8]; + unsigned short suspend; + + long no_motion_threshold; + unsigned long motion_duration; + + unsigned short motion_state; + + unsigned short data_mode; + unsigned short interrupt_sources; + + unsigned short bias_update_time; + short last_motion; + unsigned short bias_calc_time; + + unsigned char internal_motion_state; + long start_time; + + long accel_lpf_gain; + long accel_lpf[3]; + unsigned long poll_no_motion; + long no_motion_accel_threshold; + unsigned long no_motion_accel_time; + inv_error_t(*mode_change_func) (unsigned long, unsigned long); + }; + + typedef inv_error_t(*inv_obj_func) (struct inv_obj_t *); + + extern struct inv_obj_t inv_obj; + + /* --------------------- */ + /* - Params Structure. - */ + /* --------------------- */ + + struct inv_params_obj { + + unsigned short bias_mode; // A function or bitwise OR of functions that determine how the gyroscope bias will be automatically updated. + // Functions include INV_BIAS_FROM_NO_MOTION, INV_BIAS_FROM_GRAVITY, and INV_BIAS_FROM_TEMPERATURE. + // The engine INV_BIAS_UPDATE must be enabled for these algorithms to run. + + unsigned short orientation_mask; // Allows a user to register which orientations will trigger the user defined callback function. + // The orientations are INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, and INV_Z_DOWN. + // INV_ORIENTATION_ALL is equivalent to INV_X_UP | INV_X_DOWN | INV_Y_UP | INV_Y_DOWN | INV_Z_UP | INV_Z_DOWN. + + void (*fifo_processed_func) (void); // Callback function that triggers when all the processing has been finished by the motion processing engines. + + void (*orientation_cb_func) (unsigned short orient); // Callback function that will run when a change of orientation is detected. + // The new orientation. May be one of INV_X_UP, INV_X_DOWN, INV_Y_UP, INV_Y_DOWN, INV_Z_UP, or INV_Z_DOWN. + + void (*motion_cb_func) (unsigned short motion_state); // Callback function that will run when a change of motion state is detected. + // The new motion state. May be one of INV_MOTION, or INV_NO_MOTION. + + unsigned char state; + + }; + + extern struct inv_params_obj inv_params_obj; + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + + inv_error_t inv_serial_start(char const *port); + inv_error_t inv_serial_stop(void); + inv_error_t inv_set_mpu_sensors(unsigned long sensors); + void *inv_get_serial_handle(void); + + /*API for handling the buffer */ + inv_error_t inv_update_data(void); + + /*API for handling polling */ + int inv_check_flag(int flag); + + /*API for setting bias update function */ + inv_error_t inv_set_bias_update(unsigned short biasFunction); + +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + inv_error_t inv_turn_on_bias_from_no_motion(void); + inv_error_t inv_turn_off_bias_from_no_motion(void); + inv_error_t inv_set_mpu_6050_config(void); +#endif + + /* Legacy functions for handling augmented data*/ + inv_error_t inv_get_array(int dataSet, long *data); + inv_error_t inv_get_float_array(int dataSet, float *data); + inv_error_t inv_set_array(int dataSet, long *data); + inv_error_t inv_set_float_array(int dataSet, float *data); + /* Individual functions for augmented data, per specific dataset */ + + + inv_error_t inv_get_gyro(long *data); + inv_error_t inv_get_accel(long *data); + inv_error_t inv_get_temperature(long *data); + inv_error_t inv_get_temperature_raw(short *data); + inv_error_t inv_get_rot_mat(long *data); + inv_error_t inv_get_quaternion(long *data); + inv_error_t inv_get_linear_accel(long *data); + inv_error_t inv_get_linear_accel_in_world(long *data); + inv_error_t inv_get_gravity(long *data); + inv_error_t inv_get_angular_velocity(long *data); + inv_error_t inv_get_euler_angles(long *data); + inv_error_t inv_get_euler_angles_x(long *data); + inv_error_t inv_get_euler_angles_y(long *data); + inv_error_t inv_get_euler_angles_z(long *data); + inv_error_t inv_get_gyro_temp_slope(long *data); + inv_error_t inv_get_gyro_bias(long *data); + inv_error_t inv_get_accel_bias(long *data); + inv_error_t inv_get_mag_bias(long *data); + inv_error_t inv_get_gyro_and_accel_sensor(long *data); + inv_error_t inv_get_mag_raw_data(long *data); + inv_error_t inv_get_magnetometer(long *data); + inv_error_t inv_get_pressure(long *data); + inv_error_t inv_get_heading(long *data); + inv_error_t inv_get_gyro_cal_matrix(long *data); + inv_error_t inv_get_accel_cal_matrix(long *data); + inv_error_t inv_get_mag_cal_matrix(long *data); + inv_error_t inv_get_mag_bias_error(long *data); + inv_error_t inv_get_mag_scale(long *data); + inv_error_t inv_get_local_field(long *data); + inv_error_t inv_get_relative_quaternion(long *data); + inv_error_t inv_get_gyro_float(float *data); + inv_error_t inv_get_accel_float(float *data); + inv_error_t inv_get_temperature_float(float *data); + inv_error_t inv_get_rot_mat_float(float *data); + inv_error_t inv_get_quaternion_float(float *data); + inv_error_t inv_get_linear_accel_float(float *data); + inv_error_t inv_get_linear_accel_in_world_float(float *data); + inv_error_t inv_get_gravity_float(float *data); + inv_error_t inv_get_angular_velocity_float(float *data); + inv_error_t inv_get_euler_angles_float(float *data); + inv_error_t inv_get_euler_angles_x_float(float *data); + inv_error_t inv_get_euler_angles_y_float(float *data); + inv_error_t inv_get_euler_angles_z_float(float *data); + inv_error_t inv_get_gyro_temp_slope_float(float *data); + inv_error_t inv_get_gyro_bias_float(float *data); + inv_error_t inv_get_accel_bias_float(float *data); + inv_error_t inv_get_mag_bias_float(float *data); + inv_error_t inv_get_gyro_and_accel_sensor_float(float *data); + inv_error_t inv_get_mag_raw_data_float(float *data); + inv_error_t inv_get_magnetometer_float(float *data); + inv_error_t inv_get_pressure_float(float *data); + inv_error_t inv_get_heading_float(float *data); + inv_error_t inv_get_gyro_cal_matrix_float(float *data); + inv_error_t inv_get_accel_cal_matrix_float(float *data); + inv_error_t inv_get_mag_cal_matrix_float(float *data); + inv_error_t inv_get_mag_bias_error_float(float *data); + inv_error_t inv_get_mag_scale_float(float *data); + inv_error_t inv_get_local_field_float(float *data); + inv_error_t inv_get_relative_quaternion_float(float *data); + inv_error_t inv_get_compass_accuracy(int *accuracy); + inv_error_t inv_set_gyro_bias(long *data); + inv_error_t inv_set_accel_bias(long *data); + inv_error_t inv_set_mag_bias(long *data); + inv_error_t inv_set_gyro_temp_slope(long *data); + inv_error_t inv_set_local_field(long *data); + inv_error_t inv_set_mag_scale(long *data); + inv_error_t inv_set_gyro_temp_slope_float(float *data); + inv_error_t inv_set_gyro_bias_float(float *data); + inv_error_t inv_set_accel_bias_float(float *data); + inv_error_t inv_set_mag_bias_float(float *data); + inv_error_t inv_set_local_field_float(float *data); + inv_error_t inv_set_mag_scale_float(float *data); + + inv_error_t inv_apply_endian_accel(void); + inv_error_t inv_apply_calibration(void); + inv_error_t inv_set_gyro_calibration(float range, signed char *orientation); + inv_error_t inv_set_accel_calibration(float range, + signed char *orientation); + inv_error_t inv_set_compass_calibration(float range, + signed char *orientation); + + /*API for detecting change of state */ + inv_error_t + inv_set_motion_callback(void (*func) (unsigned short motion_state)); + int inv_get_motion_state(void); + + /*API for getting ML version. */ + inv_error_t inv_get_version(unsigned char **version); + + inv_error_t inv_set_motion_interrupt(unsigned char on); + inv_error_t inv_set_fifo_interrupt(unsigned char on); + + int inv_get_interrupts(void); + + /* Simulated DMP */ + int inv_get_gyro_present(void); + + inv_error_t inv_set_no_motion_time(float time); + inv_error_t inv_set_no_motion_thresh(float thresh); + inv_error_t inv_set_no_motion_threshAccel(long thresh); + inv_error_t inv_reset_motion(void); + + inv_error_t inv_update_bias(void); + inv_error_t inv_set_dead_zone(void); + void inv_start_bias_calc(void); + void inv_stop_bias_calc(void); + + // Private functions shared accross modules + void inv_init_ml(void); + + inv_error_t inv_register_dmp_interupt_cb(inv_obj_func func); + inv_error_t inv_unregister_dmp_interupt_cb(inv_obj_func func); + void inv_run_dmp_interupt_cb(void); + void inv_set_mode_change(inv_error_t(*mode_change_func) + (unsigned long, unsigned long)); + +#ifdef __cplusplus +} +#endif +#endif // INV_H +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.c b/60xx/mlsdk/mllite/mlBiasNoMotion.c new file mode 100644 index 0000000..73321ff --- /dev/null +++ b/60xx/mlsdk/mllite/mlBiasNoMotion.c @@ -0,0 +1,393 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id:$ + * + *****************************************************************************/ + +#include "mlBiasNoMotion.h" +#include "ml.h" +#include "mlinclude.h" +#include "mlos.h" +#include "mlFIFO.h" +#include "dmpKey.h" +#include "accel.h" +#include "mlMathFunc.h" +#include "mldl.h" +#include "mlstates.h" +#include "mlSetGyroBias.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-BiasNoMot" + + +#define _mlDebug(x) //{x} + +/** + * @brief inv_set_motion_callback is used to register a callback function that + * will trigger when a change of motion state is detected. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must NOT have been called. + * + * @param func A user defined callback function accepting a + * motion_state parameter, the new motion state. + * May be one of INV_MOTION or INV_NO_MOTION. + * @return INV_SUCCESS if successful or Non-zero error code otherwise. + */ +inv_error_t inv_set_motion_callback(void (*func) (unsigned short motion_state)) +{ + INVENSENSE_FUNC_START; + + if ((inv_get_state() != INV_STATE_DMP_OPENED) && + (inv_get_state() != INV_STATE_DMP_STARTED)) + return INV_ERROR_SM_IMPROPER_STATE; + + inv_params_obj.motion_cb_func = func; + + return INV_SUCCESS; +} + +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 +/** Turns on the feature to compute gyro bias from No Motion */ +inv_error_t inv_turn_on_bias_from_no_motion() +{ + inv_error_t result; + unsigned char regs[3] = { 0x0d, DINA35, 0x5d }; + inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; + result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); + return result; +} + +/** Turns off the feature to compute gyro bias from No Motion +*/ +inv_error_t inv_turn_off_bias_from_no_motion() +{ + inv_error_t result; + unsigned char regs[3] = { DINA90 + 8, DINA90 + 8, DINA90 + 8 }; + inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; + result = inv_set_mpu_memory(KEY_CFG_MOTION_BIAS, 3, regs); + return result; +} +#endif + +inv_error_t inv_update_bias(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char regs[12]; + short bias[GYRO_NUM_AXES]; + + if ((inv_params_obj.bias_mode & INV_BIAS_FROM_NO_MOTION) + && inv_get_gyro_present()) { + + regs[0] = DINAA0 + 3; + result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_get_mpu_memory(KEY_D_1_244, 12, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + inv_convert_bias(regs, bias); + + regs[0] = DINAA0 + 15; + result = inv_set_mpu_memory(KEY_FCFG_6, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_set_gyro_bias_in_hw_unit(bias, INV_SGB_NO_MOTION); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = + inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), + MPUREG_TEMP_OUT_H, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_mpu_memory(KEY_DMP_PREVPTAT, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + inv_obj.got_no_motion_bias = TRUE; + } + return INV_SUCCESS; +} + +inv_error_t MLAccelMotionDetection(struct inv_obj_t *inv_obj) +{ + long gain; + unsigned long timeChange; + long rate; + inv_error_t result; + long accel[3], temp; + long long accelMag; + unsigned long currentTime; + int kk; + + if (!inv_accel_present()) { + return INV_SUCCESS; + } + + currentTime = inv_get_tick_count(); + + // We always run the accel low pass filter at the highest sample rate possible + result = inv_get_accel(accel); + if (result != INV_ERROR_FEATURE_NOT_ENABLED) { + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + rate = inv_get_fifo_rate() * 5 + 5; + if (rate > 200) + rate = 200; + + gain = inv_obj->accel_lpf_gain * rate; + timeChange = inv_get_fifo_rate(); + + accelMag = 0; + for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { + inv_obj->accel_lpf[kk] = + inv_q30_mult(((1L << 30) - gain), inv_obj->accel_lpf[kk]); + inv_obj->accel_lpf[kk] += inv_q30_mult(gain, accel[kk]); + temp = accel[0] - inv_obj->accel_lpf[0]; + accelMag += (long long)temp *temp; + } + + if (accelMag > inv_obj->no_motion_accel_threshold) { + inv_obj->no_motion_accel_time = currentTime; + + // Check for change of state + if (!inv_get_gyro_present()) + inv_set_motion_state(INV_MOTION); + + } else if ((currentTime - inv_obj->no_motion_accel_time) > + 5 * inv_obj->motion_duration) { + // We have no motion according to accel + // Check fsor change of state + if (!inv_get_gyro_present()) + inv_set_motion_state(INV_NO_MOTION); + } + } + return INV_SUCCESS; +} + +/** + * @internal + * @brief Manually update the motion/no motion status. This is a + * convienence function for implementations that do not wish to use + * inv_update_data. + * This function can be called periodically to check for the + * 'no motion' state and update the internal motion status and bias + * calculations. + */ +inv_error_t MLPollMotionStatus(struct inv_obj_t * inv_obj) +{ + INVENSENSE_FUNC_START; + unsigned char regs[3] = { 0 }; + unsigned short motionFlag = 0; + unsigned long currentTime; + inv_error_t result; + + result = MLAccelMotionDetection(inv_obj); + + currentTime = inv_get_tick_count(); + + // If it is not time to poll for a no motion event, return + if (((inv_obj->interrupt_sources & INV_INT_MOTION) == 0) && + ((currentTime - inv_obj->poll_no_motion) <= 1000)) + return INV_SUCCESS; + + inv_obj->poll_no_motion = currentTime; + +#if defined CONFIG_MPU_SENSORS_MPU3050 + if (inv_get_gyro_present() + && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { + static long repeatBiasUpdateTime = 0; + + result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; + + _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); + ) + if (motionFlag == inv_obj->motion_duration) { + if (inv_obj->motion_state == INV_MOTION) { + inv_update_bias(); + repeatBiasUpdateTime = inv_get_tick_count(); + + regs[0] = DINAD8 + 1; + regs[1] = DINA0C; + regs[2] = DINAD8 + 2; + result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + regs[0] = 0; + regs[1] = 5; + result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + //Trigger no motion callback + inv_set_motion_state(INV_NO_MOTION); + } + } + if (motionFlag == 5) { + if (inv_obj->motion_state == INV_NO_MOTION) { + regs[0] = DINAD8 + 2; + regs[1] = DINA0C; + regs[2] = DINAD8 + 1; + result = inv_set_mpu_memory(KEY_CFG_18, 3, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + regs[0] = + (unsigned char)((inv_obj->motion_duration >> 8) & 0xff); + regs[1] = (unsigned char)(inv_obj->motion_duration & 0xff); + result = inv_set_mpu_memory(KEY_D_1_106, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + //Trigger no motion callback + inv_set_motion_state(INV_MOTION); + } + } + if (inv_obj->motion_state == INV_NO_MOTION) { + if ((inv_get_tick_count() - repeatBiasUpdateTime) > 4000) { + inv_update_bias(); + repeatBiasUpdateTime = inv_get_tick_count(); + } + } + } +#else // CONFIG_MPU_SENSORS_MPU3050 + if (inv_get_gyro_present() + && ((inv_params_obj.bias_mode & INV_BIAS_FROM_FAST_NO_MOTION) == 0)) { + result = inv_get_mpu_memory(KEY_D_1_98, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + motionFlag = (unsigned short)regs[0] * 256 + (unsigned short)regs[1]; + + _mlDebug(MPL_LOGV("motionFlag from RAM : 0x%04X\n", motionFlag); + ) + if (motionFlag > 0) { + unsigned char biasReg[12]; + long biasTmp2[3], biasTmp[3]; + int i; + + if (inv_obj->last_motion != motionFlag) { + result = inv_get_mpu_memory(KEY_D_2_96, 12, biasReg); + + for (i = 0; i < 3; i++) { + biasTmp2[i] = inv_big8_to_int32(&biasReg[i * 4]); + } + // Rotate bias vector by the transpose of the orientation matrix + for (i = 0; i < 3; ++i) { + biasTmp[i] = + inv_q30_mult(biasTmp2[0], + inv_obj->gyro_orient[i]) + + inv_q30_mult(biasTmp2[1], + inv_obj->gyro_orient[i + 3]) + + inv_q30_mult(biasTmp2[2], inv_obj->gyro_orient[i + 6]); + } + inv_obj->gyro_bias[0] = inv_q30_mult(biasTmp[0], 1501974482L); + inv_obj->gyro_bias[1] = inv_q30_mult(biasTmp[1], 1501974482L); + inv_obj->gyro_bias[2] = inv_q30_mult(biasTmp[2], 1501974482L); + } + inv_set_motion_state(INV_NO_MOTION); + } else { + // We are in a motion state + inv_set_motion_state(INV_MOTION); + } + inv_obj->last_motion = motionFlag; + + } +#endif // CONFIG_MPU_SENSORS_MPU3050 + return INV_SUCCESS; +} + +inv_error_t inv_enable_bias_no_motion(void) +{ + inv_error_t result; + inv_params_obj.bias_mode |= INV_BIAS_FROM_NO_MOTION; + result = + inv_register_fifo_rate_process(MLPollMotionStatus, + INV_PRIORITY_BIAS_NO_MOTION); +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_turn_on_bias_from_no_motion(); +#endif + return result; +} + +inv_error_t inv_disable_bias_no_motion(void) +{ + inv_error_t result; + inv_params_obj.bias_mode &= ~INV_BIAS_FROM_NO_MOTION; + result = inv_unregister_fifo_rate_process(MLPollMotionStatus); +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_turn_off_bias_from_no_motion(); +#endif + return result; +} diff --git a/60xx/mlsdk/mllite/mlBiasNoMotion.h b/60xx/mlsdk/mllite/mlBiasNoMotion.h new file mode 100644 index 0000000..030dbf9 --- /dev/null +++ b/60xx/mlsdk/mllite/mlBiasNoMotion.h @@ -0,0 +1,40 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef INV_BIAS_NO_MOTION_H__ +#define INV_BIAS_NO_MOTION_H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_enable_bias_no_motion(void); + inv_error_t inv_disable_bias_no_motion(void); + +#ifdef __cplusplus +} +#endif +#endif // INV_BIAS_NO_MOTION_H__ diff --git a/60xx/mlsdk/mllite/mlFIFO.c b/60xx/mlsdk/mllite/mlFIFO.c new file mode 100644 index 0000000..2c0d2f0 --- /dev/null +++ b/60xx/mlsdk/mllite/mlFIFO.c @@ -0,0 +1,2265 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mlFIFO.c 5653 2011-06-16 21:06:55Z nroyer $ + * + *******************************************************************************/ + +/** + * @defgroup MLFIFO + * @brief Motion Library - FIFO Driver. + * The FIFO API Interface. + * + * @{ + * @file mlFIFO.c + * @brief FIFO Interface. +**/ + +#include +#include "mpu.h" +#if defined CONFIG_MPU_SENSORS_MPU6050A2 +# include "mpu6050a2.h" +#elif defined CONFIG_MPU_SENSORS_MPU6050B1 +# include "mpu6050b1.h" +#elif defined CONFIG_MPU_SENSORS_MPU3050 +# include "mpu3050.h" +#else +#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx +#endif +#include "mlFIFO.h" +#include "mlFIFOHW.h" +#include "dmpKey.h" +#include "mlMathFunc.h" +#include "ml.h" +#include "mldl.h" +#include "mldl_cfg.h" +#include "mlstates.h" +#include "mlsupervisor.h" +#include "mlos.h" +#include "mlmath.h" +#include "accel.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-fifo" + +#define FIFO_DEBUG 0 + +#define REF_QUATERNION (0) +#define REF_GYROS (REF_QUATERNION + 4) +#define REF_CONTROL (REF_GYROS + 3) +#define REF_RAW (REF_CONTROL + 4) +#define REF_RAW_EXTERNAL (REF_RAW + 8) +#define REF_ACCEL (REF_RAW_EXTERNAL + 6) +#define REF_QUANT_ACCEL (REF_ACCEL + 3) +#define REF_QUATERNION_6AXIS (REF_QUANT_ACCEL + INV_MAX_NUM_ACCEL_SAMPLES) +#define REF_EIS (REF_QUATERNION_6AXIS + 4) +#define REF_DMP_PACKET (REF_EIS + 3) +#define REF_GARBAGE (REF_DMP_PACKET + 1) +#define REF_LAST (REF_GARBAGE + 1) + +long fifo_scale[REF_LAST] = { + (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion + // 2^(16+30)/((2^30)*((3.14159265358/180)/200)/2) + 1501974482L, 1501974482L, 1501974482L, // Gyro + (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Control + (1L << 14), // Temperature + (1L << 14), (1L << 14), (1L << 14), // Raw Gyro + (1L << 14), (1L << 14), (1L << 14), (0), // Raw Accel, plus padding + (1L << 14), (1L << 14), (1L << 14), // Raw External + (1L << 14), (1L << 14), (1L << 14), // Raw External + (1L << 16), (1L << 16), (1L << 16), // Accel + (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quant Accel + (1L << 30), (1L << 30), (1L << 30), (1L << 30), //Quant Accel + (1L << 30), (1L << 30), (1L << 30), (1L << 30), // Quaternion 6 Axis + (1L << 30), (1L << 30), (1L << 30), // EIS + (1L << 30), // Packet + (1L << 30), // Garbage +}; + +// The scale factors for tap need to match the number in fifo_scale above. +// fifo_base_offset below may also need to be changed if this is not 8 +#if INV_MAX_NUM_ACCEL_SAMPLES != 8 +#error INV_MAX_NUM_ACCEL_SAMPLES must be defined to 8 +#endif + +#define CONFIG_QUAT (0) +#define CONFIG_GYROS (CONFIG_QUAT + 1) +#define CONFIG_CONTROL_DATA (CONFIG_GYROS + 1) +#define CONFIG_TEMPERATURE (CONFIG_CONTROL_DATA + 1) +#define CONFIG_RAW_DATA (CONFIG_TEMPERATURE + 1) +#define CONFIG_RAW_EXTERNAL (CONFIG_RAW_DATA + 1) +#define CONFIG_ACCEL (CONFIG_RAW_EXTERNAL + 1) +#define CONFIG_DMP_QUANT_ACCEL (CONFIG_ACCEL + 1) +#define CONFIG_EIS (CONFIG_DMP_QUANT_ACCEL + 1) +#define CONFIG_DMP_PACKET_NUMBER (CONFIG_EIS + 1) +#define CONFIG_FOOTER (CONFIG_DMP_PACKET_NUMBER + 1) +#define NUMFIFOELEMENTS (CONFIG_FOOTER + 1) + +const int fifo_base_offset[NUMFIFOELEMENTS] = { + REF_QUATERNION * 4, + REF_GYROS * 4, + REF_CONTROL * 4, + REF_RAW * 4, + REF_RAW * 4 + 4, + REF_RAW_EXTERNAL * 4, + REF_ACCEL * 4, + REF_QUANT_ACCEL * 4, + REF_EIS * 4, + REF_DMP_PACKET * 4, + REF_GARBAGE * 4 +}; + +struct fifo_obj { + void (*fifo_process_cb) (void); + long decoded[REF_LAST]; + long decoded_accel[INV_MAX_NUM_ACCEL_SAMPLES][ACCEL_NUM_AXES]; + int offsets[REF_LAST * 4]; + int cache; + uint_fast8_t gyro_source; + unsigned short fifo_rate; + unsigned short sample_step_size_ms; + uint_fast16_t fifo_packet_size; + uint_fast16_t data_config[NUMFIFOELEMENTS]; + unsigned char reference_count[REF_LAST]; + long acc_bias_filt[3]; + float acc_filter_coef; + long gravity_cache[3]; +}; + +static struct fifo_obj fifo_obj; + +#define FIFO_CACHE_TEMPERATURE 1 +#define FIFO_CACHE_GYRO 2 +#define FIFO_CACHE_GRAVITY_BODY 4 +#define FIFO_CACHE_ACC_BIAS 8 + +struct fifo_rate_obj { + // These describe callbacks happening everytime a FIFO block is processed + int_fast8_t num_cb; + HANDLE mutex; + inv_obj_func fifo_process_cb[MAX_HIGH_RATE_PROCESSES]; + int priority[MAX_HIGH_RATE_PROCESSES]; +}; + +struct fifo_rate_obj fifo_rate_obj; + +/** Sets accuracy to be one of 0, INV_32_BIT, or INV_16_BIT. Looks up old + * accuracy if needed. + */ +static uint_fast16_t inv_set_fifo_accuracy(uint_fast16_t elements, + uint_fast16_t accuracy, + uint_fast8_t configOffset) +{ + if (elements) { + if (!accuracy) + accuracy = fifo_obj.data_config[configOffset]; + else if (accuracy & INV_16_BIT) + if ((fifo_obj.data_config[configOffset] & INV_32_BIT)) + accuracy = INV_32_BIT; // 32-bits takes priority + else + accuracy = INV_16_BIT; + else + accuracy = INV_32_BIT; + } else { + accuracy = 0; + } + + return accuracy; +} + +/** Adjusts (len) Reference Counts, at offset (refOffset). If increment is 0, + * the reference counts are subtracted, otherwise they are incremented for each + * bit set in element. The value returned are the elements that should be sent + * out as data through the FIFO. +*/ +static uint_fast16_t inv_set_fifo_reference(uint_fast16_t elements, + uint_fast16_t increment, + uint_fast8_t refOffset, + uint_fast8_t len) +{ + uint_fast8_t kk; + + if (increment == 0) { + for (kk = 0; kk < len; ++kk) { + if ((elements & 1) + && (fifo_obj.reference_count[kk + refOffset] > 0)) { + fifo_obj.reference_count[kk + refOffset]--; + } + elements >>= 1; + } + } else { + for (kk = 0; kk < len; ++kk) { + if (elements & 1) + fifo_obj.reference_count[kk + refOffset]++; + elements >>= 1; + } + } + elements = 0; + for (kk = 0; kk < len; ++kk) { + if (fifo_obj.reference_count[kk + refOffset] > 0) + elements |= (1 << kk); + } + return elements; +} + +/** + * @param[in] accuracy INV_16_BIT or INV_32_BIT when constructing data to send + * out the FIFO, 0 when removing from the FIFO. + */ +static inv_error_t inv_construct3_fifo(unsigned char *regs, + uint_fast16_t elements, + uint_fast16_t accuracy, + uint_fast8_t refOffset, + unsigned short key, + uint_fast8_t configOffset) +{ + int_fast8_t kk; + inv_error_t result; + + elements = inv_set_fifo_reference(elements, accuracy, refOffset, 3); + accuracy = inv_set_fifo_accuracy(elements, accuracy, configOffset); + + if (accuracy & INV_16_BIT) { + regs[0] = DINAF8 + 2; + } + + fifo_obj.data_config[configOffset] = elements | accuracy; + + for (kk = 0; kk < 3; ++kk) { + if ((elements & 1) == 0) + regs[kk + 1] = DINAA0 + 3; + elements >>= 1; + } + + result = inv_set_mpu_memory(key, 4, regs); + + return result; +} + +/** + * @internal + * Puts footer on FIFO data. + */ +static inv_error_t inv_set_footer(void) +{ + unsigned char regs = DINA30; + uint_fast8_t tmp_count; + int_fast8_t i, j; + int offset; + int result; + int *fifo_offsets_ptr = fifo_obj.offsets; + + fifo_obj.fifo_packet_size = 0; + for (i = 0; i < NUMFIFOELEMENTS; i++) { + tmp_count = 0; + offset = fifo_base_offset[i]; + for (j = 0; j < 8; j++) { + if ((fifo_obj.data_config[i] >> j) & 0x0001) { +#ifndef BIG_ENDIAN + // Special Case for Byte Ordering on Accel Data + if ((i == CONFIG_RAW_DATA) && (j > 2)) { + tmp_count += 2; + switch (inv_get_dl_config()->accel->endian) { + case EXT_SLAVE_BIG_ENDIAN: + *fifo_offsets_ptr++ = offset + 3; + *fifo_offsets_ptr++ = offset + 2; + break; + case EXT_SLAVE_LITTLE_ENDIAN: + *fifo_offsets_ptr++ = offset + 2; + *fifo_offsets_ptr++ = offset + 3; + break; + case EXT_SLAVE_FS8_BIG_ENDIAN: + if (j == 3) { + // Throw this byte away + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + *fifo_offsets_ptr++ = offset + 3; + } else if (j == 4) { + *fifo_offsets_ptr++ = offset + 3; + *fifo_offsets_ptr++ = offset + 7; + } else { + // Throw these byte away + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + } + break; + case EXT_SLAVE_FS16_BIG_ENDIAN: + if (j == 3) { + // Throw this byte away + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + *fifo_offsets_ptr++ = offset + 3; + } else if (j == 4) { + *fifo_offsets_ptr++ = offset - 2; + *fifo_offsets_ptr++ = offset + 3; + } else { + *fifo_offsets_ptr++ = offset - 2; + *fifo_offsets_ptr++ = offset + 3; + } + break; + default: + return INV_ERROR; // Bad value on ordering + } + } else { + tmp_count += 2; + *fifo_offsets_ptr++ = offset + 3; + *fifo_offsets_ptr++ = offset + 2; + if (fifo_obj.data_config[i] & INV_32_BIT) { + *fifo_offsets_ptr++ = offset + 1; + *fifo_offsets_ptr++ = offset; + tmp_count += 2; + } + } +#else + // Big Endian Platform + // Special Case for Byte Ordering on Accel Data + if ((i == CONFIG_RAW_DATA) && (j > 2)) { + tmp_count += 2; + switch (inv_get_dl_config()->accel->endian) { + case EXT_SLAVE_BIG_ENDIAN: + *fifo_offsets_ptr++ = offset + 2; + *fifo_offsets_ptr++ = offset + 3; + break; + case EXT_SLAVE_LITTLE_ENDIAN: + *fifo_offsets_ptr++ = offset + 3; + *fifo_offsets_ptr++ = offset + 2; + break; + case EXT_SLAVE_FS8_BIG_ENDIAN: + if (j == 3) { + // Throw this byte away + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + *fifo_offsets_ptr++ = offset; + } else if (j == 4) { + *fifo_offsets_ptr++ = offset; + *fifo_offsets_ptr++ = offset + 4; + } else { + // Throw these bytes away + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + } + break; + case EXT_SLAVE_FS16_BIG_ENDIAN: + if (j == 3) { + // Throw this byte away + *fifo_offsets_ptr++ = + fifo_base_offset[CONFIG_FOOTER]; + *fifo_offsets_ptr++ = offset; + } else if (j == 4) { + *fifo_offsets_ptr++ = offset - 3; + *fifo_offsets_ptr++ = offset; + } else { + *fifo_offsets_ptr++ = offset - 3; + *fifo_offsets_ptr++ = offset; + } + break; + default: + return INV_ERROR; // Bad value on ordering + } + } else { + tmp_count += 2; + *fifo_offsets_ptr++ = offset; + *fifo_offsets_ptr++ = offset + 1; + if (fifo_obj.data_config[i] & INV_32_BIT) { + *fifo_offsets_ptr++ = offset + 2; + *fifo_offsets_ptr++ = offset + 3; + tmp_count += 2; + } + } + +#endif + } + offset += 4; + } + fifo_obj.fifo_packet_size += tmp_count; + } + if (fifo_obj.data_config[CONFIG_FOOTER] == 0 && + fifo_obj.fifo_packet_size > 0) { + // Add footer + result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + fifo_obj.data_config[CONFIG_FOOTER] = 0x0001 | INV_16_BIT; + fifo_obj.fifo_packet_size += 2; + } else if (fifo_obj.data_config[CONFIG_FOOTER] && + (fifo_obj.fifo_packet_size == 2)) { + // Remove Footer + regs = DINAA0 + 3; + result = inv_set_mpu_memory(KEY_CFG_16, 1, ®s); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + fifo_obj.data_config[CONFIG_FOOTER] = 0; + fifo_obj.fifo_packet_size = 0; + } + + return INV_SUCCESS; +} + +inv_error_t inv_decode_quantized_accel(void) +{ + int kk; + int fifoRate = inv_get_fifo_rate(); + + if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (kk = (INV_MAX_NUM_ACCEL_SAMPLES - (fifoRate + 1)); + kk < INV_MAX_NUM_ACCEL_SAMPLES; kk++) { + union { + unsigned int u10:10; + signed int s10:10; + } temp; + + union { + uint32_t u32; + int32_t s32; + } value; + + value.u32 = fifo_obj.decoded[REF_QUANT_ACCEL + kk]; + // unquantize this samples. + // They are stored as x * 2^20 + y * 2^10 + z + // Z + temp.u10 = value.u32 & 0x3ff; + value.s32 -= temp.s10; + fifo_obj.decoded_accel[kk][2] = temp.s10 * 64; + // Y + value.s32 = value.s32 / 1024; + temp.u10 = value.u32 & 0x3ff; + value.s32 -= temp.s10; + fifo_obj.decoded_accel[kk][1] = temp.s10 * 64; + // X + value.s32 = value.s32 / 1024; + temp.u10 = value.u32 & 0x3ff; + fifo_obj.decoded_accel[kk][0] = temp.s10 * 64; + } + return INV_SUCCESS; +} + +static inv_error_t inv_state_change_fifo(unsigned char newState) +{ + inv_error_t result = INV_SUCCESS; + unsigned char regs[4]; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + /* Don't reset the fifo on a fifo rate change */ + if ((mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && + (newState != inv_get_state()) && (inv_dmpkey_supported(KEY_D_1_178))) { + /* Delay output on restart by 50ms due to warm up time of gyros */ + + short delay = (short)-((50 / inv_get_sample_step_size_ms()) + 1); + inv_init_fifo_hardare(); + inv_int16_to_big8(delay, regs); + result = inv_set_mpu_memory(KEY_D_1_178, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + if (INV_STATE_DMP_STARTED == newState) { + if (inv_dmpkey_supported(KEY_D_1_128)) { + double tmp; + tmp = (0x20000000L * M_PI) / (fifo_obj.fifo_rate + 1); + if (tmp > 0x40000000L) + tmp = 0x40000000L; + (void)inv_int32_to_big8((long)tmp, regs); + result = inv_set_mpu_memory(KEY_D_1_128, sizeof(long), regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_reset_fifo(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + return result; +} + +/** + * @internal + * @brief get the FIFO packet size + * @return the FIFO packet size + */ +uint_fast16_t inv_get_fifo_packet_size(void) +{ + return fifo_obj.fifo_packet_size; +} + +/** + * @brief Initializes all the internal static variables for + * the FIFO module. + * @note Should be called by the initialization routine such + * as inv_dmp_open(). + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_init_fifo_param(void) +{ + inv_error_t result; + memset(&fifo_obj, 0, sizeof(struct fifo_obj)); + fifo_obj.decoded[REF_QUATERNION] = 1073741824L; // Set to Identity + inv_set_linear_accel_filter_coef(0.f); + fifo_obj.fifo_rate = 20; + fifo_obj.sample_step_size_ms = 100; + memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); + result = inv_create_mutex(&fifo_rate_obj.mutex); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_register_state_callback(inv_state_change_fifo); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * @brief Close the FIFO usage. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_close_fifo(void) +{ + inv_error_t result; + inv_error_t first = INV_SUCCESS; + result = inv_unregister_state_callback(inv_state_change_fifo); + ERROR_CHECK_FIRST(first, result); + result = inv_destroy_mutex(fifo_rate_obj.mutex); + ERROR_CHECK_FIRST(first, result); + memset(&fifo_rate_obj, 0, sizeof(struct fifo_rate_obj)); + return first; +} + +/** + * Set the gyro source to output to the fifo + * + * @param source The source. One of + * - INV_GYRO_FROM_RAW + * - INV_GYRO_FROM_QUATERNION + * + * @return INV_SUCCESS or non-zero error code; + */ +inv_error_t inv_set_gyro_data_source(uint_fast8_t source) +{ + if (source != INV_GYRO_FROM_QUATERNION && source != INV_GYRO_FROM_RAW) { + return INV_ERROR_INVALID_PARAMETER; + } + + fifo_obj.gyro_source = source; + return INV_SUCCESS; +} + +/** + * @brief Reads and processes FIFO data. Also handles callbacks when data is + * ready. + * @param numPackets + * Number of FIFO packets to try to read. You should + * use a large number here, such as 100, if you want to read all + * the full packets in the FIFO, which is typical operation. + * @param processed + * The number of FIFO packets processed. This may be incremented + * even if high rate processes later fail. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, + int_fast8_t * processed) +{ + int_fast8_t packet; + inv_error_t result = INV_SUCCESS; + uint_fast16_t read; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + int kk; + + if (NULL == processed) + return INV_ERROR_INVALID_PARAMETER; + + *processed = 0; + if (fifo_obj.fifo_packet_size == 0) + return result; // Nothing to read + + for (packet = 0; packet < numPackets; ++packet) { + if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { + unsigned char footer_n_data[MAX_FIFO_LENGTH + FIFO_FOOTER_SIZE]; + unsigned char *buf = &footer_n_data[FIFO_FOOTER_SIZE]; + read = inv_get_fifo((uint_fast16_t) fifo_obj.fifo_packet_size, + footer_n_data); + if (0 == read || + read != fifo_obj.fifo_packet_size - FIFO_FOOTER_SIZE) { + result = inv_get_fifo_status(); + if (INV_SUCCESS != result) { + memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); + } + return result; + } + + result = inv_process_fifo_packet(buf); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else if (inv_accel_present()) { + long data[ACCEL_NUM_AXES]; + result = inv_get_accel_data(data); + if (result == INV_ERROR_ACCEL_DATA_NOT_READY) { + return INV_SUCCESS; + } + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + memset(fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); + fifo_obj.cache = 0; + for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { + fifo_obj.decoded[REF_RAW + 4 + kk] = + inv_q30_mult((data[kk] << 16), + fifo_scale[REF_RAW + 4 + kk]); + fifo_obj.decoded[REF_ACCEL + kk] = + inv_q30_mult((data[kk] << 15), fifo_scale[REF_ACCEL + kk]); + fifo_obj.decoded[REF_ACCEL + kk] -= + inv_obj.scaled_accel_bias[kk]; + } + } + // The buffer was processed correctly, so increment even if + // other processes fail later, which will return an error + *processed = *processed + 1; + + if ((fifo_obj.fifo_rate < INV_MAX_NUM_ACCEL_SAMPLES) && + fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) { + result = inv_decode_quantized_accel(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + if (fifo_obj.data_config[CONFIG_QUAT]) { + result = inv_accel_compass_supervisor(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + result = inv_pressure_supervisor(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + // Callbacks now that we have a buffer of data ready + result = inv_run_fifo_rate_processes(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + } + return result; +} + +/** + * @brief inv_set_fifo_processed_callback is used to set a processed data callback + * function. inv_set_fifo_processed_callback sets a user defined callback + * function that triggers when all the decoding has been finished by + * the motion processing engines. It is called before other bigger + * processing engines to allow lower latency for the user. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must NOT have been called. + * + * @param func A user defined callback function. + * + * @return INV_SUCCESS if successful, or non-zero error code otherwise. + */ +inv_error_t inv_set_fifo_processed_callback(void (*func) (void)) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + fifo_obj.fifo_process_cb = func; + + return INV_SUCCESS; +} + +/** + * @internal + * @brief Process data from the dmp read via the fifo. Takes a buffer + * filled with bytes read from the DMP FIFO. + * Currently expects only the 16 bytes of quaternion data. + * Calculates the motion parameters from that data and stores the + * results in an internal data structure. + * @param[in,out] dmpData Pointer to the buffer containing the fifo data. + * @return INV_SUCCESS or error code. +**/ +inv_error_t inv_process_fifo_packet(const unsigned char *dmpData) +{ + INVENSENSE_FUNC_START; + int N, kk; + unsigned char *p; + + p = (unsigned char *)(&fifo_obj.decoded); + N = fifo_obj.fifo_packet_size; + if (N > sizeof(fifo_obj.decoded)) + return INV_ERROR_ASSERTION_FAILURE; + + memset(&fifo_obj.decoded, 0, sizeof(fifo_obj.decoded)); + + for (kk = 0; kk < N; ++kk) { + p[fifo_obj.offsets[kk]] = *dmpData++; + } + + // If multiplies are much greater cost than if checks, you could check + // to see if fifo_scale is non-zero first, or equal to (1L<<30) + for (kk = 0; kk < REF_LAST; ++kk) { + fifo_obj.decoded[kk] = + inv_q30_mult(fifo_obj.decoded[kk], fifo_scale[kk]); + } + + memcpy(&fifo_obj.decoded[REF_QUATERNION_6AXIS], + &fifo_obj.decoded[REF_QUATERNION], 4 * sizeof(long)); + + inv_obj.flags[INV_PROCESSED_DATA_READY] = 1; + fifo_obj.cache = 0; + + return INV_SUCCESS; +} + +/** Converts 16-bit temperature data as read from temperature register +* into Celcius scaled by 2^16. +*/ +long inv_decode_temperature(short tempReg) +{ +#if defined CONFIG_MPU_SENSORS_MPU6050A2 + // Celcius = 35 + (T + 3048.7)/325.9 + return 2906830L + inv_q30_mult((long)tempReg << 16, 3294697L); +#endif +#if defined CONFIG_MPU_SENSORS_MPU6050B1 + // Celcius = 35 + (T + 927.4)/360.6 + return 2462307L + inv_q30_mult((long)tempReg << 16, 2977653L); +#endif +#if defined CONFIG_MPU_SENSORS_MPU3050 + // Celcius = 35 + (T + 13200)/280 + return 5383314L + inv_q30_mult((long)tempReg << 16, 3834792L); +#endif +} + +/** @internal +* Returns the temperature in hardware units. The scaling may change from part to part. +*/ +inv_error_t inv_get_temperature_raw(short *data) +{ + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_TEMPERATURE]) { + inv_error_t result; + unsigned char regs[2]; + if ((fifo_obj.cache & FIFO_CACHE_TEMPERATURE) == 0) { + if (FIFO_DEBUG) + MPL_LOGI("Fetching the temperature from the registers\n"); + fifo_obj.cache |= FIFO_CACHE_TEMPERATURE; + result = inv_serial_read(inv_get_serial_handle(), + inv_get_mpu_slave_addr(), MPUREG_TEMP_OUT_H, 2, + regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + fifo_obj.decoded[REF_RAW] = ((short)regs[0] << 8) | (regs[1]); + } + } + *data = (short)fifo_obj.decoded[REF_RAW]; + return INV_SUCCESS; +} + +/** + * @brief Returns 1-element vector of temperature. It is read from the hardware if it + * doesn't exist in the FIFO. + * @param[out] data 1-element vector of temperature + * @return 0 on success or an error code. + */ +inv_error_t inv_get_temperature(long *data) +{ + short tr; + inv_error_t result; + + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + result = inv_get_temperature_raw(&tr); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + data[0] = inv_decode_temperature(tr); + return INV_SUCCESS; +} + +/** + * @brief Get the Decoded Accel Data. + * @param data + * a buffer to store the quantized data. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_get_unquantized_accel(long *data) +{ + int ii, kk; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { + for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { + data[ii * ACCEL_NUM_AXES + kk] = fifo_obj.decoded_accel[ii][kk]; + } + } + + return INV_SUCCESS; +} + +/** + * @brief Get the Quantized Accel data algorithm output from the FIFO. + * @param data + * a buffer to store the quantized data. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_get_quantized_accel(long *data) +{ + int ii; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (ii = 0; ii < INV_MAX_NUM_ACCEL_SAMPLES; ii++) { + data[ii] = fifo_obj.decoded[REF_QUANT_ACCEL + ii]; + } + + return INV_SUCCESS; +} + +/** This gets raw gyro data. The data is taken from the FIFO if it was put in the FIFO +* and it is read from the registers if it was not put into the FIFO. The data is +* cached till the next FIFO processing block time. +* @param[out] data Length 3, Gyro data +*/ +inv_error_t inv_get_gyro_sensor(long *data) +{ + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + if ((fifo_obj.data_config[CONFIG_RAW_DATA] & 7) != 7) { + inv_error_t result; + unsigned char regs[6]; + if ((fifo_obj.cache & FIFO_CACHE_GYRO) == 0) { + fifo_obj.cache |= FIFO_CACHE_GYRO; + result = + inv_serial_read(inv_get_serial_handle(), + inv_get_mpu_slave_addr(), MPUREG_GYRO_XOUT_H, 6, + regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + fifo_obj.decoded[REF_RAW + 1] = + (((long)regs[0]) << 24) | (((long)regs[1]) << 16); + fifo_obj.decoded[REF_RAW + 2] = + (((long)regs[2]) << 24) | (((long)regs[3]) << 16); + fifo_obj.decoded[REF_RAW + 3] = + (((long)regs[4]) << 24) | (((long)regs[5]) << 16); + + // Temperature starts at location 0, Gyro at location 1. + fifo_obj.decoded[REF_RAW + 1] = + inv_q30_mult(fifo_obj.decoded[REF_RAW + 1], + fifo_scale[REF_RAW + 1]); + fifo_obj.decoded[REF_RAW + 2] = + inv_q30_mult(fifo_obj.decoded[REF_RAW + 2], + fifo_scale[REF_RAW + 2]); + fifo_obj.decoded[REF_RAW + 3] = + inv_q30_mult(fifo_obj.decoded[REF_RAW + 3], + fifo_scale[REF_RAW + 3]); + } + data[0] = fifo_obj.decoded[REF_RAW + 1]; + data[1] = fifo_obj.decoded[REF_RAW + 2]; + data[2] = fifo_obj.decoded[REF_RAW + 3]; + } else { + long data2[6]; + inv_get_gyro_and_accel_sensor(data2); + data[0] = data2[0]; + data[1] = data2[1]; + data[2] = data2[2]; + } + return INV_SUCCESS; +} + +/** + * @brief Returns 6-element vector of gyro and accel data + * @param[out] data 6-element vector of gyro and accel data + * @return 0 on success or an error code. + */ +inv_error_t inv_get_gyro_and_accel_sensor(long *data) +{ + int ii; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_RAW_DATA]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (ii = 0; ii < (GYRO_NUM_AXES + ACCEL_NUM_AXES); ii++) { + data[ii] = fifo_obj.decoded[REF_RAW + 1 + ii]; + } + + return INV_SUCCESS; +} + +/** + * @brief Returns 3-element vector of external sensor + * @param[out] data 3-element vector of external sensor + * @return 0 on success or an error code. + */ +inv_error_t inv_get_external_sensor_data(long *data, int size) +{ +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + int ii; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_RAW_EXTERNAL]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (ii = 0; ii < size && ii < 6; ii++) { + data[ii] = fifo_obj.decoded[REF_RAW_EXTERNAL + ii]; + } + + return INV_SUCCESS; +#else + memset(data, 0, COMPASS_NUM_AXES * sizeof(long)); + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; +#endif +} + +/** + * Sends accelerometer data to the FIFO. + * + * @param[in] elements Which of the 3 elements to send. Use INV_ALL for 3 axis + * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together + * for a subset. + * + * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 + * bit data. Set to zero to remove it from the FIFO. + */ +inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + unsigned char regs[4] = { DINAF8 + 1, DINA28, DINA30, DINA38 }; + inv_error_t result; + int kk; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + result = inv_construct3_fifo(regs, elements, accuracy, REF_ACCEL, + KEY_CFG_12, CONFIG_ACCEL); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + for (kk = 0; kk < ACCEL_NUM_AXES; kk++) { + fifo_scale[REF_ACCEL + kk] = 2 * inv_obj.accel_sens; + } + + return inv_set_footer(); +} + +/** + * Sends control data to the FIFO. Control data is a 4 length vector of 32-bits. + * + * @param[in] elements Which of the 4 elements to send. Use INV_ALL for all + * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3, INV_ELEMENT_4 or'd + * together for a subset. + * + * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 + * bit data. Set to zero to remove it from the FIFO. + */ +inv_error_t inv_send_cntrl_data(uint_fast16_t elements, uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + int_fast8_t kk; + inv_error_t result; + unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, DINA30, DINA38 }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + elements = inv_set_fifo_reference(elements, accuracy, REF_CONTROL, 4); + accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_CONTROL_DATA); + + if (accuracy & INV_16_BIT) { + regs[0] = DINAF8 + 2; + } + + fifo_obj.data_config[CONFIG_CONTROL_DATA] = elements | accuracy; + + for (kk = 0; kk < 4; ++kk) { + if ((elements & 1) == 0) + regs[kk + 1] = DINAA0 + 3; + elements >>= 1; + } + + result = inv_set_mpu_memory(KEY_CFG_1, 5, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); +} + +/** + * Adds a rolling counter to the fifo packet. When used with the footer + * the data comes out the first time: + * + * @code + * ... + * @endcode + * for every other packet it is + * + * @code + * ... + * @endcode + * + * This allows for scanning of the fifo for packets + * + * @return INV_SUCCESS or error code + */ +inv_error_t inv_send_packet_number(uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char regs; + uint_fast16_t elements; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + elements = inv_set_fifo_reference(1, accuracy, REF_DMP_PACKET, 1); + if (elements & 1) { + regs = DINA28; + fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = + INV_ELEMENT_1 | INV_16_BIT; + } else { + regs = DINAF8 + 3; + fifo_obj.data_config[CONFIG_DMP_PACKET_NUMBER] = 0; + } + result = inv_set_mpu_memory(KEY_CFG_23, 1, ®s); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); +} + +/** + * @brief Send the computed gravity vectors into the FIFO. + * The gravity vectors can be retrieved from the FIFO via + * inv_get_gravity(), to have the gravitation vector expressed + * in coordinates relative to the body. + * + * Gravity is a derived vector derived from the quaternion. + * @param elements + * the gravitation vectors components bitmask. + * To send all compoents use INV_ALL. + * @param accuracy + * The number of bits the gravitation vector is expressed + * into. + * Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 + * bit data. + * Set to zero to remove it from the FIFO. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_send_gravity(uint_fast16_t elements, uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + + result = inv_send_quaternion(accuracy); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); +} + +/** Sends gyro data to the FIFO. Gyro data is a 3 length vector + * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). + * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them + * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together + * for a subset. + * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 + * bit data. Set to zero to remove it from the FIFO. + */ +inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + unsigned char regs[4] = { DINAF8 + 1, DINA20, DINA28, DINA30 }; + inv_error_t result; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (fifo_obj.gyro_source == INV_GYRO_FROM_QUATERNION) { + regs[0] = DINA90 + 5; + result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + regs[0] = DINAF8 + 1; + regs[1] = DINA20; + regs[2] = DINA28; + regs[3] = DINA30; + } else { + regs[0] = DINA90 + 10; + result = inv_set_mpu_memory(KEY_CFG_GYRO_SOURCE, 1, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + regs[0] = DINAF8 + 1; + regs[1] = DINA28; + regs[2] = DINA30; + regs[3] = DINA38; + } + result = inv_construct3_fifo(regs, elements, accuracy, REF_GYROS, + KEY_CFG_9, CONFIG_GYROS); + + return inv_set_footer(); +} + +/** Sends linear accelerometer data to the FIFO. + * + * Linear accelerometer data is a 3 length vector of 32-bits. It is the + * acceleration in the body frame with gravity removed. + * + * + * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of + * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together + * for a subset. + * + * NOTE: Elements is ignored if the fifo rate is < INV_MAX_NUM_ACCEL_SAMPLES + * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 + * bit data. Set to zero to remove it from the FIFO. + */ +inv_error_t inv_send_linear_accel(uint_fast16_t elements, + uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char state = inv_get_state(); + + if (state < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + result = inv_send_gravity(elements, accuracy); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_send_accel(elements, accuracy); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); +} + +/** Sends linear world accelerometer data to the FIFO. Linear world + * accelerometer data is a 3 length vector of 32-bits. It is the acceleration + * in the world frame with gravity removed. Should be called once after + * inv_dmp_open() and before inv_dmp_start(). + * + * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of + * them or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together + * for a subset. + * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 + * bit data. + */ +inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements, + uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + + result = inv_send_linear_accel(INV_ALL, accuracy); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_send_quaternion(accuracy); + + return inv_set_footer(); +} + +/** Sends quaternion data to the FIFO. Quaternion data is a 4 length vector + * of 32-bits. Should be called once after inv_dmp_open() and before inv_dmp_start(). + * @param[in] accuracy Set to INV_32_BIT for 32-bit data, or INV_16_BIT for 16 + * bit data. + */ +inv_error_t inv_send_quaternion(uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, + DINA30, DINA38 + }; + uint_fast16_t elements, kk; + inv_error_t result; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + elements = inv_set_fifo_reference(0xf, accuracy, REF_QUATERNION, 4); + accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_QUAT); + + if (accuracy & INV_16_BIT) { + regs[0] = DINAF8 + 2; + } + + fifo_obj.data_config[CONFIG_QUAT] = elements | accuracy; + + for (kk = 0; kk < 4; ++kk) { + if ((elements & 1) == 0) + regs[kk + 1] = DINAA0 + 3; + elements >>= 1; + } + + result = inv_set_mpu_memory(KEY_CFG_8, 5, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); +} + +/** Sends raw data to the FIFO. + * Should be called once after inv_dmp_open() and before inv_dmp_start(). + * @param[in] elements Which of the 7 elements to send. Use INV_ALL for all of them + * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 ... INV_ELEMENT_7 or'd together + * for a subset. The first element is temperature, the next 3 are gyro data, + * and the last 3 accel data. + * @param accuracy + * The element's accuracy, can be INV_16_BIT, INV_32_BIT, or 0 to turn off. + * @return 0 if successful, a non-zero error code otherwise. + */ +inv_error_t inv_send_sensor_data(uint_fast16_t elements, uint_fast16_t accuracy) +{ + int result; +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + unsigned char regs[7] = { DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, + DINAA0 + 3, DINAA0 + 3, DINAA0 + 3, + DINAA0 + 3 + }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (accuracy) + accuracy = INV_16_BIT; + + elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); + + if (elements & 1) + fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; + else + fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; + if (elements & 0x7e) + fifo_obj.data_config[CONFIG_RAW_DATA] = + (0x3f & (elements >> 1)) | INV_16_BIT; + else + fifo_obj.data_config[CONFIG_RAW_DATA] = 0; + + if (elements & INV_ELEMENT_1) { + regs[0] = DINACA; + } + if (elements & INV_ELEMENT_2) { + regs[1] = DINBC4; + } + if (elements & INV_ELEMENT_3) { + regs[2] = DINACC; + } + if (elements & INV_ELEMENT_4) { + regs[3] = DINBC6; + } + if (elements & INV_ELEMENT_5) { + regs[4] = DINBC0; + } + if (elements & INV_ELEMENT_6) { + regs[5] = DINAC8; + } + if (elements & INV_ELEMENT_7) { + regs[6] = DINBC2; + } + result = inv_set_mpu_memory(KEY_CFG_15, 7, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); + +#else + INVENSENSE_FUNC_START; + unsigned char regs[4] = { DINAA0 + 3, + DINAA0 + 3, + DINAA0 + 3, + DINAA0 + 3 + }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (accuracy) + accuracy = INV_16_BIT; + + elements = inv_set_fifo_reference(elements, accuracy, REF_RAW, 7); + + if (elements & 0x03) { + elements |= 0x03; + regs[0] = DINA20; + } + if (elements & 0x0C) { + elements |= 0x0C; + regs[1] = DINA28; + } + if (elements & 0x30) { + elements |= 0x30; + regs[2] = DINA30; + } + if (elements & 0x40) { + elements |= 0xC0; + regs[3] = DINA38; + } + + result = inv_set_mpu_memory(KEY_CFG_15, 4, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (elements & 0x01) + fifo_obj.data_config[CONFIG_TEMPERATURE] = 1 | INV_16_BIT; + else + fifo_obj.data_config[CONFIG_TEMPERATURE] = 0; + if (elements & 0xfe) + fifo_obj.data_config[CONFIG_RAW_DATA] = + (0x7f & (elements >> 1)) | INV_16_BIT; + else + fifo_obj.data_config[CONFIG_RAW_DATA] = 0; + + return inv_set_footer(); +#endif +} + +/** Sends raw external data to the FIFO. + * Should be called once after inv_dmp_open() and before inv_dmp_start(). + * @param[in] elements Which of the 3 elements to send. Use INV_ALL for all of them + * or INV_ELEMENT_1, INV_ELEMENT_2, INV_ELEMENT_3 or'd together + * for a subset. + * @param[in] accuracy INV_16_BIT to send data, 0 to stop sending this data. + * Sending and Stop sending are reference counted, so data actually + * stops when the reference reaches zero. + */ +inv_error_t inv_send_external_sensor_data(uint_fast16_t elements, + uint_fast16_t accuracy) +{ +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + int result; + unsigned char regs[6] = { DINAA0 + 3, DINAA0 + 3, + DINAA0 + 3, DINAA0 + 3, + DINAA0 + 3, DINAA0 + 3 }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (accuracy) + accuracy = INV_16_BIT; + + elements = inv_set_fifo_reference(elements, accuracy, REF_RAW_EXTERNAL, 6); + + if (elements) + fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = elements | INV_16_BIT; + else + fifo_obj.data_config[CONFIG_RAW_EXTERNAL] = 0; + + if (elements & INV_ELEMENT_1) { + regs[0] = DINBC2; + } + if (elements & INV_ELEMENT_2) { + regs[1] = DINACA; + } + if (elements & INV_ELEMENT_3) { + regs[2] = DINBC4; + } + if (elements & INV_ELEMENT_4) { + regs[3] = DINBC0; + } + if (elements & INV_ELEMENT_5) { + regs[4] = DINAC8; + } + if (elements & INV_ELEMENT_6) { + regs[5] = DINACC; + } + + result = inv_set_mpu_memory(KEY_CFG_EXTERNAL, sizeof(regs), regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); + +#else + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; // Feature not supported +#endif +} + +/** + * @brief Send the Quantized Acceleromter data into the FIFO. The data can be + * retrieved using inv_get_quantized_accel() or inv_get_unquantized_accel(). + * + * To be useful this should be set to fifo_rate + 1 if less than + * INV_MAX_NUM_ACCEL_SAMPLES, otherwise it doesn't work. + * + * @param elements + * the components bitmask. + * To send all compoents use INV_ALL. + * + * @param accuracy + * Use INV_32_BIT for 32-bit data or INV_16_BIT for + * 16-bit data. + * Set to zero to remove it from the FIFO. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_send_quantized_accel(uint_fast16_t elements, + uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + unsigned char regs[5] = { DINAF8 + 1, DINA20, DINA28, + DINA30, DINA38 + }; + unsigned char regs2[4] = { DINA20, DINA28, + DINA30, DINA38 + }; + inv_error_t result; + int_fast8_t kk; + int_fast8_t ii; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + elements = inv_set_fifo_reference(elements, accuracy, REF_QUANT_ACCEL, 8); + + if (elements) { + fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = (elements) | INV_32_BIT; + } else { + fifo_obj.data_config[CONFIG_DMP_QUANT_ACCEL] = 0; + } + + for (kk = 0; kk < INV_MAX_NUM_ACCEL_SAMPLES; ++kk) { + fifo_obj.decoded[REF_QUANT_ACCEL + kk] = 0; + for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { + fifo_obj.decoded_accel[kk][ii] = 0; + } + } + + for (kk = 0; kk < 4; ++kk) { + if ((elements & 1) == 0) + regs[kk + 1] = DINAA0 + 3; + elements >>= 1; + } + + result = inv_set_mpu_memory(KEY_CFG_TAP0, 5, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + for (kk = 0; kk < 4; ++kk) { + if ((elements & 1) == 0) + regs2[kk] = DINAA0 + 3; + elements >>= 1; + } + + result = inv_set_mpu_memory(KEY_CFG_TAP4, 4, regs2); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return inv_set_footer(); +} + +inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy) +{ + INVENSENSE_FUNC_START; + int_fast8_t kk; + unsigned char regs[3] = { DINA28, DINA30, DINA38 }; + inv_error_t result; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (accuracy) { + accuracy = INV_32_BIT; + } + + elements = inv_set_fifo_reference(elements, accuracy, REF_EIS, 3); + accuracy = inv_set_fifo_accuracy(elements, accuracy, CONFIG_EIS); + + fifo_obj.data_config[CONFIG_EIS] = elements | accuracy; + + for (kk = 0; kk < 3; ++kk) { + if ((elements & 1) == 0) + regs[kk] = DINAA0 + 7; + elements >>= 1; + } + + result = inv_set_mpu_memory(KEY_P_EIS_FIFO_XSHIFT, 3, regs); + + return inv_set_footer(); +} + +/** + * @brief Returns 3-element vector of accelerometer data in body frame. + * + * @param[out] data 3-element vector of accelerometer data in body frame. + * One gee = 2^16. + * @return 0 on success or an error code. + */ +inv_error_t inv_get_accel(long *data) +{ + int kk; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if ((!fifo_obj.data_config[CONFIG_ACCEL] && + (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) + || + (!(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) && + !inv_accel_present())) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { + data[kk] = fifo_obj.decoded[REF_ACCEL + kk]; + } + + return INV_SUCCESS; +} + +/** + * @brief Returns 4-element quaternion vector derived from 6-axis or + * 9-axis if 9-axis was implemented. 6-axis is gyros and accels. 9-axis is + * gyros, accel and compass. + * + * @param[out] data 4-element quaternion vector. One is scaled to 2^30. + * @return 0 on success or an error code. + */ +inv_error_t inv_get_quaternion(long *data) +{ + int kk; + + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_QUAT]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (kk = 0; kk < 4; ++kk) { + data[kk] = fifo_obj.decoded[REF_QUATERNION + kk]; + } + + return INV_SUCCESS; +} + +/** + * @brief Returns 4-element quaternion vector derived from 6 + * axis sensors (gyros and accels). + * @param[out] data + * 4-element quaternion vector. One is scaled to 2^30. + * @return 0 on success or an error code. + */ +inv_error_t inv_get_6axis_quaternion(long *data) +{ + int kk; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_QUAT]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (kk = 0; kk < 4; ++kk) { + data[kk] = fifo_obj.decoded[REF_QUATERNION_6AXIS + kk]; + } + + return INV_SUCCESS; +} + +inv_error_t inv_get_relative_quaternion(long *data) +{ + if (data == NULL) + return INV_ERROR; + data[0] = inv_obj.relative_quat[0]; + data[1] = inv_obj.relative_quat[1]; + data[2] = inv_obj.relative_quat[2]; + data[3] = inv_obj.relative_quat[3]; + return INV_SUCCESS; +} + +/** + * @brief Returns 3-element vector of gyro data in body frame. + * @param[out] data + * 3-element vector of gyro data in body frame + * with gravity removed. One degree per second = 2^16. + * @return 0 on success or an error code. + */ +inv_error_t inv_get_gyro(long *data) +{ + int kk; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (fifo_obj.data_config[CONFIG_GYROS]) { + for (kk = 0; kk < 3; ++kk) { + data[kk] = fifo_obj.decoded[REF_GYROS + kk]; + } + return INV_SUCCESS; + } else { + return INV_ERROR_FEATURE_NOT_ENABLED; + } +} + +/** + * @brief Get the 3-element gravity vector from the FIFO expressed + * in coordinates relative to the body frame. + * @param data + * 3-element vector of gravity in body frame. + * @return 0 on success or an error code. + */ +inv_error_t inv_get_gravity(long *data) +{ + long quat[4]; + int ii; + inv_error_t result; + + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_QUAT]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + if ((fifo_obj.cache & FIFO_CACHE_GRAVITY_BODY) == 0) { + fifo_obj.cache |= FIFO_CACHE_GRAVITY_BODY; + + // Compute it from Quaternion + result = inv_get_quaternion(quat); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + data[0] = + inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); + data[1] = + inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); + data[2] = + (inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], quat[0])) - + 1073741824L; + + for (ii = 0; ii < ACCEL_NUM_AXES; ii++) { + data[ii] >>= 14; + fifo_obj.gravity_cache[ii] = data[ii]; + } + } else { + data[0] = fifo_obj.gravity_cache[0]; + data[1] = fifo_obj.gravity_cache[1]; + data[2] = fifo_obj.gravity_cache[2]; + } + + return INV_SUCCESS; +} + +/** +* @brief Sets the filter coefficent used for computing the acceleration +* bias which is used to compute linear acceleration. +* @param[in] coef Fitler coefficient. 0. means no filter, a small number means +* a small cutoff frequency with an increasing number meaning +* an increasing cutoff frequency. +*/ +inv_error_t inv_set_linear_accel_filter_coef(float coef) +{ + fifo_obj.acc_filter_coef = coef; + return INV_SUCCESS; +} + +/** + * @brief Returns 3-element vector of accelerometer data in body frame + * with gravity removed. + * @param[out] data 3-element vector of accelerometer data in body frame + * with gravity removed. One g = 2^16. + * @return 0 on success or an error code. data unchanged on error. + */ +inv_error_t inv_get_linear_accel(long *data) +{ + int kk; + long grav[3]; + long la[3]; + inv_error_t result; + + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + result = inv_get_gravity(grav); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_get_accel(la); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if ((fifo_obj.cache & FIFO_CACHE_ACC_BIAS) == 0) { + fifo_obj.cache |= FIFO_CACHE_ACC_BIAS; + + for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { + long x; + x = la[kk] - grav[kk]; + fifo_obj.acc_bias_filt[kk] = (long)(x * fifo_obj.acc_filter_coef + + fifo_obj.acc_bias_filt[kk] * + (1.f - + fifo_obj.acc_filter_coef)); + data[kk] = x - fifo_obj.acc_bias_filt[kk]; + } + } else { + for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { + data[kk] = la[kk] - grav[kk] - fifo_obj.acc_bias_filt[kk]; + } + } + return INV_SUCCESS; +} + +/** + * @brief Returns 3-element vector of accelerometer data in world frame + * with gravity removed. + * @param[out] data 3-element vector of accelerometer data in world frame + * with gravity removed. One g = 2^16. + * @return 0 on success or an error code. + */ +inv_error_t inv_get_linear_accel_in_world(long *data) +{ + int kk; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + if (fifo_obj.data_config[CONFIG_ACCEL] && fifo_obj.data_config[CONFIG_QUAT]) { + long wtemp[4], qi[4], wtemp2[4]; + wtemp[0] = 0; + inv_get_linear_accel(&wtemp[1]); + inv_q_mult(&fifo_obj.decoded[REF_QUATERNION], wtemp, wtemp2); + inv_q_invert(&fifo_obj.decoded[REF_QUATERNION], qi); + inv_q_mult(wtemp2, qi, wtemp); + for (kk = 0; kk < 3; ++kk) { + data[kk] = wtemp[kk + 1]; + } + return INV_SUCCESS; + } else { + return INV_ERROR_FEATURE_NOT_ENABLED; + } +} + +/** + * @brief Returns 4-element vector of control data. + * @param[out] data 4-element vector of control data. + * @return 0 for succes or an error code. + */ +inv_error_t inv_get_cntrl_data(long *data) +{ + int kk; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_CONTROL_DATA]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (kk = 0; kk < 4; ++kk) { + data[kk] = fifo_obj.decoded[REF_CONTROL + kk]; + } + + return INV_SUCCESS; + +} + +/** + * @brief Returns 3-element vector of EIS shfit data + * @param[out] data 3-element vector of EIS shift data. + * @return 0 for succes or an error code. + */ +inv_error_t inv_get_eis(long *data) +{ + int kk; + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_EIS]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (kk = 0; kk < 3; ++kk) { + data[kk] = fifo_obj.decoded[REF_EIS + kk]; + } + + return INV_SUCCESS; + +} + +/** + * @brief Returns 3-element vector of accelerometer data in body frame. + * @param[out] data 3-element vector of accelerometer data in body frame in g's. + * @return 0 for success or an error code. + */ +inv_error_t inv_get_accel_float(float *data) +{ + long lData[3]; + int kk; + int result; + + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + result = inv_get_accel(lData); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + for (kk = 0; kk < ACCEL_NUM_AXES; ++kk) { + data[kk] = lData[kk] / 65536.0f; + } + + return INV_SUCCESS; +} + +/** + * @brief Returns 4-element quaternion vector. + * @param[out] data 4-element quaternion vector. + * @return 0 on success, an error code otherwise. + */ +inv_error_t inv_get_quaternion_float(float *data) +{ + int kk; + + if (data == NULL) + return INV_ERROR_INVALID_PARAMETER; + + if (!fifo_obj.data_config[CONFIG_QUAT]) + return INV_ERROR_FEATURE_NOT_ENABLED; + + for (kk = 0; kk < 4; ++kk) { + data[kk] = fifo_obj.decoded[REF_QUATERNION + kk] / 1073741824.0f; + } + + return INV_SUCCESS; +} + +/** + * @brief Command the MPU to put data in the FIFO at a particular rate. + * + * The DMP will add fifo entries every fifoRate + 1 MPU cycles. For + * example if the MPU is running at 200Hz the following values apply: + * + * + * + * + * + * + * + * + * + *
fifoRateDMP Sample RateFIFO update frequency
0200Hz200Hz
1200Hz100Hz
2200Hz50Hz
4200Hz40Hz
9200Hz20Hz
19200Hz10Hz
+ * + * Note: if the DMP is running, (state == INV_STATE_DMP_STARTED) + * then inv_run_state_callbacks() will be called to allow features + * that depend upon fundamental constants to be updated. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * and inv_dmp_start() + * must NOT have been called. + * + * @param fifoRate Divider value - 1. Output rate is + * (DMP Sample Rate) / (fifoRate + 1). + * + * @return INV_SUCCESS if successful, ML error code on any failure. + */ +inv_error_t inv_set_fifo_rate(unsigned short fifoRate) +{ + INVENSENSE_FUNC_START; + unsigned char regs[2]; + unsigned char state; + inv_error_t result = INV_SUCCESS; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + state = inv_get_state(); + if (state != INV_STATE_DMP_OPENED && state != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + fifo_obj.fifo_rate = fifoRate; + + if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) { + + regs[0] = (unsigned char)((fifoRate >> 8) & 0xff); + regs[1] = (unsigned char)(fifoRate & 0xff); + + result = inv_set_mpu_memory(KEY_D_0_22, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + fifo_obj.sample_step_size_ms = + (unsigned short)(((long)fifoRate + 1) * + (inv_mpu_get_sampling_period_us + (mldl_cfg)) / 1000L); + + if (state == INV_STATE_DMP_STARTED) + result = inv_run_state_callbacks(state); + } else if (mldl_cfg->requested_sensors & INV_THREE_AXIS_ACCEL) { + struct ext_slave_config config; + long data; + config.key = MPU_SLAVE_CONFIG_ODR_RESUME; + config.len = sizeof(long); + config.apply = (state == INV_STATE_DMP_STARTED); + config.data = &data; + data = (1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg)) / (fifoRate + 1); + + /* Ask for the same frequency */ + result = inv_mpu_config_accel(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_mpu_get_accel_config(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), &config); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + if(FIFO_DEBUG) + MPL_LOGI("Actual ODR: %ld Hz\n", data / 1000); + /* Record the actual frequency granted odr is in mHz */ + fifo_obj.sample_step_size_ms = (unsigned short)((1000L * 1000L) / data); + } + return result; +} + +/** + * @brief Retrieve the current FIFO update divider - 1. + * See inv_set_fifo_rate() for how this value is used. + * + * The fifo rate when there is no fifo is the equivilent divider when + * derived from the value set by SetSampleSteSizeMs() + * + * @return The value of the fifo rate divider or INV_INVALID_FIFO_RATE on error. + */ +unsigned short inv_get_fifo_rate(void) +{ + return fifo_obj.fifo_rate; +} + +/** + * @brief Returns the step size for quaternion type data. + * + * Typically the data rate for each FIFO packet. When the gryos are sleeping + * this value will return the last value set by SetSampleStepSizeMs() + * + * @return step size for quaternion type data + */ +int_fast16_t inv_get_sample_step_size_ms(void) +{ + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) + return (fifo_obj.fifo_rate + 1) * + (inv_mpu_get_sampling_period_us(mldl_cfg) / 1000); + else + return fifo_obj.sample_step_size_ms; +} + +/** + * @brief Returns the step size for quaternion type data. + * + * Typically the data rate for each FIFO packet. When the gryos are sleeping + * this value will return the last value set by SetSampleStepSizeMs() + * + * @return step size for quaternion type data + */ +int_fast16_t inv_get_sample_frequency(void) +{ + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (mldl_cfg->requested_sensors & INV_DMP_PROCESSOR) + return (inv_mpu_get_sampling_rate_hz(mldl_cfg) / + (fifo_obj.fifo_rate + 1)); + else + return (1000 / fifo_obj.sample_step_size_ms); +} + +/** + * @brief The gyro data magnitude squared : + * (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT. + * @return the computed magnitude squared output of the gyroscope. + */ +unsigned long inv_get_gyro_sum_of_sqr(void) +{ + unsigned long gmag = 0; + long temp; + int kk; + + for (kk = 0; kk < 3; ++kk) { + temp = fifo_obj.decoded[REF_GYROS + kk] >> + (16 - (GYRO_MAG_SQR_SHIFT / 2)); + gmag += temp * temp; + } + + return gmag; +} + +/** + * @brief The gyro data magnitude squared: + * (1 g)^2 = 2^16 = 2^ACC_MAG_SQR_SHIFT. + * @return the computed magnitude squared output of the accelerometer. + */ +unsigned long inv_accel_sum_of_sqr(void) +{ + unsigned long amag = 0; + long temp; + int kk; + long accel[3]; + inv_error_t result; + + result = inv_get_accel(accel); + if (INV_SUCCESS != result) { + return 0; + } + + for (kk = 0; kk < 3; ++kk) { + temp = accel[kk] >> (16 - (ACC_MAG_SQR_SHIFT / 2)); + amag += temp * temp; + } + return amag; +} + +/** + * @internal + */ +void inv_override_quaternion(float *q) +{ + int kk; + for (kk = 0; kk < 4; ++kk) { + fifo_obj.decoded[REF_QUATERNION + kk] = (long)(q[kk] * (1L << 30)); + } +} + +/** + * @internal + * @brief This registers a function to be called for each set of + * gyro/quaternion/rotation matrix/etc output. + * @param[in] func The callback function to register + * @param[in] priority The unique priority number of the callback. Lower numbers + * are called first. + * @return error code. + */ +inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + int kk, nn; + + result = inv_lock_mutex(fifo_rate_obj.mutex); + if (INV_SUCCESS != result) { + return result; + } + // Make sure we haven't registered this function already + // Or used the same priority + for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { + if ((fifo_rate_obj.fifo_process_cb[kk] == func) || + (fifo_rate_obj.priority[kk] == priority)) { + inv_unlock_mutex(fifo_rate_obj.mutex); + return INV_ERROR_INVALID_PARAMETER; + } + } + + // Make sure we have not filled up our number of allowable callbacks + if (fifo_rate_obj.num_cb <= MAX_HIGH_RATE_PROCESSES - 1) { + kk = 0; + if (fifo_rate_obj.num_cb != 0) { + // set kk to be where this new callback goes in the array + while ((kk < fifo_rate_obj.num_cb) && + (fifo_rate_obj.priority[kk] < priority)) { + kk++; + } + if (kk != fifo_rate_obj.num_cb) { + // We need to move the others + for (nn = fifo_rate_obj.num_cb; nn > kk; --nn) { + fifo_rate_obj.fifo_process_cb[nn] = + fifo_rate_obj.fifo_process_cb[nn - 1]; + fifo_rate_obj.priority[nn] = fifo_rate_obj.priority[nn - 1]; + } + } + } + // Add new callback + fifo_rate_obj.fifo_process_cb[kk] = func; + fifo_rate_obj.priority[kk] = priority; + fifo_rate_obj.num_cb++; + } else { + result = INV_ERROR_MEMORY_EXAUSTED; + } + + inv_unlock_mutex(fifo_rate_obj.mutex); + return result; +} + +/** + * @internal + * @brief This unregisters a function to be called for each set of + * gyro/quaternion/rotation matrix/etc output. + * @return error code. + */ +inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func) +{ + INVENSENSE_FUNC_START; + int kk, jj; + inv_error_t result; + + result = inv_lock_mutex(fifo_rate_obj.mutex); + if (INV_SUCCESS != result) { + return result; + } + // Make sure we haven't registered this function already + result = INV_ERROR_INVALID_PARAMETER; + for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { + if (fifo_rate_obj.fifo_process_cb[kk] == func) { + for (jj = kk + 1; jj < fifo_rate_obj.num_cb; ++jj) { + fifo_rate_obj.fifo_process_cb[jj - 1] = + fifo_rate_obj.fifo_process_cb[jj]; + fifo_rate_obj.priority[jj - 1] = + fifo_rate_obj.priority[jj]; + } + fifo_rate_obj.fifo_process_cb[fifo_rate_obj.num_cb - 1] = NULL; + fifo_rate_obj.priority[fifo_rate_obj.num_cb - 1] = 0; + fifo_rate_obj.num_cb--; + result = INV_SUCCESS; + break; + } + } + + inv_unlock_mutex(fifo_rate_obj.mutex); + return result; + +} +#ifdef UMPL +bool bFIFIDataAvailable = FALSE; +bool isUmplDataInFIFO(void) +{ + return bFIFIDataAvailable; +} +void setUmplDataInFIFOFlag(bool flag) +{ + bFIFIDataAvailable = flag; +} +#endif +inv_error_t inv_run_fifo_rate_processes(void) +{ + int kk; + inv_error_t result, result2; + + result = inv_lock_mutex(fifo_rate_obj.mutex); + if (INV_SUCCESS != result) { + MPL_LOGE("MLOsLockMutex returned %d\n", result); + return result; + } + // User callbacks take priority over the fifo_process_cb callback + if (fifo_obj.fifo_process_cb) + fifo_obj.fifo_process_cb(); + + for (kk = 0; kk < fifo_rate_obj.num_cb; ++kk) { + if (fifo_rate_obj.fifo_process_cb[kk]) { + result2 = fifo_rate_obj.fifo_process_cb[kk] (&inv_obj); + if (result == INV_SUCCESS) +#ifdef UMPL + setUmplDataInFIFOFlag(TRUE); +#endif + result = result2; + } + } + + inv_unlock_mutex(fifo_rate_obj.mutex); + return result; +} + +/*********************/ + /** \}*//* defgroup */ +/*********************/ diff --git a/60xx/mlsdk/mllite/mlFIFO.h b/60xx/mlsdk/mllite/mlFIFO.h new file mode 100644 index 0000000..2114eb3 --- /dev/null +++ b/60xx/mlsdk/mllite/mlFIFO.h @@ -0,0 +1,150 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef INVENSENSE_INV_FIFO_H__ +#define INVENSENSE_INV_FIFO_H__ + +#include "mltypes.h" +#include "mlinclude.h" +#include "ml.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mlFIFO_legacy.h" +#endif + +#ifdef __cplusplus +extern "C" { +#endif + + /**************************************************************************/ + /* Elements */ + /**************************************************************************/ + +#define INV_ELEMENT_1 (0x0001) +#define INV_ELEMENT_2 (0x0002) +#define INV_ELEMENT_3 (0x0004) +#define INV_ELEMENT_4 (0x0008) +#define INV_ELEMENT_5 (0x0010) +#define INV_ELEMENT_6 (0x0020) +#define INV_ELEMENT_7 (0x0040) +#define INV_ELEMENT_8 (0x0080) + +#define INV_ALL (0xFFFF) +#define INV_ELEMENT_MASK (0x00FF) + + /**************************************************************************/ + /* Accuracy */ + /**************************************************************************/ + +#define INV_16_BIT (0x0100) +#define INV_32_BIT (0x0200) +#define INV_ACCURACY_MASK (0x0300) + + /**************************************************************************/ + /* Accuracy */ + /**************************************************************************/ + +#define INV_GYRO_FROM_RAW (0x00) +#define INV_GYRO_FROM_QUATERNION (0x01) + + /**************************************************************************/ + /* High Rate Proceses */ + /**************************************************************************/ + +#define MAX_HIGH_RATE_PROCESSES 16 + + /**************************************************************************/ + /* Prototypes */ + /**************************************************************************/ + + inv_error_t inv_set_fifo_rate(unsigned short fifoRate); + unsigned short inv_get_fifo_rate(void); + int_fast16_t inv_get_sample_step_size_ms(void); + int_fast16_t inv_get_sample_frequency(void); + long inv_decode_temperature(short tempReg); + + // Register callbacks after a packet of FIFO data is processed + inv_error_t inv_register_fifo_rate_process(inv_obj_func func, int priority); + inv_error_t inv_unregister_fifo_rate_process(inv_obj_func func); + inv_error_t inv_run_fifo_rate_processes(void); + + // Setup FIFO for various output + inv_error_t inv_send_quaternion(uint_fast16_t accuracy); + inv_error_t inv_send_gyro(uint_fast16_t elements, uint_fast16_t accuracy); + inv_error_t inv_send_accel(uint_fast16_t elements, uint_fast16_t accuracy); + inv_error_t inv_send_linear_accel(uint_fast16_t elements, + uint_fast16_t accuracy); + inv_error_t inv_send_linear_accel_in_world(uint_fast16_t elements, + uint_fast16_t accuracy); + inv_error_t inv_send_cntrl_data(uint_fast16_t elements, + uint_fast16_t accuracy); + inv_error_t inv_send_sensor_data(uint_fast16_t elements, + uint_fast16_t accuracy); + inv_error_t inv_send_external_sensor_data(uint_fast16_t elements, + uint_fast16_t accuracy); + inv_error_t inv_send_gravity(uint_fast16_t elements, + uint_fast16_t accuracy); + inv_error_t inv_send_packet_number(uint_fast16_t accuracy); + inv_error_t inv_send_quantized_accel(uint_fast16_t elements, + uint_fast16_t accuracy); + inv_error_t inv_send_eis(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + inv_error_t inv_get_accel(long *data); + inv_error_t inv_get_quaternion(long *data); + inv_error_t inv_get_6axis_quaternion(long *data); + inv_error_t inv_get_relative_quaternion(long *data); + inv_error_t inv_get_gyro(long *data); + inv_error_t inv_set_linear_accel_filter_coef(float coef); + inv_error_t inv_get_linear_accel(long *data); + inv_error_t inv_get_linear_accel_in_world(long *data); + inv_error_t inv_get_gyro_and_accel_sensor(long *data); + inv_error_t inv_get_gyro_sensor(long *data); + inv_error_t inv_get_cntrl_data(long *data); + inv_error_t inv_get_temperature(long *data); + inv_error_t inv_get_gravity(long *data); + inv_error_t inv_get_unquantized_accel(long *data); + inv_error_t inv_get_quantized_accel(long *data); + inv_error_t inv_get_external_sensor_data(long *data, int size); + inv_error_t inv_get_eis(long *data); + + // Get Floating Point data from FIFO + inv_error_t inv_get_accel_float(float *data); + inv_error_t inv_get_quaternion_float(float *data); + + inv_error_t inv_process_fifo_packet(const unsigned char *dmpData); + inv_error_t inv_read_and_process_fifo(int_fast8_t numPackets, + int_fast8_t * processed); + + inv_error_t inv_set_fifo_processed_callback(void (*func) (void)); + + inv_error_t inv_init_fifo_param(void); + inv_error_t inv_close_fifo(void); + inv_error_t inv_set_gyro_data_source(uint_fast8_t source); + inv_error_t inv_decode_quantized_accel(void); + unsigned long inv_get_gyro_sum_of_sqr(void); + unsigned long inv_accel_sum_of_sqr(void); + void inv_override_quaternion(float *q); +#ifdef UMPL + bool isUmplDataInFIFO(void); + void setUmplDataInFIFOFlag(bool flag); +#endif + uint_fast16_t inv_get_fifo_packet_size(void); +#ifdef __cplusplus +} +#endif +#endif // INVENSENSE_INV_FIFO_H__ diff --git a/60xx/mlsdk/mllite/mlFIFOHW.c b/60xx/mlsdk/mllite/mlFIFOHW.c new file mode 100644 index 0000000..7a77e66 --- /dev/null +++ b/60xx/mlsdk/mllite/mlFIFOHW.c @@ -0,0 +1,328 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mlFIFOHW.c 5653 2011-06-16 21:06:55Z nroyer $ + * + *******************************************************************************/ + +/** + * @defgroup MLFIFO_HW + * @brief Motion Library - FIFO HW Driver. + * Provides facilities to interact with the FIFO. + * + * @{ + * @file mlFIFOHW.c + * @brief The Motion Library Fifo Hardware Layer. + * + */ + +#include + +#include "mpu.h" +#if defined CONFIG_MPU_SENSORS_MPU6050A2 +# include "mpu6050a2.h" +#elif defined CONFIG_MPU_SENSORS_MPU6050B1 +# include "mpu6050b1.h" +#elif defined CONFIG_MPU_SENSORS_MPU3050 +# include "mpu3050.h" +#else +#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx +#endif +#include "mlFIFOHW.h" +#include "ml.h" +#include "mldl.h" +#include "mldl_cfg.h" + +#include "mlsl.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-fifo" + +/* + Defines +*/ + +#define _fifoDebug(x) //{x} + +/* + Typedefs +*/ + +struct fifo_hw_obj { + short fifoCount; + inv_error_t fifoError; + unsigned char fifoOverflow; + unsigned char fifoResetOnOverflow; +}; + +/* + Global variables +*/ +const unsigned char gFifoFooter[FIFO_FOOTER_SIZE] = { 0xB2, 0x6A }; + +/* + Static variables +*/ +static struct fifo_hw_obj fifo_objHW; + +/* + Definitions +*/ + +/** + * @brief Initializes the internal FIFO data structure. + */ +void inv_init_fifo_hardare(void) +{ + memset(&fifo_objHW, 0, sizeof(fifo_objHW)); + fifo_objHW.fifoResetOnOverflow = TRUE; +} + +/** + * @internal + * @brief used to get the FIFO data. + * @param length + * Number of bytes to read from the FIFO. + * @param buffer + * the bytes of FIFO data. + * Note that this buffer must be large enough + * to store and additional trailing FIFO footer when + * expected. The callers must make sure enough space + * is allocated. + * @return number of valid bytes of data. +**/ +uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + uint_fast16_t inFifo; + uint_fast16_t toRead; + int_fast8_t kk; + + toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount; + /*---- make sure length is correct ----*/ + if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) { + fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER; + return 0; + } + + result = inv_get_fifo_length(&inFifo); + if (INV_SUCCESS != result) { + fifo_objHW.fifoError = result; + return 0; + } + // fifo_objHW.fifoCount is the footer size left in the buffer, or + // 0 if this is the first time reading the fifo since it was reset + if (inFifo < length + fifo_objHW.fifoCount) { + fifo_objHW.fifoError = INV_SUCCESS; + return 0; + } + // if a trailing fifo count is expected - start storing data 2 bytes before + result = + inv_read_fifo(fifo_objHW.fifoCount > + 0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead); + if (INV_SUCCESS != result) { + fifo_objHW.fifoError = result; + return 0; + } + // Make sure the fifo didn't overflow before or during the read + result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), + MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow); + if (INV_SUCCESS != result) { + fifo_objHW.fifoError = result; + return 0; + } + + if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) { + MPL_LOGV("Resetting Fifo : Overflow\n"); + inv_reset_fifo(); + fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW; + return 0; + } + + /* Check the Footer value to give us a chance at making sure data + * didn't get corrupted */ + for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) { + if (buffer[kk] != gFifoFooter[kk]) { + MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n", + buffer[0], buffer[1]); + _fifoDebug(char out[200]; + MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount); + sprintf(out, "0x"); + for (kk = 0; kk < (int)toRead; kk++) { + sprintf(out, "%s%02X", out, buffer[kk]);} + MPL_LOGW("%s\n", out);) + inv_reset_fifo(); + fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER; + return 0; + } + } + + if (fifo_objHW.fifoCount == 0) { + fifo_objHW.fifoCount = FIFO_FOOTER_SIZE; + } + + return length - FIFO_FOOTER_SIZE; +} + +/** + * @brief Used to query the status of the FIFO. + * @return INV_SUCCESS if the fifo is OK. An error code otherwise. +**/ +inv_error_t inv_get_fifo_status(void) +{ + inv_error_t fifoError = fifo_objHW.fifoError; + fifo_objHW.fifoError = 0; + return fifoError; +} + +/** + * @internal + * @brief Get the length from the fifo + * + * @param[out] len amount of data currently stored in the fifo. + * + * @return INV_SUCCESS or non-zero error code. +**/ +inv_error_t inv_get_fifo_length(uint_fast16_t * len) +{ + INVENSENSE_FUNC_START; + unsigned char fifoBuf[2]; + inv_error_t result; + + if (NULL == len) + return INV_ERROR_INVALID_PARAMETER; + + /*---- read the 2 'count' registers and + burst read the data from the FIFO ----*/ + result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), + MPUREG_FIFO_COUNTH, 2, fifoBuf); + if (INV_SUCCESS != result) { + MPL_LOGE("ReadBurst failed %d\n", result); + inv_reset_fifo(); + fifo_objHW.fifoError = INV_ERROR_FIFO_READ_COUNT; + *len = 0; + return result; + } + + *len = (uint_fast16_t) (fifoBuf[0] << 8); + *len += (uint_fast16_t) (fifoBuf[1]); + return result; +} + +/** + * @brief inv_get_fifo_count is used to get the number of bytes left in the FIFO. + * This function returns the stored value and does not access the hardware. + * See inv_get_fifo_length(). + * @return the number of bytes left in the FIFO +**/ +short inv_get_fifo_count(void) +{ + return fifo_objHW.fifoCount; +} + +/** + * @internal + * @brief Read data from the fifo + * + * @param[out] data Location to store the date read from the fifo + * @param[in] len Amount of data to read out of the fifo + * + * @return INV_SUCCESS or non-zero error code +**/ +inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + result = inv_serial_read_fifo(inv_get_serial_handle(), + inv_get_mpu_slave_addr(), + (unsigned short)len, data); + if (INV_SUCCESS != result) { + MPL_LOGE("inv_serial_readBurst failed %d\n", result); + inv_reset_fifo(); + fifo_objHW.fifoError = INV_ERROR_FIFO_READ_DATA; + return result; + } + return result; +} + +/** + * @brief Clears the FIFO status and its content. + * @note Halt the DMP writing into the FIFO for the time + * needed to reset the FIFO. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_reset_fifo(void) +{ + INVENSENSE_FUNC_START; + int len = FIFO_HW_SIZE; + unsigned char fifoBuf[2]; + unsigned char tries = 0; + unsigned char userCtrlReg; + inv_error_t result; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + fifo_objHW.fifoCount = 0; + if (mldl_cfg->gyro_is_suspended) + return INV_SUCCESS; + + result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), + MPUREG_USER_CTRL, 1, &userCtrlReg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + while (len != 0 && tries < 6) { + result = + inv_serial_single_write(inv_get_serial_handle(), + inv_get_mpu_slave_addr(), MPUREG_USER_CTRL, + ((userCtrlReg & (~BIT_FIFO_EN)) | + BIT_FIFO_RST)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = + inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), + MPUREG_FIFO_COUNTH, 2, fifoBuf); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + len = (unsigned short)fifoBuf[0] * 256 + (unsigned short)fifoBuf[1]; + tries++; + } + + result = + inv_serial_single_write(inv_get_serial_handle(), + inv_get_mpu_slave_addr(), MPUREG_USER_CTRL, + userCtrlReg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return INV_SUCCESS; +} + +/** + * @} +**/ diff --git a/60xx/mlsdk/mllite/mlFIFOHW.h b/60xx/mlsdk/mllite/mlFIFOHW.h new file mode 100644 index 0000000..6f70185 --- /dev/null +++ b/60xx/mlsdk/mllite/mlFIFOHW.h @@ -0,0 +1,48 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#ifndef INVENSENSE_INV_FIFO_HW_H__ +#define INVENSENSE_INV_FIFO_HW_H__ + +#include "mpu.h" +#include "mltypes.h" +#include "mlinclude.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mlFIFOHW_legacy.h" +#endif + +#ifdef __cplusplus +extern "C" { +#endif + + // This is the maximum amount of FIFO data we would read in one packet +#define MAX_FIFO_LENGTH (256) + // This is the hardware size of the FIFO +#define FIFO_FOOTER_SIZE (2) + + uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer); + inv_error_t inv_get_fifo_status(void); + inv_error_t inv_get_fifo_length(uint_fast16_t * len); + short inv_get_fifo_count(void); + inv_error_t inv_reset_fifo(void); + void inv_init_fifo_hardare(); + inv_error_t inv_read_fifo(unsigned char *data, uint_fast16_t len); + +#ifdef __cplusplus +} +#endif +#endif // INVENSENSE_INV_FIFO_HW_H__ diff --git a/60xx/mlsdk/mllite/mlMathFunc.c b/60xx/mlsdk/mllite/mlMathFunc.c new file mode 100644 index 0000000..0d8e7ec --- /dev/null +++ b/60xx/mlsdk/mllite/mlMathFunc.c @@ -0,0 +1,377 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#include "mlmath.h" +#include "mlMathFunc.h" +#include "mlinclude.h" + +/** + * Performs one iteration of the filter, generating a new y(0) + * 1 / N / N \\ + * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length + * a(0) \k=0 \ k=1 // + * + * The filters A and B should be (sizeof(long) * state->length). + * The state variables x and y should be (sizeof(long) * (state->length - 1)) + * + * The state variables x and y should be initialized prior to the first call + * + * @param state Contains the length of the filter, filter coefficients and + * filter state variables x and y. + * @param x New input into the filter. + */ +void inv_filter_long(struct filter_long *state, long x) +{ + const long *b = state->b; + const long *a = state->a; + long length = state->length; + long long tmp; + int ii; + + /* filter */ + tmp = (long long)x *(b[0]); + for (ii = 0; ii < length - 1; ii++) { + tmp += ((long long)state->x[ii] * (long long)(b[ii + 1])); + } + for (ii = 0; ii < length - 1; ii++) { + tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1])); + } + tmp /= (long long)a[0]; + + /* Delay */ + for (ii = length - 2; ii > 0; ii--) { + state->y[ii] = state->y[ii - 1]; + state->x[ii] = state->x[ii - 1]; + } + /* New values */ + state->y[0] = (long)tmp; + state->x[0] = x; +} + +/** Performs a multiply and shift by 29. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>29 +*/ +long inv_q29_mult(long a, long b) +{ + long long temp; + long result; + temp = (long long)a *b; + result = (long)(temp >> 29); + return result; +} + +/** Performs a multiply and shift by 30. These are good functions to write in assembly on + * with devices with small memory where you want to get rid of the long long which some + * assemblers don't handle well + * @param[in] a + * @param[in] b + * @return ((long long)a*b)>>30 +*/ +long inv_q30_mult(long a, long b) +{ + long long temp; + long result; + temp = (long long)a *b; + result = (long)(temp >> 30); + return result; +} + +void inv_q_mult(const long *q1, const long *q2, long *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = (long)(((long long)q1[0] * q2[0] - (long long)q1[1] * q2[1] - + (long long)q1[2] * q2[2] - + (long long)q1[3] * q2[3]) >> 30); + qProd[1] = + (int)(((long long)q1[0] * q2[1] + (long long)q1[1] * q2[0] + + (long long)q1[2] * q2[3] - (long long)q1[3] * q2[2]) >> 30); + qProd[2] = + (long)(((long long)q1[0] * q2[2] - (long long)q1[1] * q2[3] + + (long long)q1[2] * q2[0] + (long long)q1[3] * q2[1]) >> 30); + qProd[3] = + (long)(((long long)q1[0] * q2[3] + (long long)q1[1] * q2[2] - + (long long)q1[2] * q2[1] + (long long)q1[3] * q2[0]) >> 30); +} + +void inv_q_add(long *q1, long *q2, long *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_q_normalize(long *q) +{ + INVENSENSE_FUNC_START; + double normSF = 0; + int i; + for (i = 0; i < 4; i++) { + normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L; + } + if (normSF > 0) { + normSF = 1 / sqrt(normSF); + for (i = 0; i < 4; i++) { + q[i] = (int)((double)q[i] * normSF); + } + } else { + q[0] = 1073741824L; + q[1] = 0; + q[2] = 0; + q[3] = 0; + } +} + +void inv_q_invert(const long *q, long *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +void inv_q_multf(const float *q1, const float *q2, float *qProd) +{ + INVENSENSE_FUNC_START; + qProd[0] = (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]); + qProd[1] = (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]); + qProd[2] = (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]); + qProd[3] = (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]); +} + +void inv_q_addf(float *q1, float *q2, float *qSum) +{ + INVENSENSE_FUNC_START; + qSum[0] = q1[0] + q2[0]; + qSum[1] = q1[1] + q2[1]; + qSum[2] = q1[2] + q2[2]; + qSum[3] = q1[3] + q2[3]; +} + +void inv_q_normalizef(float *q) +{ + INVENSENSE_FUNC_START; + float normSF = 0; + float xHalf = 0; + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (normSF < 2) { + xHalf = 0.5f * normSF; + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + normSF = normSF * (1.5f - xHalf * normSF * normSF); + q[0] *= normSF; + q[1] *= normSF; + q[2] *= normSF; + q[3] *= normSF; + } else { + q[0] = 1.0; + q[1] = 0.0; + q[2] = 0.0; + q[3] = 0.0; + } + normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); +} + +/** Performs a length 4 vector normalization with a square root. +* @param[in,out] vector to normalize. Returns [1,0,0,0] is magnitude is zero. +*/ +void inv_q_norm4(float *q) +{ + float mag; + mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + if (mag) { + q[0] /= mag; + q[1] /= mag; + q[2] /= mag; + q[3] /= mag; + } else { + q[0] = 1.f; + q[1] = 0.f; + q[2] = 0.f; + q[3] = 0.f; + } +} + +void inv_q_invertf(const float *q, float *qInverted) +{ + INVENSENSE_FUNC_START; + qInverted[0] = q[0]; + qInverted[1] = -q[1]; + qInverted[2] = -q[2]; + qInverted[3] = -q[3]; +} + +/** + * Converts a quaternion to a rotation matrix. + * @param[in] quat 4-element quaternion in fixed point. One is 2^30. + * @param[out] rot Rotation matrix in fixed point. One is 2^30. The + * First 3 elements of the rotation matrix, represent + * the first row of the matrix. Rotation matrix multiplied + * by a 3 element column vector transform a vector from Body + * to World. + */ +void inv_quaternion_to_rotation(const long *quat, long *rot) +{ + rot[0] = + inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0], + quat[0]) - 1073741824L; + rot[1] = inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]); + rot[2] = inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]); + rot[3] = inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]); + rot[4] = + inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0], + quat[0]) - 1073741824L; + rot[5] = inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]); + rot[6] = inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]); + rot[7] = inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]); + rot[8] = + inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0], + quat[0]) - 1073741824L; +} + +/** Converts a 32-bit long to a big endian byte stream */ +unsigned char *inv_int32_to_big8(long x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 24) & 0xff); + big8[1] = (unsigned char)((x >> 16) & 0xff); + big8[2] = (unsigned char)((x >> 8) & 0xff); + big8[3] = (unsigned char)(x & 0xff); + return big8; +} + +/** Converts a big endian byte stream into a 32-bit long */ +long inv_big8_to_int32(const unsigned char *big8) +{ + long x; + x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8) | + ((long)big8[3]); + return x; +} + +/** Converts a 16-bit short to a big endian byte stream */ +unsigned char *inv_int16_to_big8(short x, unsigned char *big8) +{ + big8[0] = (unsigned char)((x >> 8) & 0xff); + big8[1] = (unsigned char)(x & 0xff); + return big8; +} + +void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 10 * k + l) = *(a + 10 * i + j); + } + } + *n = *n - 1; +} + +void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y) +{ + int k, l, i, j; + for (i = 0, k = 0; i < *n; i++, k++) { + for (j = 0, l = 0; j < *n; j++, l++) { + if (i == x) + i++; + if (j == y) + j++; + *(b + 10 * k + l) = *(a + 10 * i + j); + } + } + *n = *n - 1; +} + +float inv_matrix_det(float *p, int *n) +{ + float d[10][10], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 11) - *(p + 1) ** (p + 10)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_inc(p, &d[0][0], n, i, j); + sum = + sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_det(&d[0][0], + n); + } + + return (sum); +} + +double inv_matrix_detd(double *p, int *n) +{ + double d[10][10], sum = 0; + int i, j, m; + m = *n; + if (*n == 2) + return (*p ** (p + 11) - *(p + 1) ** (p + 10)); + for (i = 0, j = 0; j < m; j++) { + *n = m; + inv_matrix_det_incd(p, &d[0][0], n, i, j); + sum = + sum + *(p + 10 * i + j) * SIGNM(i + j) * inv_matrix_detd(&d[0][0], + n); + } + + return (sum); +} + +/** Wraps angle from (-M_PI,M_PI] + * @param[in] ang Angle in radians to wrap + * @return Wrapped angle from (-M_PI,M_PI] + */ +float inv_wrap_angle(float ang) +{ + if (ang > M_PI) + return ang - 2 * (float)M_PI; + else if (ang <= -(float)M_PI) + return ang + 2 * (float)M_PI; + else + return ang; +} + +/** Finds the minimum angle difference ang1-ang2 such that difference + * is between [-M_PI,M_PI] + * @param[in] ang1 + * @param[in] ang2 + * @return angle difference ang1-ang2 + */ +float inv_angle_diff(float ang1, float ang2) +{ + float d; + ang1 = inv_wrap_angle(ang1); + ang2 = inv_wrap_angle(ang2); + d = ang1 - ang2; + if (d > M_PI) + d -= 2 * (float)M_PI; + else if (d < -(float)M_PI) + d += 2 * (float)M_PI; + return d; +} diff --git a/60xx/mlsdk/mllite/mlMathFunc.h b/60xx/mlsdk/mllite/mlMathFunc.h new file mode 100644 index 0000000..70fa9f4 --- /dev/null +++ b/60xx/mlsdk/mllite/mlMathFunc.h @@ -0,0 +1,68 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#ifndef INVENSENSE_INV_MATH_FUNC_H__ +#define INVENSENSE_INV_MATH_FUNC_H__ +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mlMathFunc_legacy.h" +#endif + +#define NUM_ROTATION_MATRIX_ELEMENTS (9) +#define ROT_MATRIX_SCALE_LONG (1073741824) +#define ROT_MATRIX_SCALE_FLOAT (1073741824.0f) +#define ROT_MATRIX_LONG_TO_FLOAT( longval ) \ + ((float) ((longval) / ROT_MATRIX_SCALE_FLOAT )) +#define SIGNM(k)((int)(k)&1?-1:1) + +#ifdef __cplusplus +extern "C" { +#endif + struct filter_long { + int length; + const long *b; + const long *a; + long *x; + long *y; + }; + + void inv_filter_long(struct filter_long *state, long x); + long inv_q29_mult(long a, long b); + long inv_q30_mult(long a, long b); + void inv_q_mult(const long *q1, const long *q2, long *qProd); + void inv_q_add(long *q1, long *q2, long *qSum); + void inv_q_normalize(long *q); + void inv_q_invert(const long *q, long *qInverted); + void inv_q_multf(const float *q1, const float *q2, float *qProd); + void inv_q_addf(float *q1, float *q2, float *qSum); + void inv_q_normalizef(float *q); + void inv_q_norm4(float *q); + void inv_q_invertf(const float *q, float *qInverted); + void inv_quaternion_to_rotation(const long *quat, long *rot); + unsigned char *inv_int32_to_big8(long x, unsigned char *big8); + long inv_big8_to_int32(const unsigned char *big8); + unsigned char *inv_int16_to_big8(short x, unsigned char *big8); + float inv_matrix_det(float *p, int *n); + void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y); + double inv_matrix_detd(double *p, int *n); + void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y); + float inv_wrap_angle(float ang); + float inv_angle_diff(float ang1, float ang2); + +#ifdef __cplusplus +} +#endif +#endif // INVENSENSE_INV_MATH_FUNC_H__ diff --git a/60xx/mlsdk/mllite/mlSetGyroBias.c b/60xx/mlsdk/mllite/mlSetGyroBias.c new file mode 100644 index 0000000..bd14d2e --- /dev/null +++ b/60xx/mlsdk/mllite/mlSetGyroBias.c @@ -0,0 +1,198 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id:$ + * + *****************************************************************************/ + +#include "mlSetGyroBias.h" +#include "mlFIFO.h" +#include "ml.h" +#include +#include "mldl.h" +#include "mlMathFunc.h" + +typedef struct { + int needToSetBias; + short currentBias[3]; + int mode; + int motion; +} tSGB; + +tSGB sgb; + +/** Records a motion event that may cause a callback when the priority for this + * feature is met. + */ +void inv_set_motion_state(int motion) +{ + sgb.motion = motion; +} + +/** Converts from internal DMP gyro bias registers to external hardware gyro bias by +* applying scaling and transformation. +*/ +void inv_convert_bias(const unsigned char *regs, short *bias) +{ + long biasTmp2[3], biasTmp[3], biasPrev[3]; + int i; + int sf; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (mldl_cfg->gyro_sens_trim != 0) { + sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; + } else { + sf = 2000; + } + for (i = 0; i < 3; i++) { + biasTmp2[i] = inv_big8_to_int32(®s[i * 4]); + } + // Rotate bias vector by the transpose of the orientation matrix + for (i = 0; i < 3; ++i) { + biasTmp[i] = inv_q30_mult(biasTmp2[0], inv_obj.gyro_orient[i]) + + inv_q30_mult(biasTmp2[1], inv_obj.gyro_orient[i + 3]) + + inv_q30_mult(biasTmp2[2], inv_obj.gyro_orient[i + 6]); + } + + for (i = 0; i < GYRO_NUM_AXES; i++) { + biasTmp[i] = (long)(biasTmp[i] * 1.39882274201861f / sf); + biasPrev[i] = (long)mldl_cfg->offset[i]; + if (biasPrev[i] > 32767) + biasPrev[i] -= 65536L; + } + + for (i = 0; i < GYRO_NUM_AXES; i++) { + bias[i] = (short)(biasPrev[i] - biasTmp[i]); + } +} + +/** Records hardware biases in format as used by hardware gyro registers. +* Note, the hardware will add this value to the measured gyro data. +*/ +inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode) +{ + if (sgb.currentBias[0] != bias[0]) + sgb.needToSetBias = 1; + if (sgb.currentBias[1] != bias[1]) + sgb.needToSetBias = 1; + if (sgb.currentBias[2] != bias[2]) + sgb.needToSetBias = 1; + if (sgb.needToSetBias) { + memcpy(sgb.currentBias, bias, sizeof(sgb.currentBias)); + sgb.mode = mode; + } + return INV_SUCCESS; +} + +/** Records gyro biases +* @param[in] bias Bias where 1dps is 2^16. In chip frame. +*/ +inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode) +{ + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + int sf, i; + long biasTmp; + short offset[3]; + inv_error_t result; + + if (mldl_cfg->gyro_sens_trim != 0) { + sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; + } else { + sf = 2000; + } + + for (i = 0; i < GYRO_NUM_AXES; i++) { + biasTmp = -bias[i] / sf; + if (biasTmp < 0) + biasTmp += 65536L; + offset[i] = (short)biasTmp; + } + result = inv_set_gyro_bias_in_hw_unit(offset, mode); + return result; +} + +inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode) +{ + long biasL[3]; + inv_error_t result; + + biasL[0] = (long)(bias[0] * (1L << 16)); + biasL[1] = (long)(bias[1] * (1L << 16)); + biasL[2] = (long)(bias[2] * (1L << 16)); + result = inv_set_gyro_bias_in_dps(biasL, mode); + return result; +} + +inv_error_t MLSetGyroBiasCB(struct inv_obj_t * inv_obj) +{ + inv_error_t result = INV_SUCCESS; + if (sgb.needToSetBias) { + result = inv_set_offset(sgb.currentBias); + sgb.needToSetBias = 0; + } + + // Check if motion state has changed + if (sgb.motion == INV_MOTION) { + // We are moving + if (inv_obj->motion_state == INV_NO_MOTION) { + //Trigger motion callback + inv_obj->motion_state = INV_MOTION; + inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_MOTION; + if (inv_params_obj.motion_cb_func) { + inv_params_obj.motion_cb_func(INV_MOTION); + } + } + } else if (sgb.motion == INV_NO_MOTION){ + // We are not moving + if (inv_obj->motion_state == INV_MOTION) { + //Trigger no motion callback + inv_obj->motion_state = INV_NO_MOTION; + inv_obj->got_no_motion_bias = TRUE; + inv_obj->flags[INV_MOTION_STATE_CHANGE] = INV_NO_MOTION; + if (inv_params_obj.motion_cb_func) { + inv_params_obj.motion_cb_func(INV_NO_MOTION); + } + } + } + + return result; +} + +inv_error_t inv_enable_set_bias(void) +{ + inv_error_t result; + memset(&sgb, 0, sizeof(sgb)); + + sgb.motion = inv_obj.motion_state; + + result = + inv_register_fifo_rate_process(MLSetGyroBiasCB, + INV_PRIORITY_SET_GYRO_BIASES); + if (result == INV_ERROR_INVALID_PARAMETER) + result = INV_SUCCESS; /* We already registered this */ + return result; +} + +inv_error_t inv_disable_set_bias(void) +{ + inv_error_t result; + result = inv_unregister_fifo_rate_process(MLSetGyroBiasCB); + return INV_SUCCESS; // FIXME need to disable +} diff --git a/60xx/mlsdk/mllite/mlSetGyroBias.h b/60xx/mlsdk/mllite/mlSetGyroBias.h new file mode 100644 index 0000000..e56f18b --- /dev/null +++ b/60xx/mlsdk/mllite/mlSetGyroBias.h @@ -0,0 +1,49 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id$ + * + *****************************************************************************/ + +#ifndef INV_SET_GYRO_BIAS__H__ +#define INV_SET_GYRO_BIAS__H__ + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define INV_SGB_NO_MOTION 4 +#define INV_SGB_FAST_NO_MOTION 5 +#define INV_SGB_TEMP_COMP 6 + + inv_error_t inv_enable_set_bias(void); + inv_error_t inv_disable_set_bias(void); + inv_error_t inv_set_gyro_bias_in_hw_unit(const short *bias, int mode); + inv_error_t inv_set_gyro_bias_in_dps(const long *bias, int mode); + inv_error_t inv_set_gyro_bias_in_dps_float(const float *bias, int mode); + void inv_convert_bias(const unsigned char *regs, short *bias); + void inv_set_motion_state(int motion); + +#ifdef __cplusplus +} +#endif +#endif // INV_SET_GYRO_BIAS__H__ diff --git a/60xx/mlsdk/mllite/ml_mputest.c b/60xx/mlsdk/mllite/ml_mputest.c new file mode 100644 index 0000000..d7fc608 --- /dev/null +++ b/60xx/mlsdk/mllite/ml_mputest.c @@ -0,0 +1,184 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: ml_mputest.c 5641 2011-06-14 02:10:02Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup MPU_SELF_TEST + * @brief C wrapper to integrate the MPU Self Test wrapper in MPL. + * Provides ML name compliant naming and an additional API that + * automates the suspension of normal MPL operations, runs the test, + * and resume. + * + * @{ + * @file ml_mputest.c + * @brief C wrapper to integrate the MPU Self Test wrapper in MPL. + * The main logic of the test and APIs can be found in mputest.c + */ + +#include +#include +#include +#include +#include + +#include "ml_mputest.h" + +#include "mlmath.h" +#include "mlinclude.h" +#include "ml.h" +#include "mlstates.h" +#include "mldl.h" +#include "mldl_cfg.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Globals +*/ +extern struct mldl_cfg *mputestCfgPtr; +extern signed char g_z_sign; + +/* + Prototypes +*/ +extern inv_error_t inv_factory_calibrate(void *mlsl_handle, + uint_fast8_t provide_result); + +/** + * @brief An MPL wrapper for the main MPU Self Test API inv_factory_calibrate(). + * See inv_factory_calibrate() function for more details. + * + * @pre inv_dmp_open() must have been called to populate the mldl_cfg + * data structure. + * On Windows, SetupPlatform() is also a madatory pre condition to + * ensure the accelerometer is properly configured before running the + * test. + * + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @param provide_result + * If 1: + * perform and analyze the offset, drive frequency, and noise + * calculation and compare it against set thresholds. Report + * also the final result using a bit-mask like error code as + * described in the inv_test_gyro_xxxx() functions. + * When 0: + * skip the noise and drive frequency calculation and pass/fail + * assessment. It simply calculates the gyro and accel biases. + * NOTE: for MPU6050 devices, this parameter is currently + * ignored. + * + * @return INV_SUCCESS or first non-zero error code otherwise. + */ +inv_error_t inv_self_test_factory_calibrate(void *mlsl_handle, + unsigned char provide_result) +{ + INVENSENSE_FUNC_START; + inv_error_t firstError = INV_SUCCESS; + inv_error_t result; + unsigned char initState = inv_get_state(); + + if (initState < INV_STATE_DMP_OPENED) { + MPL_LOGE("Self Test cannot run before inv_dmp_open()\n"); + return INV_ERROR_SM_IMPROPER_STATE; + } + + /* obtain a pointer to the 'struct mldl_cfg' data structure. */ + mputestCfgPtr = inv_get_dl_config(); + + if(initState == INV_STATE_DMP_STARTED) { + result = inv_dmp_stop(); + ERROR_CHECK_FIRST(firstError, result); + } + + result = inv_factory_calibrate(mlsl_handle, provide_result); + ERROR_CHECK_FIRST(firstError, result); + + if(initState == INV_STATE_DMP_STARTED) { + result = inv_dmp_start(); + ERROR_CHECK_FIRST(firstError, result); + } + + return firstError; +} + +/** + * @brief Runs the MPU test at MPL runtime. + * If the DMP is operating, stops the DMP temporarely, + * runs the MPU Self Test, and re-starts the DMP. + * + * @return INV_SUCCESS or a non-zero error code otherwise. + */ +inv_error_t inv_self_test_run(void) +{ +#ifdef CONFIG_MPU_SENSORS_MPU3050 + return inv_self_test_factory_calibrate(inv_get_serial_handle(), TRUE); +#else + return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE); +#endif +} + +/** + * @brief Runs the MPU test for bias correction only at MPL runtime. + * If the DMP is operating, stops the DMP temporarely, + * runs the bias calculation routines, and re-starts the DMP. + * + * @return INV_SUCCESS or a non-zero error code otherwise. + */ +inv_error_t inv_self_test_bias_only(void) +{ + return inv_self_test_factory_calibrate(inv_get_serial_handle(), FALSE); +} + +/** + * @brief Set the orientation of the acceleroemter Z axis as it will be + * expected when running the MPU Self Test. + * Specifies the orientation of the accelerometer Z axis : Z axis + * pointing upwards or downwards. + * @param z_sign + * The sign of the accelerometer Z axis; valid values are +1 and + * -1 for +Z and -Z respectively. Any other value will cause the + * setting to be ignored and an error code to be returned. + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign) +{ + if (z_sign != +1 && z_sign != -1) { + return INV_ERROR_INVALID_PARAMETER; + } + g_z_sign = z_sign; + return INV_SUCCESS; +} + + +#ifdef __cplusplus +} +#endif + +/** + * @} + */ + diff --git a/60xx/mlsdk/mllite/ml_mputest.h b/60xx/mlsdk/mllite/ml_mputest.h new file mode 100644 index 0000000..705d3cc --- /dev/null +++ b/60xx/mlsdk/mllite/ml_mputest.h @@ -0,0 +1,49 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: ml_mputest.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +#ifndef _INV_MPUTEST_H_ +#define _INV_MPUTEST_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" + +/* user APIs */ +inv_error_t inv_self_test_factory_calibrate( + void *mlsl_handle, unsigned char provide_result); +inv_error_t inv_self_test_set_accel_z_orient(signed char z_sign); +inv_error_t inv_self_test_run(void); +inv_error_t inv_self_test_bias_only(void); + +/* other functions */ +#define inv_set_self_test_parameters inv_set_test_parameters + +#ifdef __cplusplus +} +#endif + +#endif /* _INV_MPUTEST_H_ */ + diff --git a/60xx/mlsdk/mllite/ml_stored_data.c b/60xx/mlsdk/mllite/ml_stored_data.c new file mode 100644 index 0000000..9107fe2 --- /dev/null +++ b/60xx/mlsdk/mllite/ml_stored_data.c @@ -0,0 +1,1586 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: ml_stored_data.c 5641 2011-06-14 02:10:02Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML_STORED_DATA + * + * @{ + * @file ml_stored_data.c + * @brief functions for reading and writing stored data sets. + * Typically, these functions process stored calibration data. + */ + +#include "ml_stored_data.h" +#include "ml.h" +#include "mlFIFO.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "compass.h" +#include "dmpKey.h" +#include "dmpDefault.h" +#include "mlstates.h" +#include "checksum.h" +#include "mlsupervisor.h" + +#include "mlsl.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-storeload" + +/* + Typedefs +*/ +typedef inv_error_t(*tMLLoadFunc) (unsigned char *, unsigned short); + +/* + Globals +*/ +extern struct inv_obj_t inv_obj; + +/* + Debugging Definitions + set LOADCAL_DEBUG and/or STORECAL_DEBUG to 1 print the fields + being read or stored in human-readable format. + When set to 0, the compiler will optimize and remove the printouts + from the code. +*/ +#define LOADCAL_DEBUG 0 +#define STORECAL_DEBUG 0 + +#define LOADCAL_LOG(...) \ + if(LOADCAL_DEBUG) \ + MPL_LOGI("LOADCAL: " __VA_ARGS__) +#define STORECAL_LOG(...) \ + if(STORECAL_DEBUG) \ + MPL_LOGI("STORECAL: " __VA_ARGS__) + +/** + * @brief Duplicate of the inv_temp_comp_find_bin function in the libmpl + * advanced algorithms library. To remove cross-dependency, for now, + * we reimplement the same function here. + * @param temp + * the temperature (1 count == 1 degree C). + */ +int FindTempBin(float temp) +{ + int bin = (int)((temp - MIN_TEMP) / TEMP_PER_BIN); + if (bin < 0) + bin = 0; + if (bin > BINS - 1) + bin = BINS - 1; + return bin; +} + +/** + * @brief Returns the length of the MPL internal calibration data. + * Should be called before allocating the memory required to store + * this data to a file. + * This function returns the total size required to store the cal + * data including the header (4 bytes) and the checksum (2 bytes). + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have NOT + * been called. + * + * @return the length of the internal calibrated data format. + */ +unsigned int inv_get_cal_length(void) +{ + INVENSENSE_FUNC_START; + unsigned int length; + length = INV_CAL_HDR_LEN + // header + BINS * PTS_PER_BIN * 4 * 4 + BINS * 4 * 2 + // gyro cal + INV_CAL_ACCEL_LEN + // accel cal + INV_CAL_COMPASS_LEN + // compass cal + INV_CAL_CHK_LEN; // checksum + return length; +} + +/** + * @brief Loads a type 0 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 18 bytes (header and checksum included). + * Calibration format type 0 is currently NOT used and + * is substituted by type 5 : inv_load_cal_V5(). + * + * @note This calibration data format is obsoleted and no longer supported + * by the rest of the MPL + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V0(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 18; + long newGyroData[3] = { 0, 0, 0 }; + float newTemp; + int bin; + + LOADCAL_LOG("Entering inv_load_cal_V0\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); + return INV_ERROR_FILE_READ; + } + + newTemp = (float)inv_decode_temperature( + (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; + LOADCAL_LOG("newTemp = %f\n", newTemp); + + newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); + if (newGyroData[0] > 32767L) + newGyroData[0] -= 65536L; + LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); + newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); + if (newGyroData[1] > 32767L) + newGyroData[1] -= 65536L; + LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); + newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); + if (newGyroData[2] > 32767L) + newGyroData[2] -= 65536L; + LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); + + bin = FindTempBin(newTemp); + LOADCAL_LOG("bin = %d", bin); + + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; + LOADCAL_LOG("temp_data[%d][%d] = %f", + bin, inv_obj.temp_ptrs[bin], + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = + ((float)newGyroData[0]) / 65536.f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = + ((float)newGyroData[0]) / 65536.f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = + ((float)newGyroData[0]) / 65536.f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + + inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); + + if (inv_obj.temp_ptrs[bin] == 0) + inv_obj.temp_valid_data[bin] = TRUE; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + bin, inv_obj.temp_valid_data[bin]); + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V0\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 1 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes, + * - accel biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 24 bytes (header and checksum included). + * Calibration format type 1 is currently NOT used and + * is substituted by type 5 : inv_load_cal_V5(). + * + * @note In order to successfully work, the gyro bias must be stored + * expressed in 250 dps full scale (131.072 sensitivity). Other full + * scale range will produce unpredictable results in the gyro biases. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V1(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 24; + long newGyroData[3] = {0, 0, 0}; + long accelBias[3] = {0, 0, 0}; + float gyroBias[3] = {0, 0, 0}; + float newTemp = 0; + int bin; + + LOADCAL_LOG("Entering inv_load_cal_V1\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 1 must be %d bytes long\n", expLen); + return INV_ERROR_FILE_READ; + } + + newTemp = (float)inv_decode_temperature( + (unsigned short)calData[6] * 256 + calData[7]) / 65536.f; + LOADCAL_LOG("newTemp = %f\n", newTemp); + + newGyroData[0] = ((long)calData[8]) * 256 + ((long)calData[9]); + if (newGyroData[0] > 32767L) + newGyroData[0] -= 65536L; + LOADCAL_LOG("newGyroData[0] = %ld\n", newGyroData[0]); + newGyroData[1] = ((long)calData[10]) * 256 + ((long)calData[11]); + if (newGyroData[1] > 32767L) + newGyroData[1] -= 65536L; + LOADCAL_LOG("newGyroData[1] = %ld\n", newGyroData[1]); + newGyroData[2] = ((long)calData[12]) * 256 + ((long)calData[13]); + if (newGyroData[2] > 32767L) + newGyroData[2] -= 65536L; + LOADCAL_LOG("newGyroData[2] = %ld\n", newGyroData[2]); + + bin = FindTempBin(newTemp); + LOADCAL_LOG("bin = %d\n", bin); + + gyroBias[0] = ((float)newGyroData[0]) / 131.072f; + gyroBias[1] = ((float)newGyroData[1]) / 131.072f; + gyroBias[2] = ((float)newGyroData[2]) / 131.072f; + LOADCAL_LOG("gyroBias[0] = %f\n", gyroBias[0]); + LOADCAL_LOG("gyroBias[1] = %f\n", gyroBias[1]); + LOADCAL_LOG("gyroBias[2] = %f\n", gyroBias[2]); + + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; + LOADCAL_LOG("temp_data[%d][%d] = %f", + bin, inv_obj.temp_ptrs[bin], + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]]); + + inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); + + if (inv_obj.temp_ptrs[bin] == 0) + inv_obj.temp_valid_data[bin] = TRUE; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + bin, inv_obj.temp_valid_data[bin]); + + /* load accel biases and apply immediately */ + accelBias[0] = ((long)calData[14]) * 256 + ((long)calData[15]); + if (accelBias[0] > 32767) + accelBias[0] -= 65536L; + accelBias[0] = (long)((long long)accelBias[0] * 65536L * + inv_obj.accel_sens / 1073741824L); + LOADCAL_LOG("accelBias[0] = %ld\n", accelBias[0]); + accelBias[1] = ((long)calData[16]) * 256 + ((long)calData[17]); + if (accelBias[1] > 32767) + accelBias[1] -= 65536L; + accelBias[1] = (long)((long long)accelBias[1] * 65536L * + inv_obj.accel_sens / 1073741824L); + LOADCAL_LOG("accelBias[1] = %ld\n", accelBias[1]); + accelBias[2] = ((long)calData[18]) * 256 + ((int)calData[19]); + if (accelBias[2] > 32767) + accelBias[2] -= 65536L; + accelBias[2] = (long)((long long)accelBias[2] * 65536L * + inv_obj.accel_sens / 1073741824L); + LOADCAL_LOG("accelBias[2] = %ld\n", accelBias[2]); + if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); + return inv_set_array(INV_ACCEL_BIAS, accelBias); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V1\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 2 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * This calibration data is produced internally by the MPL and its + * size is 2222 bytes (header and checksum included). + * Calibration format type 2 is currently NOT used and + * is substituted by type 4 : inv_load_cal_V4(). + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V2(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 2222; + long accel_bias[3]; + int ptr = INV_CAL_HDR_LEN; + + int i, j; + long long tmp; + + LOADCAL_LOG("Entering inv_load_cal_V2\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 2 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + for (i = 0; i < BINS; i++) { + inv_obj.temp_ptrs[i] = 0; + inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_ptrs[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); + } + for (i = 0; i < BINS; i++) { + inv_obj.temp_valid_data[i] = 0; + inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_valid_data[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + i, inv_obj.temp_valid_data[i]); + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + /* read the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + accel_bias[i] = (int32_t) t; + LOADCAL_LOG("accel_bias[%d] = %ld\n", i, accel_bias[i]); + } + + if (inv_set_array(INV_ACCEL_BIAS, accel_bias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accel_bias)); + return inv_set_array(INV_ACCEL_BIAS, accel_bias); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V2\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 3 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * - compass biases for X, Y, Z axes and bias tracking algorithm + * mock-up. + * This calibration data is produced internally by the MPL and its + * size is 2429 bytes (header and checksum included). + * Calibration format type 3 is currently NOT used and + * is substituted by type 4 : inv_load_cal_V4(). + + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V3(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + union doubleToLongLong { + double db; + unsigned long long ll; + } dToLL; + + const short expLen = 2429; + long bias[3]; + int i, j; + int ptr = INV_CAL_HDR_LEN; + long long tmp; + + LOADCAL_LOG("Entering inv_load_cal_V3\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 3 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + for (i = 0; i < BINS; i++) { + inv_obj.temp_ptrs[i] = 0; + inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_ptrs[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); + } + for (i = 0; i < BINS; i++) { + inv_obj.temp_valid_data[i] = 0; + inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_valid_data[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + i, inv_obj.temp_valid_data[i]); + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + /* read the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + bias[i] = (int32_t) t; + LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); + } + if (inv_set_array(INV_ACCEL_BIAS, bias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); + return inv_set_array(INV_ACCEL_BIAS, bias); + } + + /* read the compass biases */ + inv_obj.got_compass_bias = (int)calData[ptr++]; + inv_obj.got_init_compass_bias = (int)calData[ptr++]; + inv_obj.compass_state = (int)calData[ptr++]; + + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias_error[i] = (int32_t) t; + LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, + inv_obj.compass_bias_error[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.init_compass_bias[i] = (int32_t) t; + LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, + inv_obj.init_compass_bias[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias[i] = (int32_t) t; + LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); + } + for (i = 0; i < 18; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_peaks[i] = (int32_t) t; + LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); + } + for (i = 0; i < 3; i++) { + dToLL.ll = 0; + dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_v[i] = dToLL.db; + LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); + } + for (i = 0; i < 9; i++) { + dToLL.ll = 0; + dToLL.ll += 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_ptr[i] = dToLL.db; + LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, + inv_obj.compass_bias_ptr[i]); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V3\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 4 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature compensation : temperature data points, + * - temperature compensation : gyro biases data points for X, Y, + * and Z axes. + * - accel biases for X, Y, Z axes. + * - compass biases for X, Y, Z axes, compass scale, and bias + * tracking algorithm mock-up. + * This calibration data is produced internally by the MPL and its + * size is 2777 bytes (header and checksum included). + * Calibration format type 4 is currently used and + * substitutes type 2 (inv_load_cal_V2()) and 3 (inv_load_cal_V3()). + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V4(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + union doubleToLongLong { + double db; + unsigned long long ll; + } dToLL; + + const unsigned int expLen = 2782; + long bias[3]; + int ptr = INV_CAL_HDR_LEN; + int i, j; + long long tmp; + inv_error_t result; + + LOADCAL_LOG("Entering inv_load_cal_V4\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 4 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + for (i = 0; i < BINS; i++) { + inv_obj.temp_ptrs[i] = 0; + inv_obj.temp_ptrs[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_ptrs[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_ptrs[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", i, inv_obj.temp_ptrs[i]); + } + for (i = 0; i < BINS; i++) { + inv_obj.temp_valid_data[i] = 0; + inv_obj.temp_valid_data[i] += 16777216L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 65536L * ((long)calData[ptr++]); + inv_obj.temp_valid_data[i] += 256 * ((int)calData[ptr++]); + inv_obj.temp_valid_data[i] += (int)calData[ptr++]; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + i, inv_obj.temp_valid_data[i]); + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.x_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.y_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = 0; + tmp += 16777216LL * (long long)calData[ptr++]; + tmp += 65536LL * (long long)calData[ptr++]; + tmp += 256LL * (long long)calData[ptr++]; + tmp += (long long)calData[ptr++]; + if (tmp > 2147483648LL) { + tmp -= 4294967296LL; + } + inv_obj.z_gyro_temp_data[i][j] = ((float)tmp) / 65536.0f; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + /* read the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + bias[i] = (int32_t) t; + LOADCAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); + } + if (inv_set_array(INV_ACCEL_BIAS, bias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, bias)); + return inv_set_array(INV_ACCEL_BIAS, bias); + } + + /* read the compass biases */ + inv_reset_compass_calibration(); + + inv_obj.got_compass_bias = (int)calData[ptr++]; + LOADCAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); + inv_obj.got_init_compass_bias = (int)calData[ptr++]; + LOADCAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); + inv_obj.compass_state = (int)calData[ptr++]; + LOADCAL_LOG("compass_state = %ld\n", inv_obj.compass_state); + + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias_error[i] = (int32_t) t; + LOADCAL_LOG("compass_bias_error[%d] = %ld\n", i, + inv_obj.compass_bias_error[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.init_compass_bias[i] = (int32_t) t; + LOADCAL_LOG("init_compass_bias[%d] = %ld\n", i, + inv_obj.init_compass_bias[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_bias[i] = (int32_t) t; + LOADCAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); + } + for (i = 0; i < 18; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_peaks[i] = (int32_t) t; + LOADCAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); + } + for (i = 0; i < 3; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_v[i] = dToLL.db; + LOADCAL_LOG("compass_bias_v[%d] = %lf\n", i, inv_obj.compass_bias_v[i]); + } + for (i = 0; i < 9; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_bias_ptr[i] = dToLL.db; + LOADCAL_LOG("compass_bias_ptr[%d] = %lf\n", i, + inv_obj.compass_bias_ptr[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = 0; + t += 16777216UL * ((uint32_t) calData[ptr++]); + t += 65536UL * ((uint32_t) calData[ptr++]); + t += 256u * ((uint32_t) calData[ptr++]); + t += (uint32_t) calData[ptr++]; + inv_obj.compass_scale[i] = (int32_t) t; + LOADCAL_LOG("compass_scale[%d] = %d\n", i, (int32_t) t); + } + for (i = 0; i < 6; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_prev_xty[i] = dToLL.db; + LOADCAL_LOG("compass_prev_xty[%d] = %f\n", i, dToLL.db); + } + for (i = 0; i < 36; i++) { + dToLL.ll = 72057594037927936ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 281474976710656ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 1099511627776ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 4294967296LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 16777216ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 65536ULL * ((unsigned long long)calData[ptr++]); + dToLL.ll += 256LL * ((unsigned long long)calData[ptr++]); + dToLL.ll += (unsigned long long)calData[ptr++]; + + inv_obj.compass_prev_m[i] = dToLL.db; + LOADCAL_LOG("compass_prev_m[%d] = %f\n", i, dToLL.db); + } + + /* Load the compass offset flag and values */ + inv_obj.flags[INV_COMPASS_OFFSET_VALID] = calData[ptr++]; + inv_obj.compass_offsets[0] = calData[ptr++]; + inv_obj.compass_offsets[1] = calData[ptr++]; + inv_obj.compass_offsets[2] = calData[ptr++]; + + inv_obj.compass_accuracy = calData[ptr++]; + /* push the compass offset values to the device */ + result = inv_set_compass_offset(); + + if (result == INV_SUCCESS) { + if (inv_compass_check_range() != INV_SUCCESS) { + MPL_LOGI("range check fail"); + inv_reset_compass_calibration(); + inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; + inv_set_compass_offset(); + } + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V4\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a type 5 set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * This calibrations data format stores values for (in order of + * appearance) : + * - temperature, + * - gyro biases for X, Y, Z axes, + * - accel biases for X, Y, Z axes. + * This calibration data would normally be produced by the MPU Self + * Test and its size is 36 bytes (header and checksum included). + * Calibration format type 5 is produced by the MPU Self Test and + * substitutes the type 1 : inv_load_cal_V1(). + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * @param len + * the length of the calibration + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal_V5(unsigned char *calData, unsigned short len) +{ + INVENSENSE_FUNC_START; + const short expLen = 36; + long accelBias[3] = { 0, 0, 0 }; + float gyroBias[3] = { 0, 0, 0 }; + + int ptr = INV_CAL_HDR_LEN; + unsigned short temp; + float newTemp; + int bin; + int i; + + LOADCAL_LOG("Entering inv_load_cal_V5\n"); + + if (len != expLen) { + MPL_LOGE("Calibration data type 5 must be %d bytes long (got %d)\n", + expLen, len); + return INV_ERROR_FILE_READ; + } + + /* load the temperature */ + temp = (unsigned short)calData[ptr++] * (1L << 8); + temp += calData[ptr++]; + newTemp = (float)inv_decode_temperature(temp) / 65536.f; + LOADCAL_LOG("newTemp = %f\n", newTemp); + + /* load the gyro biases (represented in 2 ^ 16 == 1 dps) */ + for (i = 0; i < 3; i++) { + int32_t tmp = 0; + tmp += (int32_t) calData[ptr++] * (1L << 24); + tmp += (int32_t) calData[ptr++] * (1L << 16); + tmp += (int32_t) calData[ptr++] * (1L << 8); + tmp += (int32_t) calData[ptr++]; + gyroBias[i] = (float)tmp / 65536.0f; + LOADCAL_LOG("gyroBias[%d] = %f\n", i, gyroBias[i]); + } + /* find the temperature bin */ + bin = FindTempBin(newTemp); + + /* populate the temp comp data structure */ + inv_obj.temp_data[bin][inv_obj.temp_ptrs[bin]] = newTemp; + LOADCAL_LOG("temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], newTemp); + + inv_obj.x_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[0]; + LOADCAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], gyroBias[0]); + inv_obj.y_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[1]; + LOADCAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], gyroBias[1]); + inv_obj.z_gyro_temp_data[bin][inv_obj.temp_ptrs[bin]] = gyroBias[2]; + LOADCAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + bin, inv_obj.temp_ptrs[bin], gyroBias[2]); + inv_obj.temp_ptrs[bin] = (inv_obj.temp_ptrs[bin] + 1) % PTS_PER_BIN; + LOADCAL_LOG("temp_ptrs[%d] = %d\n", bin, inv_obj.temp_ptrs[bin]); + + if (inv_obj.temp_ptrs[bin] == 0) + inv_obj.temp_valid_data[bin] = TRUE; + LOADCAL_LOG("temp_valid_data[%d] = %ld\n", + bin, inv_obj.temp_valid_data[bin]); + + /* load accel biases (represented in 2 ^ 16 == 1 gee) + and apply immediately */ + for (i = 0; i < 3; i++) { + int32_t tmp = 0; + tmp += (int32_t) calData[ptr++] * (1L << 24); + tmp += (int32_t) calData[ptr++] * (1L << 16); + tmp += (int32_t) calData[ptr++] * (1L << 8); + tmp += (int32_t) calData[ptr++]; + accelBias[i] = (long)tmp; + LOADCAL_LOG("accelBias[%d] = %ld\n", i, accelBias[i]); + } + if (inv_set_array(INV_ACCEL_BIAS, accelBias)) { + LOG_RESULT_LOCATION(inv_set_array(INV_ACCEL_BIAS, accelBias)); + return inv_set_array(INV_ACCEL_BIAS, accelBias); + } + + inv_obj.got_no_motion_bias = TRUE; + LOADCAL_LOG("got_no_motion_bias = 1\n"); + inv_obj.cal_loaded_flag = TRUE; + LOADCAL_LOG("cal_loaded_flag = 1\n"); + + LOADCAL_LOG("Exiting inv_load_cal_V5\n"); + return INV_SUCCESS; +} + +/** + * @brief Loads a set of calibration data. + * It parses a binary data set containing calibration data. + * The binary data set is intended to be loaded from a file. + * + * @pre inv_dmp_open() + * @ifnot MPL_MF + * or inv_open_low_power_pedometer() + * or inv_eis_open_dmp() + * @endif + * must have been called. + * + * @param calData + * A pointer to an array of bytes to be parsed. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_cal(unsigned char *calData) +{ + INVENSENSE_FUNC_START; + int calType = 0; + int len = 0; + int ptr; + uint32_t chk = 0; + uint32_t cmp_chk = 0; + + tMLLoadFunc loaders[] = { + inv_load_cal_V0, + inv_load_cal_V1, + inv_load_cal_V2, + inv_load_cal_V3, + inv_load_cal_V4, + inv_load_cal_V5 + }; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + /* read the header (type and len) + len is the total record length including header and checksum */ + len = 0; + len += 16777216L * ((int)calData[0]); + len += 65536L * ((int)calData[1]); + len += 256 * ((int)calData[2]); + len += (int)calData[3]; + + calType = ((int)calData[4]) * 256 + ((int)calData[5]); + if (calType > 5) { + MPL_LOGE("Unsupported calibration file format %d. " + "Valid types 0..5\n", calType); + return INV_ERROR_INVALID_PARAMETER; + } + + /* check the checksum */ + chk = 0; + ptr = len - INV_CAL_CHK_LEN; + + chk += 16777216L * ((uint32_t) calData[ptr++]); + chk += 65536L * ((uint32_t) calData[ptr++]); + chk += 256 * ((uint32_t) calData[ptr++]); + chk += (uint32_t) calData[ptr++]; + + cmp_chk = inv_checksum(calData + INV_CAL_HDR_LEN, + len - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); + + if (chk != cmp_chk) { + return INV_ERROR_CALIBRATION_CHECKSUM; + } + + /* call the proper method to read in the data */ + return loaders[calType] (calData, len); +} + +/** + * @brief Stores a set of calibration data. + * It generates a binary data set containing calibration data. + * The binary data set is intended to be stored into a file. + * + * @pre inv_dmp_open() + * + * @param calData + * A pointer to an array of bytes to be stored. + * @param length + * The amount of bytes available in the array. + * + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_store_cal(unsigned char *calData, int length) +{ + INVENSENSE_FUNC_START; + int ptr = 0; + int i = 0; + int j = 0; + long long tmp; + uint32_t chk; + long bias[3]; + //unsigned char state; + union doubleToLongLong { + double db; + unsigned long long ll; + } dToLL; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + STORECAL_LOG("Entering inv_store_cal\n"); + + // length + calData[0] = (unsigned char)((length >> 24) & 0xff); + calData[1] = (unsigned char)((length >> 16) & 0xff); + calData[2] = (unsigned char)((length >> 8) & 0xff); + calData[3] = (unsigned char)(length & 0xff); + STORECAL_LOG("calLen = %d\n", length); + + // calibration data format type + calData[4] = 0; + calData[5] = 4; + STORECAL_LOG("calType = %d\n", (int)calData[4] * 256 + calData[5]); + + // data + ptr = 6; + for (i = 0; i < BINS; i++) { + tmp = (int)inv_obj.temp_ptrs[i]; + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("temp_ptrs[%d] = %lld\n", i, tmp); + } + + for (i = 0; i < BINS; i++) { + tmp = (int)inv_obj.temp_valid_data[i]; + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("temp_valid_data[%d] = %lld\n", i, tmp); + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("temp_data[%d][%d] = %f\n", + i, j, inv_obj.temp_data[i][j]); + } + } + + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.x_gyro_temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("x_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.x_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.y_gyro_temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("y_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.y_gyro_temp_data[i][j]); + } + } + for (i = 0; i < BINS; i++) { + for (j = 0; j < PTS_PER_BIN; j++) { + tmp = (long long)(inv_obj.z_gyro_temp_data[i][j] * 65536.0f); + if (tmp < 0) { + tmp += 4294967296LL; + } + calData[ptr++] = (unsigned char)((tmp >> 24) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 16) & 0xff); + calData[ptr++] = (unsigned char)((tmp >> 8) & 0xff); + calData[ptr++] = (unsigned char)(tmp & 0xff); + STORECAL_LOG("z_gyro_temp_data[%d][%d] = %f\n", + i, j, inv_obj.z_gyro_temp_data[i][j]); + } + } + + inv_get_array(INV_ACCEL_BIAS, bias); + + /* write the accel biases */ + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) bias[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("accel_bias[%d] = %ld\n", i, bias[i]); + } + + /* write the compass calibration state */ + calData[ptr++] = (unsigned char)(inv_obj.got_compass_bias); + STORECAL_LOG("got_compass_bias = %ld\n", inv_obj.got_compass_bias); + calData[ptr++] = (unsigned char)(inv_obj.got_init_compass_bias); + STORECAL_LOG("got_init_compass_bias = %d\n", inv_obj.got_init_compass_bias); + if (inv_obj.compass_state == SF_UNCALIBRATED) { + calData[ptr++] = SF_UNCALIBRATED; + } else { + calData[ptr++] = SF_STARTUP_SETTLE; + } + STORECAL_LOG("compass_state = %ld\n", inv_obj.compass_state); + + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.compass_bias_error[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_bias_error[%d] = %ld\n", + i, inv_obj.compass_bias_error[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.init_compass_bias[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("init_compass_bias[%d] = %ld\n", i, + inv_obj.init_compass_bias[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.compass_bias[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_bias[%d] = %ld\n", i, inv_obj.compass_bias[i]); + } + for (i = 0; i < 18; i++) { + uint32_t t = (uint32_t) inv_obj.compass_peaks[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_peaks[%d] = %d\n", i, inv_obj.compass_peaks[i]); + } + for (i = 0; i < 3; i++) { + dToLL.db = inv_obj.compass_bias_v[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_bias_v[%d] = %lf\n", i, + inv_obj.compass_bias_v[i]); + } + for (i = 0; i < 9; i++) { + dToLL.db = inv_obj.compass_bias_ptr[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_bias_ptr[%d] = %lf\n", i, + inv_obj.compass_bias_ptr[i]); + } + for (i = 0; i < 3; i++) { + uint32_t t = (uint32_t) inv_obj.compass_scale[i]; + calData[ptr++] = (unsigned char)((t >> 24) & 0xff); + calData[ptr++] = (unsigned char)((t >> 16) & 0xff); + calData[ptr++] = (unsigned char)((t >> 8) & 0xff); + calData[ptr++] = (unsigned char)(t & 0xff); + STORECAL_LOG("compass_scale[%d] = %ld\n", i, inv_obj.compass_scale[i]); + } + for (i = 0; i < 6; i++) { + dToLL.db = inv_obj.compass_prev_xty[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_prev_xty[%d] = %lf\n", i, + inv_obj.compass_prev_xty[i]); + } + for (i = 0; i < 36; i++) { + dToLL.db = inv_obj.compass_prev_m[i]; + calData[ptr++] = (unsigned char)((dToLL.ll >> 56) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 48) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 40) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 32) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 24) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 16) & 0xff); + calData[ptr++] = (unsigned char)((dToLL.ll >> 8) & 0xff); + calData[ptr++] = (unsigned char)(dToLL.ll & 0xff); + STORECAL_LOG("compass_prev_m[%d] = %lf\n", i, + inv_obj.compass_prev_m[i]); + } + + /* store the compass offsets and validity flag */ + calData[ptr++] = (unsigned char)inv_obj.flags[INV_COMPASS_OFFSET_VALID]; + calData[ptr++] = (unsigned char)inv_obj.compass_offsets[0]; + calData[ptr++] = (unsigned char)inv_obj.compass_offsets[1]; + calData[ptr++] = (unsigned char)inv_obj.compass_offsets[2]; + + /* store the compass accuracy */ + calData[ptr++] = (unsigned char)(inv_obj.compass_accuracy); + + /* add a checksum */ + chk = inv_checksum(calData + INV_CAL_HDR_LEN, + length - (INV_CAL_HDR_LEN + INV_CAL_CHK_LEN)); + calData[ptr++] = (unsigned char)((chk >> 24) & 0xff); + calData[ptr++] = (unsigned char)((chk >> 16) & 0xff); + calData[ptr++] = (unsigned char)((chk >> 8) & 0xff); + calData[ptr++] = (unsigned char)(chk & 0xff); + + STORECAL_LOG("Exiting inv_store_cal\n"); + return INV_SUCCESS; +} + +/** + * @brief Load a calibration file. + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have NOT + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_load_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + unsigned int length; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + result = inv_serial_get_cal_length(&length); + if (result == INV_ERROR_FILE_OPEN) { + MPL_LOGI("Calibration data not loaded\n"); + return INV_SUCCESS; + } + if (result || length <= 0) { + MPL_LOGE("Could not get file calibration length - " + "error %d - aborting\n", result); + return result; + } + calData = (unsigned char *)inv_malloc(length); + if (!calData) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", length); + return INV_ERROR_MEMORY_EXAUSTED; + } + result = inv_serial_read_cal(calData, length); + if (result) { + MPL_LOGE("Could not read the calibration data from file - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + result = inv_load_cal(calData); + if (result) { + MPL_LOGE("Could not load the calibration data - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + + + +free_mem_n_exit: + inv_free(calData); + return INV_SUCCESS; +} + +/** + * @brief Store runtime calibration data to a file + * + * @pre Must be in INV_STATE_DMP_OPENED state. + * inv_dmp_open() or inv_dmp_stop() must have been called. + * inv_dmp_start() and inv_dmp_close() must have NOT + * been called. + * + * @return 0 or error code. + */ +inv_error_t inv_store_calibration(void) +{ + unsigned char *calData; + inv_error_t result; + unsigned int length; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + length = inv_get_cal_length(); + calData = (unsigned char *)inv_malloc(length); + if (!calData) { + MPL_LOGE("Could not allocate buffer of %d bytes - " + "aborting\n", length); + return INV_ERROR_MEMORY_EXAUSTED; + } + result = inv_store_cal(calData, length); + if (result) { + MPL_LOGE("Could not store calibrated data on file - " + "error %d - aborting\n", result); + goto free_mem_n_exit; + + } + result = inv_serial_write_cal(calData, length); + if (result) { + MPL_LOGE("Could not write calibration data - " "error %d\n", result); + goto free_mem_n_exit; + + } + + + +free_mem_n_exit: + inv_free(calData); + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/ml_stored_data.h b/60xx/mlsdk/mllite/ml_stored_data.h new file mode 100644 index 0000000..74c2b7c --- /dev/null +++ b/60xx/mlsdk/mllite/ml_stored_data.h @@ -0,0 +1,62 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/******************************************************************************* + * + * $Id:$ + * + ******************************************************************************/ + +#ifndef INV_STORED_DATA_H +#define INV_STORED_DATA_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Includes. +*/ + +#include "mltypes.h" + +/* + Defines +*/ +#define INV_CAL_ACCEL_LEN (12) +#define INV_CAL_COMPASS_LEN (555 + 5) +#define INV_CAL_HDR_LEN (6) +#define INV_CAL_CHK_LEN (4) + +/* + APIs +*/ + inv_error_t inv_load_calibration(void); + inv_error_t inv_store_calibration(void); + +/* + Other prototypes +*/ + inv_error_t inv_load_cal(unsigned char *calData); + inv_error_t inv_store_cal(unsigned char *calData, int length); + unsigned int inv_get_cal_length(void); + +#ifdef __cplusplus +} +#endif +#endif /* INV_STORED_DATA_H */ diff --git a/60xx/mlsdk/mllite/mlarray.c b/60xx/mlsdk/mllite/mlarray.c new file mode 100644 index 0000000..6ae85e0 --- /dev/null +++ b/60xx/mlsdk/mllite/mlarray.c @@ -0,0 +1,2524 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/****************************************************************************** + * + * $Id: mlarray.c 5085 2011-04-08 22:25:14Z phickey $ + * + *****************************************************************************/ + +/** + * @defgroup ML + * @{ + * @file mlarray.c + * @brief APIs to read different data sets from FIFO. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ +#include "ml.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "mlMathFunc.h" +#include "mlmath.h" +#include "mlstates.h" +#include "mlFIFO.h" +#include "mlsupervisor.h" +#include "mldl.h" +#include "dmpKey.h" +#include "compass.h" + +/** + * @brief inv_get_gyro is used to get the most recent gyroscope measurement. + * The argument array elements are ordered X,Y,Z. + * The values are scaled at 1 dps = 2^16 LSBs. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + + /* inv_get_gyro implemented in mlFIFO.c */ + +/** + * @brief inv_get_accel is used to get the most recent + * accelerometer measurement. + * The argument array elements are ordered X,Y,Z. + * The values are scaled in units of g (gravity), + * where 1 g = 2^16 LSBs. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + /* inv_get_accel implemented in mlFIFO.c */ + +/** + * @brief inv_get_temperature is used to get the most recent + * temperature measurement. + * The argument array should only have one element. + * The value is in units of deg C (degrees Celsius), where + * 2^16 LSBs = 1 deg C. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to the data to be passed back to the user. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + /* inv_get_temperature implemented in mlFIFO.c */ + +/** + * @brief inv_get_rot_mat is used to get the rotation matrix + * representation of the current sensor fusion solution. + * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, + * R33, representing the matrix: + *
R11 R12 R13
+ *
R21 R22 R23
+ *
R31 R32 R33
+ * Values are scaled, where 1.0 = 2^30 LSBs. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_rot_mat(long *data) +{ + inv_error_t result = INV_SUCCESS; + long qdata[4]; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + inv_get_quaternion(qdata); + inv_quaternion_to_rotation(qdata, data); + + return result; +} + +/** + * @brief inv_get_quaternion is used to get the quaternion representation + * of the current sensor fusion solution. + * The values are scaled where 1.0 = 2^30 LSBs. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 4 cells long . + * + * @return INV_SUCCESS if the command is successful; an ML error code otherwise. + */ + /* inv_get_quaternion implemented in mlFIFO.c */ + +/** + * @brief inv_get_linear_accel is used to get an estimate of linear + * acceleration, based on the most recent accelerometer measurement + * and sensor fusion solution. + * The values are scaled where 1 g (gravity) = 2^16 LSBs. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + /* inv_get_linear_accel implemented in mlFIFO.c */ + +/** + * @brief inv_get_linear_accel_in_world is used to get an estimate of + * linear acceleration, in the world frame, based on the most + * recent accelerometer measurement and sensor fusion solution. + * The values are scaled where 1 g (gravity) = 2^16 LSBs. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + /* inv_get_linear_accel_in_world implemented in mlFIFO.c */ + +/** + * @brief inv_get_gravity is used to get an estimate of the body frame + * gravity vector, based on the most recent sensor fusion solution. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + /* inv_get_gravity implemented in mlFIFO.c */ + +/** + * @internal + * @brief inv_get_angular_velocity is used to get an estimate of the body + * frame angular velocity, which is computed from the current and + * previous sensor fusion solutions. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_angular_velocity(long *data) +{ + + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + /* not implemented. old (invalid) implementation: + if ( inv_get_state() < INV_STATE_DMP_OPENED ) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + data[0] = inv_obj.ang_v_body[0]; + data[1] = inv_obj.ang_v_body[1]; + data[2] = inv_obj.ang_v_body[2]; + + return result; + */ +} + +/** + * @brief inv_get_euler_angles is used to get the Euler angle representation + * of the current sensor fusion solution. + * Euler angles may follow various conventions. This function is equivelant + * to inv_get_euler_angles_x, refer to inv_get_euler_angles_x for more + * information. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_euler_angles(long *data) +{ + return inv_get_euler_angles_x(data); +} + +/** + * @brief inv_get_euler_angles_x is used to get the Euler angle representation + * of the current sensor fusion solution. + * Euler angles are returned according to the X convention. + * This is typically the convention used for mobile devices where the X + * axis is the width of the screen, Y axis is the height, and Z the + * depth. In this case roll is defined as the rotation around the X + * axis of the device. + * + * + * + * + * + *
Element Euler angleRotation about
0 Roll X axis
1 Pitch Y axis
2 Yaw Z axis
+ * + * Values are scaled where 1.0 = 2^16. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_euler_angles_x(long *data) +{ + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_rot_mat_float(rotMatrix); + tmp = rotMatrix[6]; + if (tmp > 1.0f) { + tmp = 1.0f; + } + if (tmp < -1.0f) { + tmp = -1.0f; + } + data[0] = + (long)((float) + (atan2f(rotMatrix[7], rotMatrix[8]) * 57.29577951308) * + 65536L); + data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L); + data[2] = + (long)((float) + (atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308) * + 65536L); + return result; +} + +/** + * @brief inv_get_euler_angles_y is used to get the Euler angle representation + * of the current sensor fusion solution. + * Euler angles are returned according to the Y convention. + * This convention is typically used in augmented reality applications, + * where roll is defined as the rotation around the axis along the + * height of the screen of a mobile device, namely the Y axis. + * + * + * + * + * + *
Element Euler angleRotation about
0 Roll Y axis
1 Pitch X axis
2 Yaw Z axis
+ * + * Values are scaled where 1.0 = 2^16. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_euler_angles_y(long *data) +{ + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_rot_mat_float(rotMatrix); + tmp = rotMatrix[7]; + if (tmp > 1.0f) { + tmp = 1.0f; + } + if (tmp < -1.0f) { + tmp = -1.0f; + } + data[0] = + (long)((float) + (atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308f) * + 65536L); + data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L); + data[2] = + (long)((float) + (atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308f) * + 65536L); + return result; +} + +/** @brief inv_get_euler_angles_z is used to get the Euler angle representation + * of the current sensor fusion solution. + * This convention is mostly used in application involving the use + * of a camera, typically placed on the back of a mobile device, that + * is along the Z axis. In this convention roll is defined as the + * rotation around the Z axis. + * Euler angles are returned according to the Y convention. + * + * + * + * + * + *
Element Euler angleRotation about
0 Roll Z axis
1 Pitch X axis
2 Yaw Y axis
+ * + * Values are scaled where 1.0 = 2^16. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ + +inv_error_t inv_get_euler_angles_z(long *data) +{ + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_rot_mat_float(rotMatrix); + tmp = rotMatrix[8]; + if (tmp > 1.0f) { + tmp = 1.0f; + } + if (tmp < -1.0f) { + tmp = -1.0f; + } + data[0] = + (long)((float) + (atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308) * + 65536L); + data[1] = (long)((float)((double)asin(tmp) * 57.29577951308) * 65536L); + data[2] = + (long)((float) + (atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308) * + 65536L); + return result; +} + +/** + * @brief inv_get_gyro_temp_slope is used to get is used to get the temperature + * compensation algorithm's estimate of the gyroscope bias temperature + * coefficient. + * The argument array elements are ordered X,Y,Z. + * Values are in units of dps per deg C (degrees per second per degree + * Celcius). Values are scaled so that 1 dps per deg C = 2^16 LSBs. + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_gyro_temp_slope(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) { + data[0] = (long)(inv_obj.x_gyro_coef[1] * 65536.0f); + data[1] = (long)(inv_obj.y_gyro_coef[1] * 65536.0f); + data[2] = (long)(inv_obj.z_gyro_coef[1] * 65536.0f); + } else { + data[0] = inv_obj.temp_slope[0]; + data[1] = inv_obj.temp_slope[1]; + data[2] = inv_obj.temp_slope[2]; + } + return result; +} + +/** + * @brief inv_get_gyro_bias is used to get the gyroscope biases. + * The argument array elements are ordered X,Y,Z. + * The values are scaled such that 1 dps = 2^16 LSBs. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_gyro_bias(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + data[0] = inv_obj.gyro_bias[0]; + data[1] = inv_obj.gyro_bias[1]; + data[2] = inv_obj.gyro_bias[2]; + + return result; +} + +/** + * @brief inv_get_accel_bias is used to get the accelerometer baises. + * The argument array elements are ordered X,Y,Z. + * The values are scaled such that 1 g (gravity) = 2^16 LSBs. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_accel_bias(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + data[0] = inv_obj.accel_bias[0]; + data[1] = inv_obj.accel_bias[1]; + data[2] = inv_obj.accel_bias[2]; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_bias is used to get Magnetometer Bias + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_bias(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + data[0] = + inv_obj.compass_bias[0] + + (long)((long long)inv_obj.init_compass_bias[0] * inv_obj.compass_sens / + 16384); + data[1] = + inv_obj.compass_bias[1] + + (long)((long long)inv_obj.init_compass_bias[1] * inv_obj.compass_sens / + 16384); + data[2] = + inv_obj.compass_bias[2] + + (long)((long long)inv_obj.init_compass_bias[2] * inv_obj.compass_sens / + 16384); + + return result; +} + +/** + * @brief inv_get_gyro_and_accel_sensor is used to get the most recent set of all sensor data. + * The argument array elements are ordered gyroscope X,Y, and Z, + * accelerometer X, Y, and Z, and magnetometer X,Y, and Z. + * \if UMPL The magnetometer elements are not populated in UMPL. \endif + * The gyroscope and accelerometer data is not scaled or offset, it is + * copied directly from the sensor registers. + * In the case of accelerometers with 8-bit output resolution, the data + * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g + * full scale range + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + /* inv_get_gyro_and_accel_sensor implemented in mlFIFO.c */ + +/** + * @cond MPL + * @brief inv_get_mag_raw_data is used to get Raw magnetometer data. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_raw_data(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.compass_sensor_data[0]; + data[1] = inv_obj.compass_sensor_data[1]; + data[2] = inv_obj.compass_sensor_data[2]; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_magnetometer is used to get magnetometer data. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_magnetometer(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]; + data[1] = inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]; + data[2] = inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_pressure is used to get Pressure data. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to data to be passed back to the user. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_pressure(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.pressure; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_heading is used to get heading from Rotation Matrix. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to data to be passed back to the user. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ + +inv_error_t inv_get_heading(long *data) +{ + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_get_rot_mat_float(rotMatrix); + if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) { + tmp = + (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 - + 90.0f); + } else { + tmp = + (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 + + 90.0f); + } + if (tmp < 0) { + tmp += 360.0f; + } + data[0] = (long)((360 - tmp) * 65536.0f); + + return result; +} + +/** + * @brief inv_get_gyro_cal_matrix is used to get the gyroscope + * calibration matrix. The gyroscope calibration matrix defines the relationship + * between the gyroscope sensor axes and the sensor fusion solution axes. + * Calibration matrix data members will have a value of 1, 0, or -1. + * The matrix has members + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_gyro_cal_matrix(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.gyro_cal[0]; + data[1] = inv_obj.gyro_cal[1]; + data[2] = inv_obj.gyro_cal[2]; + data[3] = inv_obj.gyro_cal[3]; + data[4] = inv_obj.gyro_cal[4]; + data[5] = inv_obj.gyro_cal[5]; + data[6] = inv_obj.gyro_cal[6]; + data[7] = inv_obj.gyro_cal[7]; + data[8] = inv_obj.gyro_cal[8]; + + return result; +} + +/** + * @brief inv_get_accel_cal_matrix is used to get the accelerometer + * calibration matrix. + * Calibration matrix data members will have a value of 1, 0, or -1. + * The matrix has members + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_accel_cal_matrix(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.accel_cal[0]; + data[1] = inv_obj.accel_cal[1]; + data[2] = inv_obj.accel_cal[2]; + data[3] = inv_obj.accel_cal[3]; + data[4] = inv_obj.accel_cal[4]; + data[5] = inv_obj.accel_cal[5]; + data[6] = inv_obj.accel_cal[6]; + data[7] = inv_obj.accel_cal[7]; + data[8] = inv_obj.accel_cal[8]; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_cal_matrix is used to get magnetometer calibration matrix. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long at least. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_cal_matrix(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.compass_cal[0]; + data[1] = inv_obj.compass_cal[1]; + data[2] = inv_obj.compass_cal[2]; + data[3] = inv_obj.compass_cal[3]; + data[4] = inv_obj.compass_cal[4]; + data[5] = inv_obj.compass_cal[5]; + data[6] = inv_obj.compass_cal[6]; + data[7] = inv_obj.compass_cal[7]; + data[8] = inv_obj.compass_cal[8]; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_bias_error is used to get magnetometer Bias error. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long at least. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_bias_error(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + if (inv_obj.large_field == 0) { + data[0] = inv_obj.compass_bias_error[0]; + data[1] = inv_obj.compass_bias_error[1]; + data[2] = inv_obj.compass_bias_error[2]; + } else { + data[0] = P_INIT; + data[1] = P_INIT; + data[2] = P_INIT; + } + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_scale is used to get magnetometer scale. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long at least. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_scale(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.compass_scale[0]; + data[1] = inv_obj.compass_scale[1]; + data[2] = inv_obj.compass_scale[2]; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_local_field is used to get local magnetic field data. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long at least. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +inv_error_t inv_get_local_field(long *data) +{ + inv_error_t result = INV_SUCCESS; + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = inv_obj.local_field[0]; + data[1] = inv_obj.local_field[1]; + data[2] = inv_obj.local_field[2]; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_relative_quaternion is used to get relative quaternion. + * + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 4 cells long at least. + * + * @return Zero if the command is successful; an ML error code otherwise. + * @endcond + */ +/* inv_get_relative_quaternion implemented in mlFIFO.c */ + +/** + * @brief inv_get_gyro_float is used to get the most recent gyroscope measurement. + * The argument array elements are ordered X,Y,Z. + * The values are in units of dps (degrees per second). + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_gyro_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + long ldata[3]; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_get_gyro(ldata); + data[0] = (float)ldata[0] / 65536.0f; + data[1] = (float)ldata[1] / 65536.0f; + data[2] = (float)ldata[2] / 65536.0f; + + return result; +} + +/** + * @internal + * @brief inv_get_angular_velocity_float is used to get an array of three data points representing the angular + * velocity as derived from both gyroscopes and accelerometers. + * This requires that ML_SENSOR_FUSION be enabled, to fuse data from + * the gyroscope and accelerometer device, appropriately scaled and + * oriented according to the respective mounting matrices. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_angular_velocity_float(float *data) +{ + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + /* not implemented. old (invalid) implementation: + return inv_get_gyro_float(data); + */ +} + +/** + * @brief inv_get_accel_float is used to get the most recent accelerometer measurement. + * The argument array elements are ordered X,Y,Z. + * The values are in units of g (gravity). + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ + /* inv_get_accel_float implemented in mlFIFO.c */ + +/** + * @brief inv_get_temperature_float is used to get the most recent + * temperature measurement. + * The argument array should only have one element. + * The value is in units of deg C (degrees Celsius). + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to data to be passed back to the user. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_temperature_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + long ldata[1]; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_temperature(ldata); + data[0] = (float)ldata[0] / 65536.0f; + + return result; +} + +/** + * @brief inv_get_rot_mat_float is used to get an array of nine data points representing the rotation + * matrix generated from all available sensors. + * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, + * R33, representing the matrix: + *
R11 R12 R13
+ *
R21 R22 R23
+ *
R31 R32 R33
+ * Please refer to the "9-Axis Sensor Fusion Application Note" document, + * section 7 "Sensor Fusion Output", for details regarding rotation + * matrix output. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long at least. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_rot_mat_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + { + long qdata[4], rdata[9]; + inv_get_quaternion(qdata); + inv_quaternion_to_rotation(qdata, rdata); + data[0] = (float)rdata[0] / 1073741824.0f; + data[1] = (float)rdata[1] / 1073741824.0f; + data[2] = (float)rdata[2] / 1073741824.0f; + data[3] = (float)rdata[3] / 1073741824.0f; + data[4] = (float)rdata[4] / 1073741824.0f; + data[5] = (float)rdata[5] / 1073741824.0f; + data[6] = (float)rdata[6] / 1073741824.0f; + data[7] = (float)rdata[7] / 1073741824.0f; + data[8] = (float)rdata[8] / 1073741824.0f; + } + + return result; +} + +/** + * @brief inv_get_quaternion_float is used to get the quaternion representation + * of the current sensor fusion solution. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 4 cells long. + * + * @return INV_SUCCESS if the command is successful; an ML error code otherwise. + */ + /* inv_get_quaternion_float implemented in mlFIFO.c */ + +/** + * @brief inv_get_linear_accel_float is used to get an estimate of linear + * acceleration, based on the most recent accelerometer measurement + * and sensor fusion solution. + * The values are in units of g (gravity). + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_linear_accel_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + long ldata[3]; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_linear_accel(ldata); + data[0] = (float)ldata[0] / 65536.0f; + data[1] = (float)ldata[1] / 65536.0f; + data[2] = (float)ldata[2] / 65536.0f; + + return result; +} + +/** + * @brief inv_get_linear_accel_in_world_float is used to get an estimate of + * linear acceleration, in the world frame, based on the most + * recent accelerometer measurement and sensor fusion solution. + * The values are in units of g (gravity). + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_linear_accel_in_world_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + long ldata[3]; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_linear_accel_in_world(ldata); + data[0] = (float)ldata[0] / 65536.0f; + data[1] = (float)ldata[1] / 65536.0f; + data[2] = (float)ldata[2] / 65536.0f; + + return result; +} + +/** + * @brief inv_get_gravity_float is used to get an estimate of the body frame + * gravity vector, based on the most recent sensor fusion solution. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long at least. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_gravity_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + long ldata[3]; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_get_gravity(ldata); + data[0] = (float)ldata[0] / 65536.0f; + data[1] = (float)ldata[1] / 65536.0f; + data[2] = (float)ldata[2] / 65536.0f; + + return result; +} + +/** + * @brief inv_get_gyro_cal_matrix_float is used to get the gyroscope + * calibration matrix. The gyroscope calibration matrix defines the relationship + * between the gyroscope sensor axes and the sensor fusion solution axes. + * Calibration matrix data members will have a value of 1.0, 0, or -1.0. + * The matrix has members + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_gyro_cal_matrix_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.gyro_cal[0] / 1073741824.0f; + data[1] = (float)inv_obj.gyro_cal[1] / 1073741824.0f; + data[2] = (float)inv_obj.gyro_cal[2] / 1073741824.0f; + data[3] = (float)inv_obj.gyro_cal[3] / 1073741824.0f; + data[4] = (float)inv_obj.gyro_cal[4] / 1073741824.0f; + data[5] = (float)inv_obj.gyro_cal[5] / 1073741824.0f; + data[6] = (float)inv_obj.gyro_cal[6] / 1073741824.0f; + data[7] = (float)inv_obj.gyro_cal[7] / 1073741824.0f; + data[8] = (float)inv_obj.gyro_cal[8] / 1073741824.0f; + + return result; +} + +/** + * @brief inv_get_accel_cal_matrix_float is used to get the accelerometer + * calibration matrix. + * Calibration matrix data members will have a value of 1.0, 0, or -1.0. + * The matrix has members + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * The argument array elements are ordered C11, C12, C13, C21, C22, C23, C31, C32, C33. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ + +inv_error_t inv_get_accel_cal_matrix_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.accel_cal[0] / 1073741824.0f; + data[1] = (float)inv_obj.accel_cal[1] / 1073741824.0f; + data[2] = (float)inv_obj.accel_cal[2] / 1073741824.0f; + data[3] = (float)inv_obj.accel_cal[3] / 1073741824.0f; + data[4] = (float)inv_obj.accel_cal[4] / 1073741824.0f; + data[5] = (float)inv_obj.accel_cal[5] / 1073741824.0f; + data[6] = (float)inv_obj.accel_cal[6] / 1073741824.0f; + data[7] = (float)inv_obj.accel_cal[7] / 1073741824.0f; + data[8] = (float)inv_obj.accel_cal[8] / 1073741824.0f; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_cal_matrix_float is used to get an array of nine data points + * representing the calibration matrix for the compass: + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long at least. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_cal_matrix_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.compass_cal[0] / 1073741824.0f; + data[1] = (float)inv_obj.compass_cal[1] / 1073741824.0f; + data[2] = (float)inv_obj.compass_cal[2] / 1073741824.0f; + data[3] = (float)inv_obj.compass_cal[3] / 1073741824.0f; + data[4] = (float)inv_obj.compass_cal[4] / 1073741824.0f; + data[5] = (float)inv_obj.compass_cal[5] / 1073741824.0f; + data[6] = (float)inv_obj.compass_cal[6] / 1073741824.0f; + data[7] = (float)inv_obj.compass_cal[7] / 1073741824.0f; + data[8] = (float)inv_obj.compass_cal[8] / 1073741824.0f; + return result; +} + +/** + * @brief inv_get_gyro_temp_slope_float is used to get the temperature + * compensation algorithm's estimate of the gyroscope bias temperature + * coefficient. + * The argument array elements are ordered X,Y,Z. + * Values are in units of dps per deg C (degrees per second per degree + * Celcius) + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long . + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_gyro_temp_slope_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + if (inv_params_obj.bias_mode & INV_LEARN_BIAS_FROM_TEMPERATURE) { + data[0] = inv_obj.x_gyro_coef[1]; + data[1] = inv_obj.y_gyro_coef[1]; + data[2] = inv_obj.z_gyro_coef[1]; + } else { + data[0] = (float)inv_obj.temp_slope[0] / 65536.0f; + data[1] = (float)inv_obj.temp_slope[1] / 65536.0f; + data[2] = (float)inv_obj.temp_slope[2] / 65536.0f; + } + + return result; +} + +/** + * @brief inv_get_gyro_bias_float is used to get the gyroscope biases. + * The argument array elements are ordered X,Y,Z. + * The values are in units of dps (degrees per second). + + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_gyro_bias_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.gyro_bias[0] / 65536.0f; + data[1] = (float)inv_obj.gyro_bias[1] / 65536.0f; + data[2] = (float)inv_obj.gyro_bias[2] / 65536.0f; + + return result; +} + +/** + * @brief inv_get_accel_bias_float is used to get the accelerometer baises. + * The argument array elements are ordered X,Y,Z. + * The values are in units of g (gravity). + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_accel_bias_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.accel_bias[0] / 65536.0f; + data[1] = (float)inv_obj.accel_bias[1] / 65536.0f; + data[2] = (float)inv_obj.accel_bias[2] / 65536.0f; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_bias_float is used to get an array of three data points representing + * the compass biases. + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_bias_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = + ((float) + (inv_obj.compass_bias[0] + + (long)((long long)inv_obj.init_compass_bias[0] * + inv_obj.compass_sens / 16384))) / 65536.0f; + data[1] = + ((float) + (inv_obj.compass_bias[1] + + (long)((long long)inv_obj.init_compass_bias[1] * + inv_obj.compass_sens / 16384))) / 65536.0f; + data[2] = + ((float) + (inv_obj.compass_bias[2] + + (long)((long long)inv_obj.init_compass_bias[2] * + inv_obj.compass_sens / 16384))) / 65536.0f; + + return result; +} + +/** + * @brief inv_get_gyro_and_accel_sensor_float is used to get the most recent set of all sensor data. + * The argument array elements are ordered gyroscope X,Y, and Z, + * accelerometer X, Y, and Z, and magnetometer X,Y, and Z. + * \if UMPL The magnetometer elements are not populated in UMPL. \endif + * The gyroscope and accelerometer data is not scaled or offset, it is + * copied directly from the sensor registers, and cast as a float. + * In the case of accelerometers with 8-bit output resolution, the data + * is scaled up to match the 2^14 = 1 g typical represntation of +/- 2 g + * full scale range + + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_gyro_and_accel_sensor_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + long ldata[6]; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_gyro_and_accel_sensor(ldata); + data[0] = (float)ldata[0]; + data[1] = (float)ldata[1]; + data[2] = (float)ldata[2]; + data[3] = (float)ldata[3]; + data[4] = (float)ldata[4]; + data[5] = (float)ldata[5]; + data[6] = (float)inv_obj.compass_sensor_data[0]; + data[7] = (float)inv_obj.compass_sensor_data[1]; + data[8] = (float)inv_obj.compass_sensor_data[2]; + + return result; +} + +/** + * @brief inv_get_euler_angles_x is used to get the Euler angle representation + * of the current sensor fusion solution. + * Euler angles are returned according to the X convention. + * This is typically the convention used for mobile devices where the X + * axis is the width of the screen, Y axis is the height, and Z the + * depth. In this case roll is defined as the rotation around the X + * axis of the device. + * + * + * + * + * + * +
Element Euler angleRotation about
0 Roll X axis
1 Pitch Y axis
2 Yaw Z axis
+ * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_euler_angles_x_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_rot_mat_float(rotMatrix); + tmp = rotMatrix[6]; + if (tmp > 1.0f) { + tmp = 1.0f; + } + if (tmp < -1.0f) { + tmp = -1.0f; + } + data[0] = + (float)(atan2f(rotMatrix[7], + rotMatrix[8]) * 57.29577951308); + data[1] = (float)((double)asin(tmp) * 57.29577951308); + data[2] = + (float)(atan2f(rotMatrix[3], rotMatrix[0]) * 57.29577951308); + + return result; +} + +/** + * @brief inv_get_euler_angles_float is used to get an array of three data points three data points + * representing roll, pitch, and yaw corresponding to the INV_EULER_ANGLES_X output and it is + * therefore the default convention for Euler angles. + * Please refer to the INV_EULER_ANGLES_X for a detailed description. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_euler_angles_float(float *data) +{ + return inv_get_euler_angles_x_float(data); +} + +/** @brief inv_get_euler_angles_y_float is used to get the Euler angle representation + * of the current sensor fusion solution. + * Euler angles are returned according to the Y convention. + * This convention is typically used in augmented reality applications, + * where roll is defined as the rotation around the axis along the + * height of the screen of a mobile device, namely the Y axis. + * + * + * + * + * + *
Element Euler angleRotation about
0 Roll Y axis
1 Pitch X axis
2 Yaw Z axis
+ * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_euler_angles_y_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_rot_mat_float(rotMatrix); + tmp = rotMatrix[7]; + if (tmp > 1.0f) { + tmp = 1.0f; + } + if (tmp < -1.0f) { + tmp = -1.0f; + } + data[0] = + (float)(atan2f(rotMatrix[8], rotMatrix[6]) * 57.29577951308); + data[1] = (float)((double)asin(tmp) * 57.29577951308); + data[2] = + (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308); + + return result; +} + +/** @brief inv_get_euler_angles_z_float is used to get the Euler angle representation + * of the current sensor fusion solution. + * This convention is mostly used in application involving the use + * of a camera, typically placed on the back of a mobile device, that + * is along the Z axis. In this convention roll is defined as the + * rotation around the Z axis. + * Euler angles are returned according to the Y convention. + * + * + * + * + * + *
Element Euler angleRotation about
0 Roll Z axis
1 Pitch X axis
2 Yaw Y axis
+ * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_euler_angles_z_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + result = inv_get_rot_mat_float(rotMatrix); + tmp = rotMatrix[8]; + if (tmp > 1.0f) { + tmp = 1.0f; + } + if (tmp < -1.0f) { + tmp = -1.0f; + } + data[0] = + (float)(atan2f(rotMatrix[6], rotMatrix[7]) * 57.29577951308); + data[1] = (float)((double)asin(tmp) * 57.29577951308); + data[2] = + (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308); + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_raw_data_float is used to get Raw magnetometer data + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_raw_data_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = + (float)(inv_obj.compass_sensor_data[0] + inv_obj.init_compass_bias[0]); + data[1] = + (float)(inv_obj.compass_sensor_data[1] + inv_obj.init_compass_bias[1]); + data[2] = + (float)(inv_obj.compass_sensor_data[2] + inv_obj.init_compass_bias[2]); + + return result; +} + +/** + * @cond MPL + * @brief inv_get_magnetometer_float is used to get magnetometer data + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_magnetometer_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.compass_calibrated_data[0] / 65536.0f; + data[1] = (float)inv_obj.compass_calibrated_data[1] / 65536.0f; + data[2] = (float)inv_obj.compass_calibrated_data[2] / 65536.0f; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_pressure_float is used to get a single value representing the pressure in Pascal + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to the data to be passed back to the user. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_pressure_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.pressure; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_heading_float is used to get single number representing the heading of the device + * relative to the Earth, in which 0 represents North, 90 degrees + * represents East, and so on. + * The heading is defined as the direction of the +Y axis if the Y + * axis is horizontal, and otherwise the direction of the -Z axis. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to the data to be passed back to the user. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_heading_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + float rotMatrix[9]; + float tmp; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + inv_get_rot_mat_float(rotMatrix); + if ((rotMatrix[7] < 0.707) && (rotMatrix[7] > -0.707)) { + tmp = + (float)(atan2f(rotMatrix[4], rotMatrix[1]) * 57.29577951308 - + 90.0f); + } else { + tmp = + (float)(atan2f(rotMatrix[5], rotMatrix[2]) * 57.29577951308 + + 90.0f); + } + if (tmp < 0) { + tmp += 360.0f; + } + data[0] = 360 - tmp; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_bias_error_float is used to get an array of three numbers representing + * the current estimated error in the compass biases. These numbers are unitless and serve + * as rough estimates in which numbers less than 100 typically represent + * reasonably well calibrated compass axes. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_bias_error_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + if (inv_obj.large_field == 0) { + data[0] = (float)inv_obj.compass_bias_error[0]; + data[1] = (float)inv_obj.compass_bias_error[1]; + data[2] = (float)inv_obj.compass_bias_error[2]; + } else { + data[0] = (float)P_INIT; + data[1] = (float)P_INIT; + data[2] = (float)P_INIT; + } + + return result; +} + +/** + * @cond MPL + * @brief inv_get_mag_scale_float is used to get magnetometer scale. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_mag_scale_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.compass_scale[0] / 65536.0f; + data[1] = (float)inv_obj.compass_scale[1] / 65536.0f; + data[2] = (float)inv_obj.compass_scale[2] / 65536.0f; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_local_field_float is used to get local magnetic field data. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 3 cells long. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_local_field_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.local_field[0] / 65536.0f; + data[1] = (float)inv_obj.local_field[1] / 65536.0f; + data[2] = (float)inv_obj.local_field[2] / 65536.0f; + + return result; +} + +/** + * @cond MPL + * @brief inv_get_relative_quaternion_float is used to get relative quaternion data. + * + * @pre MLDmpOpen() \ifnot UMPL or MLDmpPedometerStandAloneOpen() \endif + * must have been called. + * + * @param data + * A pointer to an array to be passed back to the user. + * Must be 4 cells long at least. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + * @endcond + */ +inv_error_t inv_get_relative_quaternion_float(float *data) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + data[0] = (float)inv_obj.relative_quat[0] / 1073741824.0f; + data[1] = (float)inv_obj.relative_quat[1] / 1073741824.0f; + data[2] = (float)inv_obj.relative_quat[2] / 1073741824.0f; + data[3] = (float)inv_obj.relative_quat[3] / 1073741824.0f; + + return result; +} + +/** + * Returns the curren compass accuracy. + * + * - 0: Unknown: The accuracy is unreliable and compass data should not be used + * - 1: Low: The compass accuracy is low. + * - 2: Medium: The compass accuracy is medium. + * - 3: High: The compas acurracy is high and can be trusted + * + * @param accuracy The accuracy level in the range 0-3 + * + * @return ML_SUCCESS or non-zero error code + */ +inv_error_t inv_get_compass_accuracy(int *accuracy) +{ + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + *accuracy = inv_obj.compass_accuracy; + return INV_SUCCESS; +} + +/** + * @brief inv_set_gyro_bias is used to set the gyroscope bias. + * The argument array elements are ordered X,Y,Z. + * The values are scaled at 1 dps = 2^16 LSBs. + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_gyro_bias(long *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + long biasTmp; + long sf = 0; + short offset[GYRO_NUM_AXES]; + int i; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + if (mldl_cfg->gyro_sens_trim != 0) { + sf = 2000 * 131 / mldl_cfg->gyro_sens_trim; + } else { + sf = 2000; + } + for (i = 0; i < GYRO_NUM_AXES; i++) { + inv_obj.gyro_bias[i] = data[i]; + biasTmp = -inv_obj.gyro_bias[i] / sf; + if (biasTmp < 0) + biasTmp += 65536L; + offset[i] = (short)biasTmp; + } + result = inv_set_offset(offset); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return INV_SUCCESS; +} + +/** + * @brief inv_set_accel_bias is used to set the accelerometer bias. + * The argument array elements are ordered X,Y,Z. + * The values are scaled in units of g (gravity), + * where 1 g = 2^16 LSBs. + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_accel_bias(long *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + long biasTmp; + int i, j; + unsigned char regs[6]; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + for (i = 0; i < ACCEL_NUM_AXES; i++) { + inv_obj.accel_bias[i] = data[i]; + if (inv_obj.accel_sens != 0 && mldl_cfg && mldl_cfg->pdata) { + long long tmp64; + inv_obj.scaled_accel_bias[i] = 0; + for (j = 0; j < ACCEL_NUM_AXES; j++) { + inv_obj.scaled_accel_bias[i] += + data[j] * + (long)mldl_cfg->pdata->accel.orientation[i * 3 + j]; + } + tmp64 = (long long)inv_obj.scaled_accel_bias[i] << 13; + biasTmp = (long)(tmp64 / inv_obj.accel_sens); + } else { + biasTmp = 0; + } + if (biasTmp < 0) + biasTmp += 65536L; + regs[2 * i + 0] = (unsigned char)(biasTmp / 256); + regs[2 * i + 1] = (unsigned char)(biasTmp % 256); + } + result = inv_set_mpu_memory(KEY_D_1_8, 2, ®s[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_mpu_memory(KEY_D_1_10, 2, ®s[2]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_set_mpu_memory(KEY_D_1_2, 2, ®s[4]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return INV_SUCCESS; +} + +/** + * @cond MPL + * @brief inv_set_mag_bias is used to set Compass Bias + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + * @endcond + */ +inv_error_t inv_set_mag_bias(long *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + inv_set_compass_bias(data); + inv_obj.init_compass_bias[0] = 0; + inv_obj.init_compass_bias[1] = 0; + inv_obj.init_compass_bias[2] = 0; + inv_obj.got_compass_bias = 1; + inv_obj.got_init_compass_bias = 1; + inv_obj.compass_state = SF_STARTUP_SETTLE; + + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return INV_SUCCESS; +} + +/** + * @brief inv_set_gyro_temp_slope is used to set the temperature + * compensation algorithm's estimate of the gyroscope bias temperature + * coefficient. + * The argument array elements are ordered X,Y,Z. + * Values are in units of dps per deg C (degrees per second per degree + * Celcius), and scaled such that 1 dps per deg C = 2^16 LSBs. + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document. + * + * @brief inv_set_gyro_temp_slope is used to set Gyro temperature slope + * + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_gyro_temp_slope(long *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + int i; + long sf; + unsigned char regs[3]; + + inv_obj.factory_temp_comp = 1; + inv_obj.temp_slope[0] = data[0]; + inv_obj.temp_slope[1] = data[1]; + inv_obj.temp_slope[2] = data[2]; + for (i = 0; i < GYRO_NUM_AXES; i++) { + sf = -inv_obj.temp_slope[i] / 1118; + if (sf > 127) { + sf -= 256; + } + regs[i] = (unsigned char)sf; + } + result = inv_set_offsetTC(regs); + + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return INV_SUCCESS; +} + +/** + * @cond MPL + * @brief inv_set_local_field is used to set local magnetic field + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + * @endcond + */ +inv_error_t inv_set_local_field(long *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + inv_obj.local_field[0] = data[0]; + inv_obj.local_field[1] = data[1]; + inv_obj.local_field[2] = data[2]; + inv_obj.new_local_field = 1; + + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return INV_SUCCESS; +} + +/** + * @cond MPL + * @brief inv_set_mag_scale is used to set magnetometer scale + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + * @endcond + */ +inv_error_t inv_set_mag_scale(long *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + inv_obj.compass_scale[0] = data[0]; + inv_obj.compass_scale[1] = data[1]; + inv_obj.compass_scale[2] = data[2]; + + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return INV_SUCCESS; +} + +/** + * @brief inv_set_gyro_temp_slope_float is used to get the temperature + * compensation algorithm's estimate of the gyroscope bias temperature + * coefficient. + * The argument array elements are ordered X,Y,Z. + * Values are in units of dps per deg C (degrees per second per degree + * Celcius) + + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_gyro_temp_slope_float(float *data) +{ + long arrayTmp[3]; + arrayTmp[0] = (long)(data[0] * 65536.f); + arrayTmp[1] = (long)(data[1] * 65536.f); + arrayTmp[2] = (long)(data[2] * 65536.f); + return inv_set_gyro_temp_slope(arrayTmp); +} + +/** + * @brief inv_set_gyro_bias_float is used to set the gyroscope bias. + * The argument array elements are ordered X,Y,Z. + * The values are in units of dps (degrees per second). + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_gyro_bias_float(float *data) +{ + long arrayTmp[3]; + arrayTmp[0] = (long)(data[0] * 65536.f); + arrayTmp[1] = (long)(data[1] * 65536.f); + arrayTmp[2] = (long)(data[2] * 65536.f); + return inv_set_gyro_bias(arrayTmp); + +} + +/** + * @brief inv_set_accel_bias_float is used to set the accelerometer bias. + * The argument array elements are ordered X,Y,Z. + * The values are in units of g (gravity). + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_accel_bias_float(float *data) +{ + long arrayTmp[3]; + arrayTmp[0] = (long)(data[0] * 65536.f); + arrayTmp[1] = (long)(data[1] * 65536.f); + arrayTmp[2] = (long)(data[2] * 65536.f); + return inv_set_accel_bias(arrayTmp); + +} + +/** + * @cond MPL + * @brief inv_set_mag_bias_float is used to set compass bias + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen()\ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + * @endcond + */ +inv_error_t inv_set_mag_bias_float(float *data) +{ + long arrayTmp[3]; + arrayTmp[0] = (long)(data[0] * 65536.f); + arrayTmp[1] = (long)(data[1] * 65536.f); + arrayTmp[2] = (long)(data[2] * 65536.f); + return inv_set_mag_bias(arrayTmp); +} + +/** + * @cond MPL + * @brief inv_set_local_field_float is used to set local magnetic field + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + * @endcond + */ +inv_error_t inv_set_local_field_float(float *data) +{ + long arrayTmp[3]; + arrayTmp[0] = (long)(data[0] * 65536.f); + arrayTmp[1] = (long)(data[1] * 65536.f); + arrayTmp[2] = (long)(data[2] * 65536.f); + return inv_set_local_field(arrayTmp); +} + +/** + * @cond MPL + * @brief inv_set_mag_scale_float is used to set magnetometer scale + * + * Please refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() \ifnot UMPL or + * MLDmpPedometerStandAloneOpen() \endif + * @pre MLDmpStart() must NOT have been called. + * + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + * @endcond + */ +inv_error_t inv_set_mag_scale_float(float *data) +{ + long arrayTmp[3]; + arrayTmp[0] = (long)(data[0] * 65536.f); + arrayTmp[1] = (long)(data[1] * 65536.f); + arrayTmp[2] = (long)(data[2] * 65536.f); + return inv_set_mag_scale(arrayTmp); +} diff --git a/60xx/mlsdk/mllite/mlarray_legacy.c b/60xx/mlsdk/mllite/mlarray_legacy.c new file mode 100644 index 0000000..9dce8f3 --- /dev/null +++ b/60xx/mlsdk/mllite/mlarray_legacy.c @@ -0,0 +1,587 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/****************************************************************************** + * + * $Id: mlarray_legacy.c $ + * + *****************************************************************************/ + +/** + * @defgroup MLArray_Legacy + * @brief Legacy Motion Library Array APIs. + * The Motion Library Array APIs provide the user access to the + * Motion Library state. These Legacy APIs provide access to + * individual state arrays using a data set name as the first + * argument to the API. This format has been replaced by unique + * named APIs for each data set, found in the MLArray group. + * + * @{ + * @file mlarray_legacy.c + * @brief The Legacy Motion Library Array APIs. + */ + +#include "ml.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "mlFIFO.h" +#include "mldl_cfg.h" + +/** + * @brief inv_get_array is used to get an array of processed motion sensor data. + * inv_get_array can be used to retrieve various data sets. Certain data + * sets require functions to be enabled using MLEnable in order to be + * valid. + * + * The available data sets are: + * + * - INV_ROTATION_MATRIX + * - INV_QUATERNION + * - INV_EULER_ANGLES_X + * - INV_EULER_ANGLES_Y + * - INV_EULER_ANGLES_Z + * - INV_EULER_ANGLES + * - INV_LINEAR_ACCELERATION + * - INV_LINEAR_ACCELERATION_WORLD + * - INV_GRAVITY + * - INV_ANGULAR_VELOCITY + * - INV_RAW_DATA + * - INV_GYROS + * - INV_ACCELS + * - INV_MAGNETOMETER + * - INV_GYRO_BIAS + * - INV_ACCEL_BIAS + * - INV_MAG_BIAS + * - INV_HEADING + * - INV_MAG_BIAS_ERROR + * - INV_PRESSURE + * + * Please refer to the documentation of inv_get_float_array() for a + * description of these data sets. + * + * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() + * must have been called. + * + * @param dataSet + * A constant specifying an array of data processed by the + * motion processor. + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long at least. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_array(int dataSet, long *data) +{ + inv_error_t result; + switch (dataSet) { + case INV_GYROS: + result = inv_get_gyro(data); + break; + case INV_ACCELS: + result = inv_get_accel(data); + break; + case INV_TEMPERATURE: + result = inv_get_temperature(data); + break; + case INV_ROTATION_MATRIX: + result = inv_get_rot_mat(data); + break; + case INV_QUATERNION: + result = inv_get_quaternion(data); + break; + case INV_LINEAR_ACCELERATION: + result = inv_get_linear_accel(data); + break; + case INV_LINEAR_ACCELERATION_WORLD: + result = inv_get_linear_accel_in_world(data); + break; + case INV_GRAVITY: + result = inv_get_gravity(data); + break; + case INV_ANGULAR_VELOCITY: + result = inv_get_angular_velocity(data); + break; + case INV_EULER_ANGLES: + result = inv_get_euler_angles(data); + break; + case INV_EULER_ANGLES_X: + result = inv_get_euler_angles_x(data); + break; + case INV_EULER_ANGLES_Y: + result = inv_get_euler_angles_y(data); + break; + case INV_EULER_ANGLES_Z: + result = inv_get_euler_angles_z(data); + break; + case INV_GYRO_TEMP_SLOPE: + result = inv_get_gyro_temp_slope(data); + break; + case INV_GYRO_BIAS: + result = inv_get_gyro_bias(data); + break; + case INV_ACCEL_BIAS: + result = inv_get_accel_bias(data); + break; + case INV_MAG_BIAS: + result = inv_get_mag_bias(data); + break; + case INV_RAW_DATA: + result = inv_get_gyro_and_accel_sensor(data); + break; + case INV_MAG_RAW_DATA: + result = inv_get_mag_raw_data(data); + break; + case INV_MAGNETOMETER: + result = inv_get_magnetometer(data); + break; + case INV_PRESSURE: + result = inv_get_pressure(data); + break; + case INV_HEADING: + result = inv_get_heading(data); + break; + case INV_GYRO_CALIBRATION_MATRIX: + result = inv_get_gyro_cal_matrix(data); + break; + case INV_ACCEL_CALIBRATION_MATRIX: + result = inv_get_accel_cal_matrix(data); + break; + case INV_MAG_CALIBRATION_MATRIX: + result = inv_get_mag_cal_matrix(data); + break; + case INV_MAG_BIAS_ERROR: + result = inv_get_mag_bias_error(data); + break; + case INV_MAG_SCALE: + result = inv_get_mag_scale(data); + break; + case INV_LOCAL_FIELD: + result = inv_get_local_field(data); + break; + case INV_RELATIVE_QUATERNION: + result = inv_get_relative_quaternion(data); + break; + default: + return INV_ERROR_INVALID_PARAMETER; + break; + } + return result; +} + +/** + * @brief inv_get_float_array is used to get an array of processed motion sensor + * data. inv_get_array can be used to retrieve various data sets. + * Certain data sets require functions to be enabled using MLEnable + * in order to be valid. + * + * The available data sets are: + * + * - INV_ROTATION_MATRIX : + * Returns an array of nine data points representing the rotation + * matrix generated from all available sensors. + * This requires that ML_SENSOR_FUSION be enabled. + * The array format will be R11, R12, R13, R21, R22, R23, R31, R32, + * R33, representing the matrix: + *
R11 R12 R13
+ *
R21 R22 R23
+ *
R31 R32 R33
+ * Please refer to the "9-Axis Sensor Fusion Application Note" document, + * section 7 "Sensor Fusion Output", for details regarding rotation + * matrix output. + * + * - INV_QUATERNION : + * Returns an array of four data points representing the quaternion + * generated from all available sensors. + * This requires that ML_SENSOR_FUSION be enabled. + * + * - INV_EULER_ANGLES_X : + * Returns an array of three data points representing roll, pitch, and + * yaw using the X axis of the gyroscope, accelerometer, and compass as + * reference axis. + * This is typically the convention used for mobile devices where the X + * axis is the width of the screen, Y axis is the height, and Z the + * depth. In this case roll is defined as the rotation around the X + * axis of the device. + * The euler angles convention for this output is the following: + * + * + * + * + * + *
EULER ANGLEROTATION AROUND
roll X axis
pitch Y axis
yaw Z axis
+ * INV_EULER_ANGLES_X corresponds to the INV_EULER_ANGLES output and is + * therefore the default convention. + * + * - INV_EULER_ANGLES_Y : + * Returns an array of three data points representing roll, pitch, and + * yaw using the Y axis of the gyroscope, accelerometer, and compass as + * reference axis. + * This convention is typically used in augmented reality applications, + * where roll is defined as the rotation around the axis along the + * height of the screen of a mobile device, namely the Y axis. + * The euler angles convention for this output is the following: + * + * + * + * + * + *
EULER ANGLEROTATION AROUND
roll Y axis
pitch X axis
yaw Z axis
+ * + * - INV_EULER_ANGLES_Z : + * Returns an array of three data points representing roll, pitch, and + * yaw using the Z axis of the gyroscope, accelerometer, and compass as + * reference axis. + * This convention is mostly used in application involving the use + * of a camera, typically placed on the back of a mobile device, that + * is along the Z axis. In this convention roll is defined as the + * rotation around the Z axis. + * The euler angles convention for this output is the following: + * + * + * + * + * + *
EULER ANGLEROTATION AROUND
roll Z axis
pitch X axis
yaw Y axis
+ * + * - INV_EULER_ANGLES : + * Returns an array of three data points representing roll, pitch, and + * yaw corresponding to the INV_EULER_ANGLES_X output and it is + * therefore the default convention for Euler angles. + * Please refer to the INV_EULER_ANGLES_X for a detailed description. + * + * - INV_LINEAR_ACCELERATION : + * Returns an array of three data points representing the linear + * acceleration as derived from both gyroscopes and accelerometers. + * This requires that ML_SENSOR_FUSION be enabled. + * + * - INV_LINEAR_ACCELERATION_WORLD : + * Returns an array of three data points representing the linear + * acceleration in world coordinates, as derived from both gyroscopes + * and accelerometers. + * This requires that ML_SENSOR_FUSION be enabled. + * + * - INV_GRAVITY : + * Returns an array of three data points representing the direction + * of gravity in body coordinates, as derived from both gyroscopes + * and accelerometers. + * This requires that ML_SENSOR_FUSION be enabled. + * + * - INV_ANGULAR_VELOCITY : + * Returns an array of three data points representing the angular + * velocity as derived from both gyroscopes and accelerometers. + * This requires that ML_SENSOR_FUSION be enabled, to fuse data from + * the gyroscope and accelerometer device, appropriately scaled and + * oriented according to the respective mounting matrices. + * + * - INV_RAW_DATA : + * Returns an array of nine data points representing raw sensor data + * of the gyroscope X, Y, Z, accelerometer X, Y, Z, and + * compass X, Y, Z values. + * These values are not scaled and come out directly from the devices' + * sensor data output. In case of accelerometers with lower output + * resolution, e.g 8-bit, the sensor data is scaled up to match the + * 2^14 = 1 gee typical representation for a +/- 2 gee full scale + * range. + * + * - INV_GYROS : + * Returns an array of three data points representing the X gyroscope, + * Y gyroscope, and Z gyroscope values. + * The values are not sensor fused with other sensor types data but + * reflect the orientation from the mounting matrices in use. + * The INV_GYROS values are scaled to ensure 1 dps corresponds to 2^16 + * codes. + * + * - INV_ACCELS : + * Returns an array of three data points representing the X + * accelerometer, Y accelerometer, and Z accelerometer values. + * The values are not sensor fused with other sensor types data but + * reflect the orientation from the mounting matrices in use. + * The INV_ACCELS values are scaled to ensure 1 gee corresponds to 2^16 + * codes. + * + * - INV_MAGNETOMETER : + * Returns an array of three data points representing the compass + * X, Y, and Z values. + * The values are not sensor fused with other sensor types data but + * reflect the orientation from the mounting matrices in use. + * The INV_MAGNETOMETER values are scaled to ensure 1 micro Tesla (uT) + * corresponds to 2^16 codes. + * + * - INV_GYRO_BIAS : + * Returns an array of three data points representing the gyroscope + * biases. + * + * - INV_ACCEL_BIAS : + * Returns an array of three data points representing the + * accelerometer biases. + * + * - INV_MAG_BIAS : + * Returns an array of three data points representing the compass + * biases. + * + * - INV_GYRO_CALIBRATION_MATRIX : + * Returns an array of nine data points representing the calibration + * matrix for the gyroscopes: + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * + * - INV_ACCEL_CALIBRATION_MATRIX : + * Returns an array of nine data points representing the calibration + * matrix for the accelerometers: + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * + * - INV_MAG_CALIBRATION_MATRIX : + * Returns an array of nine data points representing the calibration + * matrix for the compass: + *
C11 C12 C13
+ *
C21 C22 C23
+ *
C31 C32 C33
+ * + * - INV_PRESSURE : + * Returns a single value representing the pressure in Pascal + * + * - INV_HEADING : + * Returns a single number representing the heading of the device + * relative to the Earth, in which 0 represents North, 90 degrees + * represents East, and so on. + * The heading is defined as the direction of the +Y axis if the Y + * axis is horizontal, and otherwise the direction of the -Z axis. + * + * - INV_MAG_BIAS_ERROR : + * Returns an array of three numbers representing the current estimated + * error in the compass biases. These numbers are unitless and serve + * as rough estimates in which numbers less than 100 typically represent + * reasonably well calibrated compass axes. + * + * @pre MLDmpOpen() or MLDmpPedometerStandAloneOpen() + * must have been called. + * + * @param dataSet + * A constant specifying an array of data processed by + * the motion processor. + * @param data + * A pointer to an array to be passed back to the user. + * Must be 9 cells long at least. + * + * @return INV_SUCCESS if the command is successful; an error code otherwise. + */ +inv_error_t inv_get_float_array(int dataSet, float *data) +{ + inv_error_t result; + switch (dataSet) { + case INV_GYROS: + result = inv_get_gyro_float(data); + break; + case INV_ACCELS: + result = inv_get_accel_float(data); + break; + case INV_TEMPERATURE: + result = inv_get_temperature_float(data); + break; + case INV_ROTATION_MATRIX: + result = inv_get_rot_mat_float(data); + break; + case INV_QUATERNION: + result = inv_get_quaternion_float(data); + break; + case INV_LINEAR_ACCELERATION: + result = inv_get_linear_accel_float(data); + break; + case INV_LINEAR_ACCELERATION_WORLD: + result = inv_get_linear_accel_in_world_float(data); + break; + case INV_GRAVITY: + result = inv_get_gravity_float(data); + break; + case INV_ANGULAR_VELOCITY: + result = inv_get_angular_velocity_float(data); + break; + case INV_EULER_ANGLES: + result = inv_get_euler_angles_float(data); + break; + case INV_EULER_ANGLES_X: + result = inv_get_euler_angles_x_float(data); + break; + case INV_EULER_ANGLES_Y: + result = inv_get_euler_angles_y_float(data); + break; + case INV_EULER_ANGLES_Z: + result = inv_get_euler_angles_z_float(data); + break; + case INV_GYRO_TEMP_SLOPE: + result = inv_get_gyro_temp_slope_float(data); + break; + case INV_GYRO_BIAS: + result = inv_get_gyro_bias_float(data); + break; + case INV_ACCEL_BIAS: + result = inv_get_accel_bias_float(data); + break; + case INV_MAG_BIAS: + result = inv_get_mag_bias_float(data); + break; + case INV_RAW_DATA: + result = inv_get_gyro_and_accel_sensor_float(data); + break; + case INV_MAG_RAW_DATA: + result = inv_get_mag_raw_data_float(data); + break; + case INV_MAGNETOMETER: + result = inv_get_magnetometer_float(data); + break; + case INV_PRESSURE: + result = inv_get_pressure_float(data); + break; + case INV_HEADING: + result = inv_get_heading_float(data); + break; + case INV_GYRO_CALIBRATION_MATRIX: + result = inv_get_gyro_cal_matrix_float(data); + break; + case INV_ACCEL_CALIBRATION_MATRIX: + result = inv_get_accel_cal_matrix_float(data); + break; + case INV_MAG_CALIBRATION_MATRIX: + result = inv_get_mag_cal_matrix_float(data); + break; + case INV_MAG_BIAS_ERROR: + result = inv_get_mag_bias_error_float(data); + break; + case INV_MAG_SCALE: + result = inv_get_mag_scale_float(data); + break; + case INV_LOCAL_FIELD: + result = inv_get_local_field_float(data); + break; + case INV_RELATIVE_QUATERNION: + result = inv_get_relative_quaternion_float(data); + break; + default: + return INV_ERROR_INVALID_PARAMETER; + break; + } + return result; +} + +/** + * @brief used to set an array of motion sensor data. + * Handles the following data sets: + * - INV_GYRO_BIAS + * - INV_ACCEL_BIAS + * - INV_MAG_BIAS + * - INV_GYRO_TEMP_SLOPE + * + * For more details about the use of the data sets + * please refer to the documentation of inv_set_float_array(). + * + * Please also refer to the provided "9-Axis Sensor Fusion + * Application Note" document provided. + * + * @pre MLDmpOpen() or + * MLDmpPedometerStandAloneOpen() + * @pre MLDmpStart() must NOT have been called. + * + * @param dataSet A constant specifying an array of data. + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_array(int dataSet, long *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + switch (dataSet) { + case INV_GYRO_BIAS: + result = inv_set_gyro_bias(data); + break; + case INV_ACCEL_BIAS: + result = inv_set_accel_bias(data); + break; + case INV_MAG_BIAS: + result = inv_set_mag_bias(data); + break; + case INV_GYRO_TEMP_SLOPE: + result = inv_set_gyro_temp_slope(data); + break; + case INV_LOCAL_FIELD: + result = inv_set_local_field(data); + break; + case INV_MAG_SCALE: + result = inv_set_mag_scale(data); + break; + default: + return INV_ERROR_INVALID_PARAMETER; + break; + } + return result; +} + +/** + * @brief used to set an array of motion sensor data. + * Handles various data sets: + * - INV_GYRO_BIAS + * - INV_ACCEL_BIAS + * - INV_MAG_BIAS + * - INV_GYRO_TEMP_SLOPE + * + * Please refer to the provided "9-Axis Sensor Fusion Application + * Note" document provided. + * + * @pre MLDmpOpen() or + * MLDmpPedometerStandAloneOpen() + * @pre MLDmpStart() must NOT have been called. + * + * @param dataSet A constant specifying an array of data. + * @param data A pointer to an array to be copied from the user. + * + * @return INV_SUCCESS if successful; a non-zero error code otherwise. + */ +inv_error_t inv_set_float_array(int dataSet, float *data) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + + switch (dataSet) { + case INV_GYRO_TEMP_SLOPE: // internal + result = inv_set_gyro_temp_slope_float(data); + break; + case INV_GYRO_BIAS: // internal + result = inv_set_gyro_bias_float(data); + break; + case INV_ACCEL_BIAS: // internal + result = inv_set_accel_bias_float(data); + break; + case INV_MAG_BIAS: // internal + result = inv_set_mag_bias_float(data); + break; + case INV_LOCAL_FIELD: // internal + result = inv_set_local_field_float(data); + break; + case INV_MAG_SCALE: // internal + result = inv_set_mag_scale_float(data); + break; + default: + result = INV_ERROR_INVALID_PARAMETER; + break; + } + + return result; +} diff --git a/60xx/mlsdk/mllite/mlcontrol.c b/60xx/mlsdk/mllite/mlcontrol.c new file mode 100644 index 0000000..bd186fa --- /dev/null +++ b/60xx/mlsdk/mllite/mlcontrol.c @@ -0,0 +1,797 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/******************************************************************************* + * + * $Id: mlcontrol.c 5641 2011-06-14 02:10:02Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup CONTROL + * @brief Motion Library - Control Engine. + * The Control Library processes gyroscopes, accelerometers, and + * compasses to provide control signals that can be used in user + * interfaces. + * These signals can be used to manipulate objects such as documents, + * images, cursors, menus, etc. + * + * @{ + * @file mlcontrol.c + * @brief The Control Library. + * + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include "mltypes.h" +#include "mlinclude.h" +#include "mltypes.h" +#include "ml.h" +#include "mlos.h" +#include "mlsl.h" +#include "mldl.h" +#include "mlcontrol.h" +#include "dmpKey.h" +#include "mlstates.h" +#include "mlFIFO.h" +#include "string.h" + +/* - Global Vars. - */ +struct control_params cntrl_params = { + { + MLCTRL_SENSITIVITY_0_DEFAULT, + MLCTRL_SENSITIVITY_1_DEFAULT, + MLCTRL_SENSITIVITY_2_DEFAULT, + MLCTRL_SENSITIVITY_3_DEFAULT}, // sensitivity + MLCTRL_FUNCTIONS_DEFAULT, // functions + { + MLCTRL_PARAMETER_ARRAY_0_DEFAULT, + MLCTRL_PARAMETER_ARRAY_1_DEFAULT, + MLCTRL_PARAMETER_ARRAY_2_DEFAULT, + MLCTRL_PARAMETER_ARRAY_3_DEFAULT}, // parameterArray + { + MLCTRL_PARAMETER_AXIS_0_DEFAULT, + MLCTRL_PARAMETER_AXIS_1_DEFAULT, + MLCTRL_PARAMETER_AXIS_2_DEFAULT, + MLCTRL_PARAMETER_AXIS_3_DEFAULT}, // parameterAxis + { + MLCTRL_GRID_THRESHOLD_0_DEFAULT, + MLCTRL_GRID_THRESHOLD_1_DEFAULT, + MLCTRL_GRID_THRESHOLD_2_DEFAULT, + MLCTRL_GRID_THRESHOLD_3_DEFAULT}, // gridThreshold + { + MLCTRL_GRID_MAXIMUM_0_DEFAULT, + MLCTRL_GRID_MAXIMUM_1_DEFAULT, + MLCTRL_GRID_MAXIMUM_2_DEFAULT, + MLCTRL_GRID_MAXIMUM_3_DEFAULT}, // gridMaximum + MLCTRL_GRID_CALLBACK_DEFAULT // gridCallback +}; + +/* - Extern Vars. - */ +struct control_obj cntrl_obj; +extern const unsigned char *dmpConfig1; + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief inv_set_control_sensitivity is used to set the sensitivity for a control + * signal. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param sensitivity The sensitivity of the control signal. + * + * @return error code + */ +inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, + long sensitivity) +{ + INVENSENSE_FUNC_START; + unsigned char regs[2]; + long finalSens = 0; + inv_error_t result; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + finalSens = sensitivity * 100; + if (finalSens > 16384) { + finalSens = 16384; + } + regs[0] = (unsigned char)(finalSens / 256); + regs[1] = (unsigned char)(finalSens % 256); + switch (controlSignal) { + case INV_CONTROL_1: + result = inv_set_mpu_memory(KEY_D_0_224, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[0] = (unsigned short)sensitivity; + break; + case INV_CONTROL_2: + result = inv_set_mpu_memory(KEY_D_0_228, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[1] = (unsigned short)sensitivity; + break; + case INV_CONTROL_3: + result = inv_set_mpu_memory(KEY_D_0_232, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[2] = (unsigned short)sensitivity; + break; + case INV_CONTROL_4: + result = inv_set_mpu_memory(KEY_D_0_236, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.sensitivity[3] = (unsigned short)sensitivity; + break; + default: + break; + } + if (finalSens != sensitivity * 100) { + return INV_ERROR_INVALID_PARAMETER; + } else { + return INV_SUCCESS; + } +} + +/** + * @brief inv_set_control_func allows the user to choose how the sensor data will + * be processed in order to provide a control parameter. + * inv_set_control_func allows the user to choose which control functions + * will be incorporated in the sensor data processing. + * The control functions are: + * - INV_GRID + * Indicates that the user will be controlling a system that + * has discrete steps, such as icons, menu entries, pixels, etc. + * - INV_SMOOTH + * Indicates that noise from unintentional motion should be filtered out. + * - INV_DEAD_ZONE + * Indicates that a dead zone should be used, below which sensor + * data is set to zero. + * - INV_HYSTERESIS + * Indicates that, when INV_GRID is selected, hysteresis should + * be used to prevent the control signal from switching rapidly across + * elements of the grid. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param function Indicates what functions will be used. + * Can be a bitwise OR of several values. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_set_control_func(unsigned short function) +{ + INVENSENSE_FUNC_START; + unsigned char regs[8] = { DINA06, DINA26, + DINA46, DINA66, + DINA0E, DINA2E, + DINA4E, DINA6E + }; + unsigned char i; + inv_error_t result; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if ((function & INV_SMOOTH) == 0) { + for (i = 0; i < 8; i++) { + regs[i] = DINA80 + 3; + } + } + result = inv_set_mpu_memory(KEY_CFG_4, 8, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + cntrl_params.functions = function; + result = inv_set_dead_zone(); + + return result; +} + +/** + * @brief inv_get_control_signal is used to get the current control signal with + * high precision. + * inv_get_control_signal is used to acquire the current data of a control signal. + * If INV_GRID is being used, inv_get_grid_number will probably be preferrable. + * + * @param controlSignal Indicates which control signal is being queried. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param reset Indicates whether the control signal should be reset to zero. + * Options are INV_RESET or INV_NO_RESET + * @param data A pointer to the current control signal data. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ +inv_error_t inv_get_control_signal(unsigned short controlSignal, + unsigned short reset, long *data) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + *data = cntrl_obj.controlInt[0]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[0] = 0; + } + break; + case INV_CONTROL_2: + *data = cntrl_obj.controlInt[1]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[1] = 0; + } + break; + case INV_CONTROL_3: + *data = cntrl_obj.controlInt[2]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[2] = 0; + } + break; + case INV_CONTROL_4: + *data = cntrl_obj.controlInt[3]; + if (reset == INV_RESET) { + cntrl_obj.controlInt[3] = 0; + } + break; + default: + break; + } + return INV_SUCCESS; +} + +/** + * @brief inv_get_grid_num is used to get the current grid location for a certain + * control signal. + * inv_get_grid_num is used to acquire the current grid location. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being queried. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param reset Indicates whether the control signal should be reset to zero. + * Options are INV_RESET or INV_NO_RESET + * @param data A pointer to the current grid number. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_get_grid_num(unsigned short controlSignal, unsigned short reset, + long *data) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + *data = cntrl_obj.gridNum[0]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[0] = 0; + } + break; + case INV_CONTROL_2: + *data = cntrl_obj.gridNum[1]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[1] = 0; + } + break; + case INV_CONTROL_3: + *data = cntrl_obj.gridNum[2]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[2] = 0; + } + break; + case INV_CONTROL_4: + *data = cntrl_obj.gridNum[3]; + if (reset == INV_RESET) { + cntrl_obj.gridNum[3] = 0; + } + break; + default: + break; + } + + return INV_SUCCESS; +} + +/** + * @brief inv_set_grid_thresh is used to set the grid size for a control signal. + * inv_set_grid_thresh is used to adjust the size of the grid being controlled. + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 and + * - INV_CONTROL_4. + * @param threshold The threshold of the control signal at which the grid + * number will be incremented or decremented. + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_set_grid_thresh(unsigned short controlSignal, long threshold) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() < INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + cntrl_params.gridThreshold[0] = threshold; + break; + case INV_CONTROL_2: + cntrl_params.gridThreshold[1] = threshold; + break; + case INV_CONTROL_3: + cntrl_params.gridThreshold[2] = threshold; + break; + case INV_CONTROL_4: + cntrl_params.gridThreshold[3] = threshold; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + break; + } + + return INV_SUCCESS; +} + +/** + * @brief inv_set_grid_max is used to set the maximum grid number for a control signal. + * inv_set_grid_max is used to adjust the maximum allowed grid number, above + * which the grid number will not be incremented. + * The minimum grid number is always zero. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 and + * - INV_CONTROL_4. + * + * @param maximum The maximum grid number for a control signal. + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + switch (controlSignal) { + case INV_CONTROL_1: + cntrl_params.gridMaximum[0] = maximum; + break; + case INV_CONTROL_2: + cntrl_params.gridMaximum[1] = maximum; + break; + case INV_CONTROL_3: + cntrl_params.gridMaximum[2] = maximum; + break; + case INV_CONTROL_4: + cntrl_params.gridMaximum[3] = maximum; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + break; + } + + return INV_SUCCESS; +} + +/** + * @brief GridCallback function pointer type, to be passed as argument of + * inv_set_grid_callback. + * + * @param controlSignal Indicates which control signal crossed a grid threshold. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 and + * - INV_CONTROL_4. + * + * @param gridNumber An array of four numbers representing the grid number for each + * control signal. + * @param gridChange An array of four numbers representing the change in grid number + * for each control signal. +**/ +typedef void (*fpGridCb) (unsigned short controlSignal, long *gridNum, + long *gridChange); + +/** + * @brief inv_set_grid_callback is used to register a callback function that + * will trigger when the grid location changes. + * inv_set_grid_callback allows a user to define a callback function that will + * run when a control signal crosses a grid threshold. + + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). inv_dmp_start must NOT have + * been called. + * + * @param func A user defined callback function + * @return Zero if the command is successful; an ML error code otherwise. +**/ +inv_error_t inv_set_grid_callback(fpGridCb func) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + cntrl_params.gridCallback = func; + return INV_SUCCESS; +} + +/** + * @brief inv_set_control_data is used to assign physical parameters to control signals. + * inv_set_control_data allows flexibility in assigning physical parameters to + * control signals. For example, the user is allowed to use raw gyroscope data + * as an input to the control algorithm. + * Alternatively, angular velocity can be used, which combines gyroscopes and + * accelerometers to provide a more robust physical parameter. Finally, angular + * velocity in world coordinates can be used, providing a control signal in + * which pitch and yaw are provided relative to gravity. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being modified. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param parameterArray Indicates which parameter array is being assigned to a + * control signal. Must be one of: + * - INV_GYROS, + * - INV_ANGULAR_VELOCITY, or + * + * @param parameterAxis Indicates which axis of the parameter array will be used. + * Must be: + * - INV_ROLL, + * - INV_PITCH, or + * - INV_YAW. + */ + +inv_error_t inv_set_control_data(unsigned short controlSignal, + unsigned short parameterArray, + unsigned short parameterAxis) +{ + INVENSENSE_FUNC_START; + unsigned char regs[2] = { DINA80 + 10, DINA20 }; + inv_error_t result; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + if (parameterArray == INV_ANGULAR_VELOCITY) { + regs[0] = DINA80 + 5; + regs[1] = DINA00; + } + switch (controlSignal) { + case INV_CONTROL_1: + cntrl_params.parameterArray[0] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += 0x02; + cntrl_params.parameterAxis[0] = 0; + break; + case INV_ROLL: + regs[1] = DINA22; + cntrl_params.parameterAxis[0] = 1; + break; + case INV_YAW: + regs[1] = DINA42; + cntrl_params.parameterAxis[0] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case INV_CONTROL_2: + cntrl_params.parameterArray[1] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += DINA0E; + cntrl_params.parameterAxis[1] = 0; + break; + case INV_ROLL: + regs[1] += DINA2E; + cntrl_params.parameterAxis[1] = 1; + break; + case INV_YAW: + regs[1] += DINA4E; + cntrl_params.parameterAxis[1] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3B, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case INV_CONTROL_3: + cntrl_params.parameterArray[2] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += DINA0E; + cntrl_params.parameterAxis[2] = 0; + break; + case INV_ROLL: + regs[1] += DINA2E; + cntrl_params.parameterAxis[2] = 1; + break; + case INV_YAW: + regs[1] += DINA4E; + cntrl_params.parameterAxis[2] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3C, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case INV_CONTROL_4: + cntrl_params.parameterArray[3] = parameterArray; + switch (parameterAxis) { + case INV_PITCH: + regs[1] += DINA0E; + cntrl_params.parameterAxis[3] = 0; + break; + case INV_ROLL: + regs[1] += DINA2E; + cntrl_params.parameterAxis[3] = 1; + break; + case INV_YAW: + regs[1] += DINA4E; + cntrl_params.parameterAxis[3] = 2; + break; + default: + return INV_ERROR_INVALID_PARAMETER; + } + result = inv_set_mpu_memory(KEY_CFG_3D, 2, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + default: + result = INV_ERROR_INVALID_PARAMETER; + break; + } + return result; +} + +/** + * @brief inv_get_control_data is used to get the current control data. + * + * @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or + * inv_open_low_power_pedometer(). + * + * @param controlSignal Indicates which control signal is being queried. + * Must be one of: + * - INV_CONTROL_1, + * - INV_CONTROL_2, + * - INV_CONTROL_3 or + * - INV_CONTROL_4. + * + * @param gridNum A pointer to pass gridNum info back to the user. + * @param gridChange A pointer to pass gridChange info back to the user. + * + * @return Zero if the command is successful; an ML error code otherwise. + */ + +inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, + long *gridChange) +{ + INVENSENSE_FUNC_START; + int_fast8_t i = 0; + + if (inv_get_state() != INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + for (i = 0; i < 4; i++) { + controlSignal[i] = cntrl_obj.controlInt[i]; + gridNum[i] = cntrl_obj.gridNum[i]; + gridChange[i] = cntrl_obj.gridChange[i]; + } + return INV_SUCCESS; +} + +/** + * @internal + * @brief Update the ML Control engine. This function should be called + * every time new data from the MPU becomes available. + * Control engine outputs are written to the cntrl_obj data + * structure. + * @return INV_SUCCESS or an error code. +**/ +inv_error_t inv_update_control(struct inv_obj_t * inv_obj) +{ + INVENSENSE_FUNC_START; + unsigned char i; + long gridTmp; + long tmp; + + inv_get_cntrl_data(cntrl_obj.mlGridNumDMP); + + for (i = 0; i < 4; i++) { + if (cntrl_params.functions & INV_GRID) { + if (cntrl_params.functions & INV_HYSTERESIS) { + cntrl_obj.mlGridNumDMP[i] += cntrl_obj.gridNumOffset[i]; + } + cntrl_obj.mlGridNumDMP[i] = + cntrl_obj.mlGridNumDMP[i] / 2 + 1073741824L; + cntrl_obj.controlInt[i] = + (cntrl_obj.mlGridNumDMP[i] % + (128 * cntrl_params.gridThreshold[i])) / 128; + gridTmp = + cntrl_obj.mlGridNumDMP[i] / (128 * + cntrl_params.gridThreshold[i]); + tmp = 1 + 16777216L / cntrl_params.gridThreshold[i]; + cntrl_obj.gridChange[i] = gridTmp - cntrl_obj.lastGridNum[i]; + if (cntrl_obj.gridChange[i] > tmp / 2) { + cntrl_obj.gridChange[i] = + gridTmp - tmp - cntrl_obj.lastGridNum[i]; + } else if (cntrl_obj.gridChange[i] < -tmp / 2) { + cntrl_obj.gridChange[i] = + gridTmp + tmp - cntrl_obj.lastGridNum[i]; + } + if ((cntrl_params.functions & INV_HYSTERESIS) + && (cntrl_obj.gridChange[i] != 0)) { + if (cntrl_obj.gridChange[i] > 0) { + cntrl_obj.gridNumOffset[i] += + 128 * cntrl_params.gridThreshold[i]; + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + if (cntrl_obj.gridChange[i] < 0) { + cntrl_obj.gridNumOffset[i] -= + 128 * cntrl_params.gridThreshold[i]; + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + } + cntrl_obj.gridNum[i] += cntrl_obj.gridChange[i]; + if (cntrl_obj.gridNum[i] >= cntrl_params.gridMaximum[i]) { + cntrl_obj.gridNum[i] = cntrl_params.gridMaximum[i]; + if (cntrl_obj.controlInt[i] >= + cntrl_params.gridThreshold[i] / 2) { + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + } else if (cntrl_obj.gridNum[i] <= 0) { + cntrl_obj.gridNum[i] = 0; + if (cntrl_obj.controlInt[i] < cntrl_params.gridThreshold[i] / 2) { + cntrl_obj.controlInt[i] = cntrl_params.gridThreshold[i] / 2; + } + } + cntrl_obj.lastGridNum[i] = gridTmp; + if ((cntrl_params.gridCallback) && (cntrl_obj.gridChange[i] != 0)) { + cntrl_params.gridCallback((INV_CONTROL_1 << i), + cntrl_obj.gridNum, + cntrl_obj.gridChange); + } + + } else { + cntrl_obj.controlInt[i] = cntrl_obj.mlGridNumDMP[i]; + } + + } + + return INV_SUCCESS; +} + +/** + * @brief Enables the INV_CONTROL engine. + * + * @note This function replaces MLEnable(INV_CONTROL) + * + * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must + * have been called. + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_enable_control(void) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() != INV_STATE_DMP_OPENED) + return INV_ERROR_SM_IMPROPER_STATE; + + memset(&cntrl_obj, 0, sizeof(cntrl_obj)); + + inv_register_fifo_rate_process(inv_update_control, INV_PRIORITY_CONTROL); // fixme, someone needs to send control data to the fifo + return INV_SUCCESS; +} + +/** + * @brief Disables the INV_CONTROL engine. + * + * @note This function replaces MLDisable(INV_CONTROL) + * + * @pre inv_dmp_open() with MLDmpDefaultOpen or MLDmpPedometerStandAlone() must + * have been called. + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_disable_control(void) +{ + INVENSENSE_FUNC_START; + + if (inv_get_state() < INV_STATE_DMP_STARTED) + return INV_ERROR_SM_IMPROPER_STATE; + + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/mlcontrol.h b/60xx/mlsdk/mllite/mlcontrol.h new file mode 100644 index 0000000..a834fc6 --- /dev/null +++ b/60xx/mlsdk/mllite/mlcontrol.h @@ -0,0 +1,217 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $RCSfile: mlcontrol.h,v $ + * + * $Date: 2011-06-10 20:13:08 -0700 (Fri, 10 Jun 2011) $ + * + * $Revision: 5629 $ + * + *******************************************************************************/ + +/*******************************************************************************/ +/** @defgroup INV_CONTROL + + The Control processes gyroscopes and accelerometers to provide control + signals that can be used in user interfaces to manipulate objects such as + documents, images, cursors, menus, etc. + + @{ + @file mlcontrol.h + @brief Header file for the Control Library. +*/ +/******************************************************************************/ +#ifndef MLCONTROL_H +#define MLCONTROL_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#include "ml.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mlcontrol_legacy.h" +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /*******************************************************************************/ + /* Control Signals. */ + /*******************************************************************************/ + +#define INV_CONTROL_1 0x0001 +#define INV_CONTROL_2 0x0002 +#define INV_CONTROL_3 0x0004 +#define INV_CONTROL_4 0x0008 + + /*******************************************************************************/ + /* Control Functions. */ + /*******************************************************************************/ + +#define INV_GRID 0x0001 // Indicates that the user will be controlling a system that + // has discrete steps, such as icons, menu entries, pixels, etc. +#define INV_SMOOTH 0x0002 // Indicates that noise from unintentional motion should be filtered out. +#define INV_DEAD_ZONE 0x0004 // Indicates that a dead zone should be used, below which sensor data is set to zero. +#define INV_HYSTERESIS 0x0008 // Indicates that, when INV_GRID is selected, hysteresis should be used to prevent + // the control signal from switching rapidly across elements of the grid. + + /*******************************************************************************/ + /* Integral reset options. */ + /*******************************************************************************/ + +#define INV_NO_RESET 0x0000 +#define INV_RESET 0x0001 + + /*******************************************************************************/ + /* Data select options. */ + /*******************************************************************************/ + +#define INV_CTRL_SIGNAL 0x0000 +#define INV_CTRL_GRID_NUM 0x0001 + + /*******************************************************************************/ + /* Control Axis. */ + /*******************************************************************************/ +#define INV_CTRL_PITCH 0x0000 // (INV_PITCH >> 1) +#define INV_CTRL_ROLL 0x0001 // (INV_ROLL >> 1) +#define INV_CTRL_YAW 0x0002 // (INV_YAW >> 1) + + /*******************************************************************************/ + /* control_params structure default values. */ + /*******************************************************************************/ + +#define MLCTRL_SENSITIVITY_0_DEFAULT 128 +#define MLCTRL_SENSITIVITY_1_DEFAULT 128 +#define MLCTRL_SENSITIVITY_2_DEFAULT 128 +#define MLCTRL_SENSITIVITY_3_DEFAULT 128 +#define MLCTRL_FUNCTIONS_DEFAULT 0 +#define MLCTRL_CONTROL_SIGNALS_DEFAULT 0 +#define MLCTRL_PARAMETER_ARRAY_0_DEFAULT 0 +#define MLCTRL_PARAMETER_ARRAY_1_DEFAULT 0 +#define MLCTRL_PARAMETER_ARRAY_2_DEFAULT 0 +#define MLCTRL_PARAMETER_ARRAY_3_DEFAULT 0 +#define MLCTRL_PARAMETER_AXIS_0_DEFAULT 0 +#define MLCTRL_PARAMETER_AXIS_1_DEFAULT 0 +#define MLCTRL_PARAMETER_AXIS_2_DEFAULT 0 +#define MLCTRL_PARAMETER_AXIS_3_DEFAULT 0 +#define MLCTRL_GRID_THRESHOLD_0_DEFAULT 1 +#define MLCTRL_GRID_THRESHOLD_1_DEFAULT 1 +#define MLCTRL_GRID_THRESHOLD_2_DEFAULT 1 +#define MLCTRL_GRID_THRESHOLD_3_DEFAULT 1 +#define MLCTRL_GRID_MAXIMUM_0_DEFAULT 0 +#define MLCTRL_GRID_MAXIMUM_1_DEFAULT 0 +#define MLCTRL_GRID_MAXIMUM_2_DEFAULT 0 +#define MLCTRL_GRID_MAXIMUM_3_DEFAULT 0 +#define MLCTRL_GRID_CALLBACK_DEFAULT 0 + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /**************************************************************************/ + /* Control Parameters Structure. */ + /**************************************************************************/ + + struct control_params { + // Sensitivity of control signal 1, 2, 3, and 4. + unsigned short sensitivity[4]; + // Indicates what functions will be used. Can be a bitwise OR of INV_GRID, + // ML_SMOOT, INV_DEAD_ZONE, and INV_HYSTERISIS. + unsigned short functions; + // Indicates which parameter array is being assigned to a control signal. + // Must be one of INV_GYROS, INV_ANGULAR_VELOCITY, or + // INV_ANGULAR_VELOCITY_WORLD. + unsigned short parameterArray[4]; + // Indicates which axis of the parameter array will be used. Must be + // INV_ROLL, INV_PITCH, or INV_YAW. + unsigned short parameterAxis[4]; + // Threshold of the control signal at which the grid number will be + // incremented or decremented. + long gridThreshold[4]; + // Maximum grid number for the control signal. + long gridMaximum[4]; + // User defined callback that will trigger when the grid location changes. + void (*gridCallback) ( + // Indicates which control signal crossed a grid threshold. Must be + // one of INV_CONTROL_1, INV_CONTROL_2, INV_CONTROL_3 or INV_CONTROL_4. + unsigned short controlSignal, + // An array of four numbers representing the grid number for each + // control signal. + long *gridNum, + // An array of four numbers representing the change in grid number + // for each control signal. + long *gridChange); + }; + + struct control_obj { + + long gridNum[4]; // Current grid number for each control signal. + long controlInt[4]; // Current data for each control signal. + long lastGridNum[4]; // Previous grid number + unsigned char controlDir[4]; // Direction of control signal + long gridChange[4]; // Change in grid number + + long mlGridNumDMP[4]; + long gridNumOffset[4]; + long prevDMPGridNum[4]; + + }; + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + + /**************************************************************************/ + /* ML Control Functions. */ + /**************************************************************************/ + + unsigned short inv_get_control_params(struct control_params *params); + unsigned short inv_set_control_params(struct control_params *params); + + /*API for handling control signals */ + inv_error_t inv_set_control_sensitivity(unsigned short controlSignal, + long sensitivity); + inv_error_t inv_set_control_func(unsigned short function); + inv_error_t inv_get_control_signal(unsigned short controlSignal, + unsigned short reset, long *data); + inv_error_t inv_get_grid_num(unsigned short controlSignal, + unsigned short reset, long *data); + inv_error_t inv_set_grid_thresh(unsigned short controlSignal, + long threshold); + inv_error_t inv_set_grid_max(unsigned short controlSignal, long maximum); + inv_error_t + inv_set_grid_callback(void (*func) + (unsigned short controlSignal, long *gridNum, + long *gridChange)); + inv_error_t inv_set_control_data(unsigned short controlSignal, + unsigned short parameterArray, + unsigned short parameterNum); + inv_error_t inv_get_control_data(long *controlSignal, long *gridNum, + long *gridChange); + inv_error_t inv_update_control(struct inv_obj_t *inv_obj); + inv_error_t inv_enable_control(void); + inv_error_t inv_disable_control(void); + +#ifdef __cplusplus +} +#endif +#endif /* MLCONTROL_H */ diff --git a/60xx/mlsdk/mllite/mldl.c b/60xx/mlsdk/mllite/mldl.c new file mode 100644 index 0000000..7054532 --- /dev/null +++ b/60xx/mlsdk/mllite/mldl.c @@ -0,0 +1,1092 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/****************************************************************************** + * + * $Id: mldl.c 5653 2011-06-16 21:06:55Z nroyer $ + * + *****************************************************************************/ + +/** + * @defgroup MLDL + * @brief Motion Library - Driver Layer. + * The Motion Library Driver Layer provides the intrface to the + * system drivers that are used by the Motion Library. + * + * @{ + * @file mldl.c + * @brief The Motion Library Driver Layer. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include + +#include "mpu.h" +#if defined CONFIG_MPU_SENSORS_MPU6050A2 +# include "mpu6050a2.h" +#elif defined CONFIG_MPU_SENSORS_MPU6050B1 +# include "mpu6050b1.h" +#elif defined CONFIG_MPU_SENSORS_MPU3050 +# include "mpu3050.h" +#else +#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx +#endif +#include "mldl.h" +#include "mldl_cfg.h" +#include "compass.h" +#include "mlsl.h" +#include "mlos.h" +#include "mlinclude.h" +#include "ml.h" +#include "dmpKey.h" +#include "mlFIFOHW.h" +#include "compass.h" +#include "pressure.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mldl" + +#define _mldlDebug(x) //{x} + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + +#define MAX_LOAD_WRITE_SIZE (MPU_MEM_BANK_SIZE/2) /* 128 */ + +/*---- structure containing control variables used by MLDL ----*/ +static struct mldl_cfg mldlCfg; +struct ext_slave_descr gAccel; +struct ext_slave_descr gCompass; +struct ext_slave_descr gPressure; +struct mpu_platform_data gPdata; +static void *sMLSLHandle; +int_fast8_t intTrigger[NUM_OF_INTSOURCES]; + +/******************************************************************************* + * Functions for accessing the DMP memory via keys + ******************************************************************************/ + +unsigned short (*sGetAddress) (unsigned short key) = NULL; +static const unsigned char *localDmpMemory = NULL; +static unsigned short localDmpMemorySize = 0; + +/** + * @internal + * @brief Sets the function to use to convert keys to addresses. This + * will changed for each DMP code loaded. + * @param func + * Function used to convert keys to addresses. + * @endif + */ +void inv_set_get_address(unsigned short (*func) (unsigned short key)) +{ + INVENSENSE_FUNC_START; + _mldlDebug(MPL_LOGV("setGetAddress %d", (int)func); + ) + sGetAddress = func; +} + +/** + * @internal + * @brief Check if the feature is supported in the currently loaded + * DMP code basing on the fact that the key is assigned a + * value or not. + * @param key the DMP key + * @return whether the feature associated with the key is supported + * or not. + */ +uint_fast8_t inv_dmpkey_supported(unsigned short key) +{ + unsigned short memAddr; + + if (sGetAddress == NULL) { + MPL_LOGE("%s : sGetAddress is NULL\n", __func__); + return FALSE; + } + + memAddr = sGetAddress(key); + if (memAddr >= 0xffff) { + MPL_LOGV("inv_set_mpu_memory unsupported key\n"); + return FALSE; + } + + return TRUE; +} + +/** + * @internal + * @brief used to get the specified number of bytes from the original + * MPU memory location specified by the key. + * Reads the specified number of bytes from the MPU location + * that was used to program the MPU specified by the key. Each + * set of code specifies a function that changes keys into + * addresses. This function is set with setGetAddress(). + * + * @param key The key to use when looking up the address. + * @param length Number of bytes to read. + * @param buffer Result for data. + * + * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key + * not corresponding to a memory address will result in INV_ERROR. + * @endif + */ +inv_error_t inv_get_mpu_memory_original(unsigned short key, + unsigned short length, + unsigned char *buffer) +{ + unsigned short offset; + + if (sGetAddress == NULL) { + return INV_ERROR_NOT_OPENED; + } + + offset = sGetAddress(key); + if (offset >= localDmpMemorySize || (offset + length) > localDmpMemorySize) { + return INV_ERROR_INVALID_PARAMETER; + } + + memcpy(buffer, &localDmpMemory[offset], length); + + return INV_SUCCESS; +} + +unsigned short inv_dl_get_address(unsigned short key) +{ + unsigned short offset; + if (sGetAddress == NULL) { + return INV_ERROR_NOT_OPENED; + } + + offset = sGetAddress(key); + return offset; +} + +/* ---------------------- */ +/* - Static Functions. - */ +/* ---------------------- */ + +/** + * @brief Open the driver layer and resets the internal + * gyroscope, accelerometer, and compass data + * structures. + * @param mlslHandle + * the serial handle. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_dl_open(void *mlslHandle) +{ + inv_error_t result; + memset(&mldlCfg, 0, sizeof(mldlCfg)); + memset(intTrigger, INT_CLEAR, sizeof(intTrigger)); + + sMLSLHandle = mlslHandle; + + mldlCfg.addr = 0x68; /* default incase the driver doesn't set it */ + mldlCfg.accel = &gAccel; + mldlCfg.compass = &gCompass; + mldlCfg.pressure = &gPressure; + mldlCfg.pdata = &gPdata; + + result = (inv_error_t) inv_mpu_open(&mldlCfg, sMLSLHandle, + sMLSLHandle, sMLSLHandle, sMLSLHandle); + return result; +} + +/** + * @brief Closes/Cleans up the ML Driver Layer. + * Put the device in sleep mode. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_dl_close(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + result = (inv_error_t) inv_mpu_suspend(&mldlCfg, + sMLSLHandle, + sMLSLHandle, + sMLSLHandle, + sMLSLHandle, + INV_ALL_SENSORS); + + result = (inv_error_t) inv_mpu_close(&mldlCfg, sMLSLHandle, + sMLSLHandle, sMLSLHandle, sMLSLHandle); + /* Clear all previous settings */ + memset(&mldlCfg, 0, sizeof(mldlCfg)); + sMLSLHandle = NULL; + sGetAddress = NULL; + return result; +} + +/** + * @brief Sets the requested_sensors + * + * Accessor to set the requested_sensors field of the mldl_cfg structure. + * Typically set at initialization. + * + * @param sensors + * Bitfield of the sensors that are going to be used. Combination of the + * following: + * - INV_X_GYRO + * - INV_Y_GYRO + * - INV_Z_GYRO + * - INV_DMP_PROCESSOR + * - INV_X_ACCEL + * - INV_Y_ACCEL + * - INV_Z_ACCEL + * - INV_X_COMPASS + * - INV_Y_COMPASS + * - INV_Z_COMPASS + * - INV_X_PRESSURE + * - INV_Y_PRESSURE + * - INV_Z_PRESSURE + * - INV_THREE_AXIS_GYRO + * - INV_THREE_AXIS_ACCEL + * - INV_THREE_AXIS_COMPASS + * - INV_THREE_AXIS_PRESSURE + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_init_requested_sensors(unsigned long sensors) +{ + mldlCfg.requested_sensors = sensors; + + return INV_SUCCESS; +} + +/** + * @brief Starts the DMP running + * + * Resumes the sensor if any of the sensor axis or components are requested + * + * @param sensors + * Bitfield of the sensors to turn on. Combination of the following: + * - INV_X_GYRO + * - INV_Y_GYRO + * - INV_Z_GYRO + * - INV_DMP_PROCESSOR + * - INV_X_ACCEL + * - INV_Y_ACCEL + * - INV_Z_ACCEL + * - INV_X_COMPASS + * - INV_Y_COMPASS + * - INV_Z_COMPASS + * - INV_X_PRESSURE + * - INV_Y_PRESSURE + * - INV_Z_PRESSURE + * - INV_THREE_AXIS_GYRO + * - INV_THREE_AXIS_ACCEL + * - INV_THREE_AXIS_COMPASS + * - INV_THREE_AXIS_PRESSURE + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_dl_start(unsigned long sensors) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + mldlCfg.requested_sensors = sensors; + result = inv_mpu_resume(&mldlCfg, + sMLSLHandle, + sMLSLHandle, + sMLSLHandle, + sMLSLHandle, + sensors); + return result; +} + +/** + * @brief Stops the DMP running and puts it in low power as requested + * + * Suspends each sensor according to the bitfield, if all axis and components + * of the sensor is off. + * + * @param sensors Bitfiled of the sensors to leave on. Combination of the + * following: + * - INV_X_GYRO + * - INV_Y_GYRO + * - INV_Z_GYRO + * - INV_X_ACCEL + * - INV_Y_ACCEL + * - INV_Z_ACCEL + * - INV_X_COMPASS + * - INV_Y_COMPASS + * - INV_Z_COMPASS + * - INV_X_PRESSURE + * - INV_Y_PRESSURE + * - INV_Z_PRESSURE + * - INV_THREE_AXIS_GYRO + * - INV_THREE_AXIS_ACCEL + * - INV_THREE_AXIS_COMPASS + * - INV_THREE_AXIS_PRESSURE + * + * + * @return INV_SUCCESS or non-zero error code + */ +inv_error_t inv_dl_stop(unsigned long sensors) +{ + INVENSENSE_FUNC_START; + inv_error_t result = INV_SUCCESS; + + result = inv_mpu_suspend(&mldlCfg, + sMLSLHandle, + sMLSLHandle, + sMLSLHandle, + sMLSLHandle, + sensors); + return result; +} + +/** + * @brief Get a pointer to the internal data structure + * storing the configuration for the MPU, the accelerometer + * and the compass in use. + * @return a pointer to the data structure of type 'struct mldl_cfg'. + */ +struct mldl_cfg *inv_get_dl_config(void) +{ + return &mldlCfg; +} + +/** + * @brief Query the MPU slave address. + * @return The 7-bit mpu slave address. + */ +unsigned char inv_get_mpu_slave_addr(void) +{ + INVENSENSE_FUNC_START; + return mldlCfg.addr; +} + +/** + * @internal + * @brief MLDLCfgDMP configures the Digital Motion Processor internal to + * the MPU. The DMP can be enabled or disabled and the start address + * can be set. + * + * @param enableRun Enables the DMP processing if set to TRUE. + * @param enableFIFO Enables DMP output to the FIFO if set to TRUE. + * @param startAddress start address + * + * @return Zero if the command is successful, an error code otherwise. +*/ +inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun, + unsigned char enableFIFO) +{ + INVENSENSE_FUNC_START; + + mldlCfg.dmp_enable = enableRun; + mldlCfg.fifo_enable = enableFIFO; + mldlCfg.gyro_needs_reset = TRUE; + + return INV_SUCCESS; +} + +/** + * @brief inv_get_dl_cfg_int configures the interrupt function on the specified pin. + * The basic interrupt signal characteristics can be set + * (i.e. active high/low, open drain/push pull, etc.) and the + * triggers can be set. + * Currently only INTPIN_MPU is supported. + * + * @param triggers + * bitmask of triggers to enable for interrupt. + * The available triggers are: + * - BIT_MPU_RDY_EN + * - BIT_DMP_INT_EN + * - BIT_RAW_RDY_EN + * + * @return Zero if the command is successful, an error code otherwise. +*/ +inv_error_t inv_get_dl_cfg_int(unsigned char triggers) +{ + inv_error_t result = INV_SUCCESS; + +#if defined CONFIG_MPU_SENSORS_MPU3050 + /* Mantis has 8 bits of interrupt config bits */ + if (triggers & !(BIT_MPU_RDY_EN | BIT_DMP_INT_EN | BIT_RAW_RDY_EN)) { + return INV_ERROR_INVALID_PARAMETER; + } +#endif + + mldlCfg.int_config = triggers; + if (!mldlCfg.gyro_is_suspended) { + result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, + MPUREG_INT_CFG, + (mldlCfg.int_config | mldlCfg.pdata-> + int_config)); + } else { + mldlCfg.gyro_needs_reset = TRUE; + } + + return result; +} + +/** + * @brief configures the output sampling rate on the MPU. + * Three parameters control the sampling: + * + * 1) Low pass filter bandwidth, and + * 2) output sampling divider. + * + * The output sampling rate is determined by the divider and the low + * pass filter setting. If the low pass filter is set to + * 'MPUFILTER_256HZ_NOLPF2', then the sample rate going into the + * divider is 8kHz; for all other settings it is 1kHz. + * The 8-bit divider will divide this frequency to get the resulting + * sample frequency. + * For example, if the filter setting is not 256Hz and the divider is + * set to 7, then the sample rate is as follows: + * sample rate = internal sample rate / div = 1kHz / 8 = 125Hz (or 8ms). + * + * The low pass filter selection codes control both the cutoff frequency of + * the internal low pass filter and internal analog sampling rate. The + * latter, in turn, affects the final output sampling rate according to the + * sample rate divider settig. + * 0 -> 256 Hz cutoff BW, 8 kHz analog sample rate, + * 1 -> 188 Hz cutoff BW, 1 kHz analog sample rate, + * 2 -> 98 Hz cutoff BW, 1 kHz analog sample rate, + * 3 -> 42 Hz cutoff BW, 1 kHz analog sample rate, + * 4 -> 20 Hz cutoff BW, 1 kHz analog sample rate, + * 5 -> 10 Hz cutoff BW, 1 kHz analog sample rate, + * 6 -> 5 Hz cutoff BW, 1 kHz analog sample rate, + * 7 -> 2.1 kHz cutoff BW, 8 kHz analog sample rate. + * + * @param lpf low pass filter, 0 to 7. + * @param divider Output sampling rate divider, 0 to 255. + * + * @return ML_SUCESS if successful; a non-zero error code otherwise. +**/ +inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider) +{ + /*---- do range checking ----*/ + if (lpf >= NUM_MPU_FILTER) { + return INV_ERROR_INVALID_PARAMETER; + } + + mldlCfg.lpf = lpf; + mldlCfg.divider = divider; + mldlCfg.gyro_needs_reset = TRUE; + + return INV_SUCCESS; +} + +/** + * @brief set the full scale range for the gyros. + * The full scale selection codes correspond to: + * 0 -> 250 dps, + * 1 -> 500 dps, + * 2 -> 1000 dps, + * 3 -> 2000 dps. + * Full scale range affect the MPU's measurement + * sensitivity. + * + * @param fullScale + * the gyro full scale range in dps. + * + * @return INV_SUCCESS or non-zero error code. +**/ +inv_error_t inv_set_full_scale(float fullScale) +{ + if (fullScale == 250.f) + mldlCfg.full_scale = MPU_FS_250DPS; + else if (fullScale == 500.f) + mldlCfg.full_scale = MPU_FS_500DPS; + else if (fullScale == 1000.f) + mldlCfg.full_scale = MPU_FS_1000DPS; + else if (fullScale == 2000.f) + mldlCfg.full_scale = MPU_FS_2000DPS; + else { // not a valid setting + MPL_LOGE("Invalid full scale range specification for gyros : %f\n", + fullScale); + MPL_LOGE + ("\tAvailable values : +/- 250 dps, +/- 500 dps, +/- 1000 dps, +/- 2000 dps\n"); + return INV_ERROR_INVALID_PARAMETER; + } + mldlCfg.gyro_needs_reset = TRUE; + + return INV_SUCCESS; +} + +/** + * @brief This function sets the external sync for the MPU sampling. + * It can be synchronized on the LSB of any of the gyros, any of the + * external accels, or on the temp readings. + * + * @param extSync External sync selection, 0 to 7. + * @return Zero if the command is successful; an error code otherwise. +**/ +inv_error_t inv_set_external_sync(unsigned char extSync) +{ + INVENSENSE_FUNC_START; + + /*---- do range checking ----*/ + if (extSync >= NUM_MPU_EXT_SYNC) { + return INV_ERROR_INVALID_PARAMETER; + } + mldlCfg.ext_sync = extSync; + mldlCfg.gyro_needs_reset = TRUE; + + return INV_SUCCESS; +} + +inv_error_t inv_set_ignore_system_suspend(unsigned char ignore) +{ + INVENSENSE_FUNC_START; + + mldlCfg.ignore_system_suspend = ignore; + + return INV_SUCCESS; +} + +/** + * @brief inv_clock_source function sets the clock source for the MPU gyro + * processing. + * The source can be any of the following: + * - Internal 8MHz oscillator, + * - PLL with X gyro as reference, + * - PLL with Y gyro as reference, + * - PLL with Z gyro as reference, + * - PLL with external 32.768Mhz reference, or + * - PLL with external 19.2MHz reference + * + * For best accuracy and timing, it is highly recommended to use one + * of the gyros as the clock source; however this gyro must be + * enabled to use its clock (see 'MLDLPowerMgmtMPU()'). + * + * @param clkSource Clock source selection. + * Can be one of: + * - CLK_INTERNAL, + * - CLK_PLLGYROX, + * - CLK_PLLGYROY, + * - CLK_PLLGYROZ, + * - CLK_PLLEXT32K, or + * - CLK_PLLEXT19M. + * + * @return Zero if the command is successful; an error code otherwise. +**/ +inv_error_t inv_clock_source(unsigned char clkSource) +{ + INVENSENSE_FUNC_START; + + /*---- do range checking ----*/ + if (clkSource >= NUM_CLK_SEL) { + return INV_ERROR_INVALID_PARAMETER; + } + + mldlCfg.clk_src = clkSource; + mldlCfg.gyro_needs_reset = TRUE; + + return INV_SUCCESS; +} + +/** + * @brief Set the Temperature Compensation offset. + * @param tc + * a pointer to the temperature compensations offset + * for the 3 gyro axes. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_set_offsetTC(const unsigned char *tc) +{ + int ii; + inv_error_t result; + + for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset_tc); ii++) { + mldlCfg.offset_tc[ii] = tc[ii]; + } + + if (!mldlCfg.gyro_is_suspended) { +#ifdef CONFIG_MPU_SENSORS_MPU3050 + result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, + MPUREG_XG_OFFS_TC, tc[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, + MPUREG_YG_OFFS_TC, tc[1]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, + MPUREG_ZG_OFFS_TC, tc[2]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#else + unsigned char reg; + result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, + MPUREG_XG_OFFS_TC, + ((mldlCfg.offset_tc[0] << 1) + & BITS_XG_OFFS_TC)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + reg = ((mldlCfg.offset_tc[1] << 1) & BITS_YG_OFFS_TC); +#ifdef CONFIG_MPU_SENSORS_MPU6050B1 + if (mldlCfg.pdata->level_shifter) + reg |= BIT_I2C_MST_VDDIO; +#endif + result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, + MPUREG_YG_OFFS_TC, reg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write(sMLSLHandle, mldlCfg.addr, + MPUREG_ZG_OFFS_TC, + ((mldlCfg.offset_tc[2] << 1) + & BITS_ZG_OFFS_TC)); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#endif + } else { + mldlCfg.gyro_needs_reset = TRUE; + } + return INV_SUCCESS; +} + +/** + * @brief Set the gyro offset. + * @param offset + * a pointer to the gyro offset for the 3 gyro axes. This is scaled + * as it would be written to the hardware registers. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_set_offset(const short *offset) +{ + inv_error_t result; + unsigned char regs[7]; + int ii; + long sf; + + sf = (2000L * 131) / mldlCfg.gyro_sens_trim; + for (ii = 0; ii < ARRAY_SIZE(mldlCfg.offset); ii++) { + // Record the bias in the units the register uses + mldlCfg.offset[ii] = offset[ii]; + // Convert the bias to 1 dps = 1<<16 + inv_obj.gyro_bias[ii] = -offset[ii] * sf; + regs[1 + ii * 2] = (unsigned char)(offset[ii] >> 8) & 0xff; + regs[1 + ii * 2 + 1] = (unsigned char)(offset[ii] & 0xff); + } + + if (!mldlCfg.gyro_is_suspended) { + regs[0] = MPUREG_X_OFFS_USRH; + result = inv_serial_write(sMLSLHandle, mldlCfg.addr, 7, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else { + mldlCfg.gyro_needs_reset = TRUE; + } + return INV_SUCCESS; +} + +/** + * @internal + * @brief used to get the specified number of bytes in the specified MPU + * memory bank. + * The memory bank is one of the following: + * - MPUMEM_RAM_BANK_0, + * - MPUMEM_RAM_BANK_1, + * - MPUMEM_RAM_BANK_2, or + * - MPUMEM_RAM_BANK_3. + * + * @param bank Memory bank to write. + * @param memAddr Starting address for write. + * @param length Number of bytes to write. + * @param buffer Result for data. + * + * @return zero if the command is successful, an error code otherwise. + * @endif + */ +inv_error_t +inv_get_mpu_memory_one_bank(unsigned char bank, + unsigned char memAddr, + unsigned short length, unsigned char *buffer) +{ + inv_error_t result; + + if ((bank >= MPU_MEM_NUM_RAM_BANKS) || + //(memAddr >= MPU_MEM_BANK_SIZE) || always 0, memAddr is an u_char, therefore limited to 255 + ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { + return INV_ERROR_INVALID_PARAMETER; + } + + if (mldlCfg.gyro_is_suspended) { + memcpy(buffer, &mldlCfg.ram[bank][memAddr], length); + result = INV_SUCCESS; + } else { + result = inv_serial_read_mem(sMLSLHandle, mldlCfg.addr, + ((bank << 8) | memAddr), length, buffer); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + return result; +} + +/** + * @internal + * @brief used to set the specified number of bytes in the specified MPU + * memory bank. + * The memory bank is one of the following: + * - MPUMEM_RAM_BANK_0, + * - MPUMEM_RAM_BANK_1, + * - MPUMEM_RAM_BANK_2, or + * - MPUMEM_RAM_BANK_3. + * + * @param bank Memory bank to write. + * @param memAddr Starting address for write. + * @param length Number of bytes to write. + * @param buffer Result for data. + * + * @return zero if the command is successful, an error code otherwise. + * @endif + */ +inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, + unsigned short memAddr, + unsigned short length, + const unsigned char *buffer) +{ + inv_error_t result = INV_SUCCESS; + int different; + + if ((bank >= MPU_MEM_NUM_RAM_BANKS) || (memAddr >= MPU_MEM_BANK_SIZE) || + ((memAddr + length) > MPU_MEM_BANK_SIZE) || (NULL == buffer)) { + return INV_ERROR_INVALID_PARAMETER; + } + + different = memcmp(&mldlCfg.ram[bank][memAddr], buffer, length); + memcpy(&mldlCfg.ram[bank][memAddr], buffer, length); + if (!mldlCfg.gyro_is_suspended) { + result = inv_serial_write_mem(sMLSLHandle, mldlCfg.addr, + ((bank << 8) | memAddr), length, buffer); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else if (different) { + mldlCfg.gyro_needs_reset = TRUE; + } + + return result; +} + +/** + * @internal + * @brief used to get the specified number of bytes from the MPU location + * specified by the key. + * Reads the specified number of bytes from the MPU location + * specified by the key. Each set of code specifies a function + * that changes keys into addresses. This function is set with + * setGetAddress(). + * + * @param key The key to use when looking up the address. + * @param length Number of bytes to read. + * @param buffer Result for data. + * + * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key + * not corresponding to a memory address will result in INV_ERROR. + * @endif + */ +inv_error_t inv_get_mpu_memory(unsigned short key, + unsigned short length, unsigned char *buffer) +{ + unsigned char bank; + inv_error_t result; + unsigned short memAddr; + + if (sGetAddress == NULL) { + return INV_ERROR_NOT_OPENED; + } + + memAddr = sGetAddress(key); + if (memAddr >= 0xffff) + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; + bank = memAddr >> 8; // Get Bank + memAddr &= 0xff; + + while (memAddr + length > MPU_MEM_BANK_SIZE) { + // We cross a bank in the middle + unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; + result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, + sub_length, buffer); + if (INV_SUCCESS != result) + return result; + bank++; + length -= sub_length; + buffer += sub_length; + memAddr = 0; + } + result = inv_get_mpu_memory_one_bank(bank, (unsigned char)memAddr, + length, buffer); + + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/** + * @internal + * @brief used to set the specified number of bytes from the MPU location + * specified by the key. + * Set the specified number of bytes from the MPU location + * specified by the key. Each set of DMP code specifies a function + * that changes keys into addresses. This function is set with + * setGetAddress(). + * + * @param key The key to use when looking up the address. + * @param length Number of bytes to write. + * @param buffer Result for data. + * + * @return INV_SUCCESS if the command is successful, INV_ERROR otherwise. The key + * not corresponding to a memory address will result in INV_ERROR. + * @endif + */ +inv_error_t inv_set_mpu_memory(unsigned short key, + unsigned short length, + const unsigned char *buffer) +{ + inv_error_t result = INV_SUCCESS; + unsigned short memAddr; + unsigned char bank; + + if (sGetAddress == NULL) { + MPL_LOGE("MLDSetMemoryMPU sGetAddress is NULL\n"); + return INV_ERROR_INVALID_MODULE; + } + memAddr = sGetAddress(key); + + if (memAddr >= 0xffff) { + MPL_LOGE("inv_set_mpu_memory unsupported key\n"); + return INV_ERROR_INVALID_MODULE; // This key not supported + } + + bank = (unsigned char)(memAddr >> 8); + memAddr &= 0xff; + + while (memAddr + length > MPU_MEM_BANK_SIZE) { + // We cross a bank in the middle + unsigned short sub_length = MPU_MEM_BANK_SIZE - memAddr; + + result = inv_set_mpu_memory_one_bank(bank, memAddr, sub_length, buffer); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + bank++; + length -= sub_length; + buffer += sub_length; + memAddr = 0; + } + result = inv_set_mpu_memory_one_bank(bank, memAddr, length, buffer); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * @brief Load the DMP with the given code and configuration. + * @param buffer + * the DMP data. + * @param length + * the length in bytes of the DMP data. + * @param config + * the DMP configuration. + * @return INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_load_dmp(const unsigned char *buffer, + unsigned short length, unsigned short config) +{ + INVENSENSE_FUNC_START; + + inv_error_t result = INV_SUCCESS; + unsigned short toWrite; + unsigned short memAddr = 0; + localDmpMemory = buffer; + localDmpMemorySize = length; + + mldlCfg.dmp_cfg1 = (config >> 8); + mldlCfg.dmp_cfg2 = (config & 0xff); + + while (length > 0) { + toWrite = length; + if (toWrite > MAX_LOAD_WRITE_SIZE) + toWrite = MAX_LOAD_WRITE_SIZE; + + result = + inv_set_mpu_memory_one_bank(memAddr >> 8, memAddr & 0xff, toWrite, + buffer); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + buffer += toWrite; + memAddr += toWrite; + length -= toWrite; + } + + return result; +} + +/** + * @brief Get the silicon revision ID. + * @return The silicon revision ID + * (0 will be read if inv_mpu_open returned an error) + */ +unsigned char inv_get_silicon_rev(void) +{ + return mldlCfg.silicon_revision; +} + +/** + * @brief Get the product revision ID. + * @return The product revision ID + * (0 will be read if inv_mpu_open returned an error) + */ +unsigned char inv_get_product_rev(void) +{ + return mldlCfg.product_revision; +} + +/******************************************************************************* + ******************************************************************************* + ******************************************************************************* + * @todo these belong with an interface to the kernel driver layer + ******************************************************************************* + ******************************************************************************* + ******************************************************************************/ + +/** + * @brief inv_get_interrupt_status returns the interrupt status from the specified + * interrupt pin. + * @param intPin + * Currently only the value INTPIN_MPU is supported. + * @param status + * The available statuses are: + * - BIT_MPU_RDY_EN + * - BIT_DMP_INT_EN + * - BIT_RAW_RDY_EN + * + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t inv_get_interrupt_status(unsigned char intPin, + unsigned char *status) +{ + INVENSENSE_FUNC_START; + + inv_error_t result; + + switch (intPin) { + + case INTPIN_MPU: + /*---- return the MPU interrupt status ----*/ + result = inv_serial_read(sMLSLHandle, mldlCfg.addr, + MPUREG_INT_STATUS, 1, status); + break; + + default: + result = INV_ERROR_INVALID_PARAMETER; + break; + } + + return result; +} + +/** + * @brief query the current status of an interrupt source. + * @param srcIndex + * index of the interrupt source. + * Currently the only source supported is INTPIN_MPU. + * + * @return 1 if the interrupt has been triggered. + */ +unsigned char inv_get_interrupt_trigger(unsigned char srcIndex) +{ + INVENSENSE_FUNC_START; + return intTrigger[srcIndex]; +} + +/** + * @brief clear the 'triggered' status for an interrupt source. + * @param srcIndex + * index of the interrupt source. + * Currently only INTPIN_MPU is supported. + */ +void inv_clear_interrupt_trigger(unsigned char srcIndex) +{ + INVENSENSE_FUNC_START; + intTrigger[srcIndex] = 0; +} + +/** + * @brief inv_interrupt_handler function should be called when an interrupt is + * received. The source parameter identifies which interrupt source + * caused the interrupt. Note that this routine should not be called + * directly from the interrupt service routine. + * + * @param intSource MPU, AUX1, AUX2, or timer. Can be one of: INTSRC_MPU, INTSRC_AUX1, + * INTSRC_AUX2, or INT_SRC_TIMER. + * + * @return Zero if the command is successful; an error code otherwise. + */ +inv_error_t inv_interrupt_handler(unsigned char intSource) +{ + INVENSENSE_FUNC_START; + /*---- range check ----*/ + if (intSource >= NUM_OF_INTSOURCES) { + return INV_ERROR; + } + + /*---- save source of interrupt ----*/ + intTrigger[intSource] = INT_TRIGGERED; + +#ifdef ML_USE_DMP_SIM + if (intSource == INTSRC_AUX1 || intSource == INTSRC_TIMER) { + MLSimHWDataInput(); + } +#endif + + return INV_SUCCESS; +} + +/***************************/ + /**@}*//* end of defgroup */ +/***************************/ diff --git a/60xx/mlsdk/mllite/mldl.h b/60xx/mlsdk/mllite/mldl.h new file mode 100644 index 0000000..961d860 --- /dev/null +++ b/60xx/mlsdk/mllite/mldl.h @@ -0,0 +1,176 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mldl.h 5639 2011-06-14 01:23:05Z nroyer $ + * + *******************************************************************************/ + +#ifndef MLDL_H +#define MLDL_H + +#include "mltypes.h" +#include "mlsl.h" +#include +#include "mldl_cfg.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mldl_legacy.h" +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + +typedef enum _DEVICE_CONFIG { + DEVICE_MPU_ACCEL = 0, + DEVICE_MPU, + NUM_OF_DEVICES +} DEVICE_CONFIG; + +#define SERIAL_I2C 0 +#define SERIAL_SPI 1 + +#define DATAMODE_MANUAL 0 // Manual data mode +#define DATAMODE_AUTO 1 // Auto data mode + +#define DATASRC_IMMEDIATE 0 // Return data immediately +#define DATASRC_WHENREADY 1 // Only return data when new data is available +#define DATASRC_FIFO 2 // Use FIFO for data + +#define SENSOR_DATA_GYROX 0x0001 +#define SENSOR_DATA_GYROY 0x0002 +#define SENSOR_DATA_GYROZ 0x0004 +#define SENSOR_DATA_ACCELX 0x0008 +#define SENSOR_DATA_ACCELY 0x0010 +#define SENSOR_DATA_ACCELZ 0x0020 +#define SENSOR_DATA_AUX1 0x0040 +#define SENSOR_DATA_AUX2 0x0080 +#define SENSOR_DATA_AUX3 0x0100 +#define SENSOR_DATA_TEMP 0x0200 + +#define INTPIN_MPU 0 + +#define INTLOGIC_HIGH 0 +#define INTLOGIC_LOW 1 + +#define INTTYPE_PUSHPULL 0 +#define INTTYPE_OPENDRAIN 1 + +#define INTLATCH_DISABLE 0 +#define INTLATCH_ENABLE 1 + +#define MPUINT_MPU_READY 0x04 +#define MPUINT_DMP_DONE 0x02 +#define MPUINT_DATA_READY 0x01 + +#define INTLATCHCLEAR_READSTATUS 0 +#define INTLATCHCLEAR_ANYREAD 1 + +#define DMP_DONTRUN 0 +#define DMP_RUN 1 + + /*---- defines for external interrupt status (via external call into library) ----*/ +#define INT_CLEAR 0 +#define INT_TRIGGERED 1 + +typedef enum { + INTSRC_MPU = 0, + INTSRC_AUX1, + INTSRC_AUX2, + INTSRC_AUX3, + INTSRC_TIMER, + INTSRC_UNKNOWN, + INTSRC_MPU_FIFO, + INTSRC_MPU_MOTION, + NUM_OF_INTSOURCES, +} INT_SOURCE; + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------- */ + /* - Variables. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_dl_open(void *mlslHandle); + inv_error_t inv_dl_close(void); + + inv_error_t inv_dl_start(unsigned long sensors); + inv_error_t inv_dl_stop(unsigned long sensors); + + struct mldl_cfg *inv_get_dl_config(void); + + inv_error_t inv_init_requested_sensors(unsigned long sensors); + unsigned char inv_get_mpu_slave_addr(void); + inv_error_t inv_get_dl_ctrl_dmp(unsigned char enableRun, + unsigned char enableFIFO); + inv_error_t inv_get_dl_cfg_int(unsigned char triggers); + inv_error_t inv_dl_cfg_sampling(unsigned char lpf, unsigned char divider); + inv_error_t inv_set_full_scale(float fullScale); + inv_error_t inv_set_external_sync(unsigned char extSync); + inv_error_t inv_set_ignore_system_suspend(unsigned char ignore); + inv_error_t inv_clock_source(unsigned char clkSource); + inv_error_t inv_get_mpu_memory(unsigned short key, + unsigned short length, + unsigned char *buffer); + inv_error_t inv_set_mpu_memory(unsigned short key, + unsigned short length, + const unsigned char *buffer); + inv_error_t inv_load_dmp(const unsigned char *buffer, + unsigned short length, + unsigned short startAddress); + + unsigned char inv_get_slicon_rev(void); + inv_error_t inv_set_offsetTC(const unsigned char *tc); + inv_error_t inv_set_offset(const short *offset); + + /* Functions for setting and retrieving the DMP memory */ + inv_error_t inv_get_mpu_memory_original(unsigned short key, + unsigned short length, + unsigned char *buffer); + void inv_set_get_address(unsigned short (*func) (unsigned short key)); + unsigned short inv_dl_get_address(unsigned short key); + uint_fast8_t inv_dmpkey_supported(unsigned short key); + + inv_error_t inv_get_interrupt_status(unsigned char intPin, + unsigned char *value); + unsigned char inv_get_interrupt_trigger(unsigned char index); + void inv_clear_interrupt_trigger(unsigned char index); + inv_error_t inv_interrupt_handler(unsigned char intSource); + + /** Only exposed for testing purposes */ + inv_error_t inv_set_mpu_memory_one_bank(unsigned char bank, + unsigned short memAddr, + unsigned short length, + const unsigned char *buffer); + inv_error_t inv_get_mpu_memory_one_bank(unsigned char bank, + unsigned char memAddr, + unsigned short length, + unsigned char *buffer); +#ifdef __cplusplus +} +#endif +#endif // MLDL_H diff --git a/60xx/mlsdk/mllite/mldl_cfg.h b/60xx/mlsdk/mllite/mldl_cfg.h new file mode 100644 index 0000000..b4914e2 --- /dev/null +++ b/60xx/mlsdk/mllite/mldl_cfg.h @@ -0,0 +1,336 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/** + * @addtogroup MLDL + * + * @{ + * @file mldl_cfg.h + * @brief The Motion Library Driver Layer Configuration header file. + */ + +#ifndef __MLDL_CFG_H__ +#define __MLDL_CFG_H__ + +#include "mltypes.h" +#include "mlsl.h" +#include +#if defined CONFIG_MPU_SENSORS_MPU6050A2 +# include "mpu6050a2.h" +#elif defined CONFIG_MPU_SENSORS_MPU6050B1 +# include "mpu6050b1.h" +#elif defined CONFIG_MPU_SENSORS_MPU3050 +# include "mpu3050.h" +#else +#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx +#endif + +#include "log.h" + +/************************************************************************* + * Sensors + *************************************************************************/ + +#define INV_X_GYRO (0x0001) +#define INV_Y_GYRO (0x0002) +#define INV_Z_GYRO (0x0004) +#define INV_DMP_PROCESSOR (0x0008) + +#define INV_X_ACCEL (0x0010) +#define INV_Y_ACCEL (0x0020) +#define INV_Z_ACCEL (0x0040) + +#define INV_X_COMPASS (0x0080) +#define INV_Y_COMPASS (0x0100) +#define INV_Z_COMPASS (0x0200) + +#define INV_X_PRESSURE (0x0300) +#define INV_Y_PRESSURE (0x0800) +#define INV_Z_PRESSURE (0x1000) + +#define INV_TEMPERATURE (0x2000) +#define INV_TIME (0x4000) + +#define INV_THREE_AXIS_GYRO (0x000F) +#define INV_THREE_AXIS_ACCEL (0x0070) +#define INV_THREE_AXIS_COMPASS (0x0380) +#define INV_THREE_AXIS_PRESSURE (0x1C00) + +#define INV_FIVE_AXIS (0x007B) +#define INV_SIX_AXIS_GYRO_ACCEL (0x007F) +#define INV_SIX_AXIS_ACCEL_COMPASS (0x03F0) +#define INV_NINE_AXIS (0x03FF) +#define INV_ALL_SENSORS (0x7FFF) + +#define MPL_PROD_KEY(ver, rev) (ver * 100 + rev) + +/* -------------------------------------------------------------------------- */ + +/* Platform data for the MPU */ +struct mldl_cfg { + /* MPU related configuration */ + unsigned long requested_sensors; + unsigned char ignore_system_suspend; + unsigned char addr; + unsigned char int_config; + unsigned char ext_sync; + unsigned char full_scale; + unsigned char lpf; + unsigned char clk_src; + unsigned char divider; + unsigned char dmp_enable; + unsigned char fifo_enable; + unsigned char dmp_cfg1; + unsigned char dmp_cfg2; + unsigned char offset_tc[GYRO_NUM_AXES]; + unsigned short offset[GYRO_NUM_AXES]; + unsigned char ram[MPU_MEM_NUM_RAM_BANKS][MPU_MEM_BANK_SIZE]; + + /* MPU Related stored status and info */ + unsigned char product_revision; + unsigned char silicon_revision; + unsigned char product_id; + unsigned short gyro_sens_trim; +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + unsigned short accel_sens_trim; +#endif + + /* Driver/Kernel related state information */ + int gyro_is_bypassed; + int i2c_slaves_enabled; + int dmp_is_running; + int gyro_is_suspended; + int accel_is_suspended; + int compass_is_suspended; + int pressure_is_suspended; + int gyro_needs_reset; + + /* Slave related information */ + struct ext_slave_descr *accel; + struct ext_slave_descr *compass; + struct ext_slave_descr *pressure; + + /* Platform Data */ + struct mpu_platform_data *pdata; +}; + +/* -------------------------------------------------------------------------- */ + +int inv_mpu_open(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle); +int inv_mpu_close(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle); +int inv_mpu_resume(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + unsigned long sensors); +int inv_mpu_suspend(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + unsigned long sensors); + +/* Slave Read functions */ +int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data); +static inline int inv_mpu_read_accel(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, unsigned char *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_slave_read(mldl_cfg, gyro_handle, accel_handle, + mldl_cfg->accel, &mldl_cfg->pdata->accel, + data); +} + +static inline int inv_mpu_read_compass(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *compass_handle, + unsigned char *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_slave_read(mldl_cfg, gyro_handle, compass_handle, + mldl_cfg->compass, &mldl_cfg->pdata->compass, + data); +} + +static inline int inv_mpu_read_pressure(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *pressure_handle, + unsigned char *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_slave_read(mldl_cfg, gyro_handle, pressure_handle, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure, data); +} + +/* Slave Config functions */ +int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_config *data, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); +static inline int inv_mpu_config_accel(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + struct ext_slave_config *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_slave_config(mldl_cfg, gyro_handle, accel_handle, data, + mldl_cfg->accel, &mldl_cfg->pdata->accel); +} + +static inline int inv_mpu_config_compass(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *compass_handle, + struct ext_slave_config *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_slave_config(mldl_cfg, gyro_handle, compass_handle, data, + mldl_cfg->compass, + &mldl_cfg->pdata->compass); +} + +static inline int inv_mpu_config_pressure(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *pressure_handle, + struct ext_slave_config *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_slave_config(mldl_cfg, gyro_handle, pressure_handle, + data, mldl_cfg->pressure, + &mldl_cfg->pdata->pressure); +} + +/* Slave get config functions */ +int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_config *data, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + +static inline int inv_mpu_get_accel_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *accel_handle, + struct ext_slave_config *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, accel_handle, + data, mldl_cfg->accel, + &mldl_cfg->pdata->accel); +} + +static inline int inv_mpu_get_compass_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *compass_handle, + struct ext_slave_config *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, compass_handle, + data, mldl_cfg->compass, + &mldl_cfg->pdata->compass); +} + +static inline int inv_mpu_get_pressure_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *pressure_handle, + struct ext_slave_config *data) +{ + if (!mldl_cfg || !(mldl_cfg->pdata)) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + return inv_mpu_get_slave_config(mldl_cfg, gyro_handle, + pressure_handle, data, + mldl_cfg->pressure, + &mldl_cfg->pdata->pressure); +} + +/* -------------------------------------------------------------------------- */ + +static inline long inv_mpu_get_sampling_rate_hz(struct mldl_cfg *mldl_cfg) +{ + if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7)) + return 8000L / (mldl_cfg->divider + 1); + else + return 1000L / (mldl_cfg->divider + 1); +} + +static inline long inv_mpu_get_sampling_period_us(struct mldl_cfg *mldl_cfg) +{ + if (((mldl_cfg->lpf) == 0) || ((mldl_cfg->lpf) == 7)) + return (long) (1000000L * (mldl_cfg->divider + 1)) / 8000L; + else + return (long) (1000000L * (mldl_cfg->divider + 1)) / 1000L; +} + +#endif /* __MLDL_CFG_H__ */ + +/** + *@} + */ diff --git a/60xx/mlsdk/mllite/mldl_cfg_mpu.c b/60xx/mlsdk/mllite/mldl_cfg_mpu.c new file mode 100644 index 0000000..3d4496e --- /dev/null +++ b/60xx/mlsdk/mllite/mldl_cfg_mpu.c @@ -0,0 +1,477 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: mldl_cfg_mpu.c 5653 2011-06-16 21:06:55Z nroyer $ + * + *****************************************************************************/ + +/** + * @addtogroup MLDL + * + * @{ + * @file mldl_cfg_mpu.c + * @brief The Motion Library Driver Layer. + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include +#include "mldl_cfg.h" +#include "mlsl.h" +#include "mpu.h" + +#ifdef LINUX +#include +#endif + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mldl_cfg_mpu:" + +/* --------------------- */ +/* - Variables. - */ +/* --------------------- */ + + +/* ---------------------- */ +/* - Static Functions. - */ +/* ---------------------- */ +void mpu_print_cfg(struct mldl_cfg * mldl_cfg) +{ + struct mpu_platform_data *pdata = mldl_cfg->pdata; + struct ext_slave_platform_data *accel = &mldl_cfg->pdata->accel; + struct ext_slave_platform_data *compass = &mldl_cfg->pdata->compass; + struct ext_slave_platform_data *pressure = &mldl_cfg->pdata->pressure; + + MPL_LOGD("mldl_cfg.addr = %02x\n", mldl_cfg->addr); + MPL_LOGD("mldl_cfg.int_config = %02x\n", mldl_cfg->int_config); + MPL_LOGD("mldl_cfg.ext_sync = %02x\n", mldl_cfg->ext_sync); + MPL_LOGD("mldl_cfg.full_scale = %02x\n", mldl_cfg->full_scale); + MPL_LOGD("mldl_cfg.lpf = %02x\n", mldl_cfg->lpf); + MPL_LOGD("mldl_cfg.clk_src = %02x\n", mldl_cfg->clk_src); + MPL_LOGD("mldl_cfg.divider = %02x\n", mldl_cfg->divider); + MPL_LOGD("mldl_cfg.dmp_enable = %02x\n", mldl_cfg->dmp_enable); + MPL_LOGD("mldl_cfg.fifo_enable = %02x\n", mldl_cfg->fifo_enable); + MPL_LOGD("mldl_cfg.dmp_cfg1 = %02x\n", mldl_cfg->dmp_cfg1); + MPL_LOGD("mldl_cfg.dmp_cfg2 = %02x\n", mldl_cfg->dmp_cfg2); + MPL_LOGD("mldl_cfg.offset_tc[0] = %02x\n", mldl_cfg->offset_tc[0]); + MPL_LOGD("mldl_cfg.offset_tc[1] = %02x\n", mldl_cfg->offset_tc[1]); + MPL_LOGD("mldl_cfg.offset_tc[2] = %02x\n", mldl_cfg->offset_tc[2]); + MPL_LOGD("mldl_cfg.silicon_revision = %02x\n", mldl_cfg->silicon_revision); + MPL_LOGD("mldl_cfg.product_id = %02x\n", mldl_cfg->product_id); + MPL_LOGD("mldl_cfg.gyro_sens_trim = %02x\n", mldl_cfg->gyro_sens_trim); +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || defined CONFIG_MPU_SENSORS_MPU6050B1 + MPL_LOGD("mldl_cfg.accel_sens_trim = %02x\n", mldl_cfg->accel_sens_trim); +#endif + + if (mldl_cfg->accel) { + MPL_LOGD("slave_accel->suspend = %p\n", mldl_cfg->accel->suspend); + MPL_LOGD("slave_accel->resume = %p\n", mldl_cfg->accel->resume); + MPL_LOGD("slave_accel->read = %p\n", mldl_cfg->accel->read); + MPL_LOGD("slave_accel->type = %02x\n", mldl_cfg->accel->type); + MPL_LOGD("slave_accel->read_reg = %02x\n", + mldl_cfg->accel->read_reg); + MPL_LOGD("slave_accel->read_len = %02x\n", + mldl_cfg->accel->read_len); + MPL_LOGD("slave_accel->endian = %02x\n", mldl_cfg->accel->endian); + MPL_LOGD("slave_accel->range.mantissa= %02x\n", (int)mldl_cfg->accel->range.mantissa); + MPL_LOGD("slave_accel->range.fraction= %02x\n", (int)mldl_cfg->accel->range.fraction); + } else { + MPL_LOGD("slave_accel = NULL\n"); + } + + if (mldl_cfg->compass) { + MPL_LOGD("slave_compass->suspend = %p\n", mldl_cfg->compass->suspend); + MPL_LOGD("slave_compass->resume = %p\n", mldl_cfg->compass->resume); + MPL_LOGD("slave_compass->read = %p\n", mldl_cfg->compass->read); + MPL_LOGD("slave_compass->type = %02x\n", mldl_cfg->compass->type); + MPL_LOGD("slave_compass->read_reg = %02x\n", + mldl_cfg->compass->read_reg); + MPL_LOGD("slave_compass->read_len = %02x\n", + mldl_cfg->compass->read_len); + MPL_LOGD("slave_compass->endian = %02x\n", mldl_cfg->compass->endian); + MPL_LOGD("slave_compass->range.mantissa= %02x\n", (int)mldl_cfg->compass->range.mantissa); + MPL_LOGD("slave_compass->range.fraction= %02x\n", (int)mldl_cfg->compass->range.fraction); + } else { + MPL_LOGD("slave_compass = NULL\n"); + } + + if (mldl_cfg->pressure) { + MPL_LOGD("slave_pressure->suspend = %p\n", mldl_cfg->pressure->suspend); + MPL_LOGD("slave_pressure->resume = %p\n", mldl_cfg->pressure->resume); + MPL_LOGD("slave_pressure->read = %p\n", mldl_cfg->pressure->read); + MPL_LOGD("slave_pressure->type = %02x\n", mldl_cfg->pressure->type); + MPL_LOGD("slave_pressure->read_reg = %02x\n", + mldl_cfg->pressure->read_reg); + MPL_LOGD("slave_pressure->read_len = %02x\n", + mldl_cfg->pressure->read_len); + MPL_LOGD("slave_pressure->endian = %02x\n", mldl_cfg->pressure->endian); + MPL_LOGD("slave_pressure->range.mantissa= %02x\n", (int)mldl_cfg->pressure->range.mantissa); + MPL_LOGD("slave_pressure->range.fraction= %02x\n", (int)mldl_cfg->pressure->range.fraction); + } else { + MPL_LOGD("slave_pressure = NULL\n"); + } + + MPL_LOGD("accel->get_slave_descr = %p\n",accel->get_slave_descr); + MPL_LOGD("accel->adapt_num = %02x\n", accel->adapt_num); + MPL_LOGD("accel->bus = %02x\n", accel->bus); + MPL_LOGD("accel->address = %02x\n", accel->address); + MPL_LOGD("accel->orientation = \n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + accel->orientation[0],accel->orientation[1],accel->orientation[2], + accel->orientation[3],accel->orientation[4],accel->orientation[5], + accel->orientation[6],accel->orientation[7],accel->orientation[8]); + MPL_LOGD("compass->get_slave_descr = %p\n",compass->get_slave_descr); + MPL_LOGD("compass->adapt_num = %02x\n", compass->adapt_num); + MPL_LOGD("compass->bus = %02x\n", compass->bus); + MPL_LOGD("compass->address = %02x\n", compass->address); + MPL_LOGD("compass->orientation = \n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + compass->orientation[0],compass->orientation[1],compass->orientation[2], + compass->orientation[3],compass->orientation[4],compass->orientation[5], + compass->orientation[6],compass->orientation[7],compass->orientation[8]); + MPL_LOGD("pressure->get_slave_descr = %p\n",pressure->get_slave_descr); + MPL_LOGD("pressure->adapt_num = %02x\n", pressure->adapt_num); + MPL_LOGD("pressure->bus = %02x\n", pressure->bus); + MPL_LOGD("pressure->address = %02x\n", pressure->address); + MPL_LOGD("pressure->orientation = \n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + pressure->orientation[0],pressure->orientation[1],pressure->orientation[2], + pressure->orientation[3],pressure->orientation[4],pressure->orientation[5], + pressure->orientation[6],pressure->orientation[7],pressure->orientation[8]); + + MPL_LOGD("pdata->int_config = %02x\n", pdata->int_config); + MPL_LOGD("pdata->level_shifter = %02x\n", pdata->level_shifter); + MPL_LOGD("pdata->orientation = \n" + " %2d %2d %2d\n" + " %2d %2d %2d\n" + " %2d %2d %2d\n", + pdata->orientation[0],pdata->orientation[1],pdata->orientation[2], + pdata->orientation[3],pdata->orientation[4],pdata->orientation[5], + pdata->orientation[6],pdata->orientation[7],pdata->orientation[8]); + + MPL_LOGD("Struct sizes: mldl_cfg: %zu, " + "ext_slave_descr:%zu, mpu_platform_data:%zu: RamOffset: %zu\n", + sizeof(struct mldl_cfg), sizeof(struct ext_slave_descr), + sizeof(struct mpu_platform_data), + offsetof(struct mldl_cfg, ram)); +} + +/****************************************************************************** + ****************************************************************************** + * Exported functions + ****************************************************************************** + *****************************************************************************/ + +/** + * Initializes the pdata structure to defaults. + * + * Opens the device to read silicon revision, product id and whoami. Leaves + * the device in suspended state for low power. + * + * @param mldl_cfg handle to the config structure + * @param mlsl_handle handle to the mpu serial layer + * @param accel_handle handle to the accel serial layer + * @param compass_handle handle to the compass serial layer + * @param pressure_handle handle to the pressure serial layer + * + * @return INV_SUCCESS if silicon revision, product id and woami are supported + * by this software. + */ +int inv_mpu_open(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle) +{ + int result; + result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL, + INV_ALL_SENSORS); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + return result; +} + +/** + * Stub for driver close. Just verify that the devices are suspended + * + * @param mldl_cfg handle to the config structure + * @param mlsl_handle handle to the mpu serial layer + * @param accel_handle handle to the accel serial layer + * @param compass_handle handle to the compass serial layer + * @param pressure_handle handle to the compass serial layer + * + * @return INV_SUCCESS or non-zero error code + */ +int inv_mpu_close(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle) +{ + int result = INV_SUCCESS; + + result = inv_mpu_suspend(mldl_cfg, mlsl_handle, NULL, NULL, NULL, + INV_ALL_SENSORS); + return result; +} + +int inv_mpu_resume(struct mldl_cfg* mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + unsigned long sensors) +{ + int result; + + mldl_cfg->requested_sensors = sensors; + result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = ioctl((int)(uintptr_t)mlsl_handle, MPU_RESUME, NULL); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + //MPL_LOGI("%s: Resuming to %04lx\n", __func__, mldl_cfg->requested_sensors); + + return result; +} + + +int inv_mpu_suspend(struct mldl_cfg *mldl_cfg, + void *mlsl_handle, + void *accel_handle, + void *compass_handle, + void *pressure_handle, + unsigned long sensors) +{ + int result; + unsigned long requested = mldl_cfg->requested_sensors; + + mldl_cfg->requested_sensors = (~sensors) & INV_ALL_SENSORS; + //MPL_LOGI("%s: suspending sensors to %04lx\n", __func__, + // mldl_cfg->requested_sensors); + + result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SET_MPU_CONFIG, mldl_cfg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = ioctl((int)(uintptr_t)mlsl_handle, MPU_SUSPEND, NULL); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = ioctl((int)(uintptr_t)mlsl_handle, MPU_GET_MPU_CONFIG, mldl_cfg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + mldl_cfg->requested_sensors = requested; + //MPL_LOGI("%s: Will resume next to %04lx\n", __func__, requested); + + return result; +} + +/** + * Send slave configuration information + * + * @param mldl_cfg pointer to the mldl configuration structure + * @param gyro_handle handle to the gyro sensor + * @param slave_handle handle to the slave sensor + * @param slave slave description + * @param pdata slave platform data + * @param data where to store the read data + * + * @return 0 or non-zero error code + */ +int inv_mpu_slave_read(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data) +{ + int result; + if (!mldl_cfg || !gyro_handle || !data || !slave) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + switch (slave->type) { + case EXT_SLAVE_TYPE_ACCELEROMETER: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_ACCEL, data); + break; + case EXT_SLAVE_TYPE_COMPASS: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_COMPASS, data); + break; + case EXT_SLAVE_TYPE_PRESSURE: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_READ_PRESSURE, data); + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + break; + } + + return result; +} + +/** + * Send slave configuration information + * + * @param mldl_cfg pointer to the mldl configuration structure + * @param gyro_handle handle to the gyro sensor + * @param slave_handle handle to the slave sensor + * @param data the data being sent + * @param slave slave description + * @param pdata slave platform data + * + * @return 0 or non-zero error code + */ +int inv_mpu_slave_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_config *data, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + if (!mldl_cfg || !data || !slave || !pdata || !slave) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + + switch (slave->type) { + case EXT_SLAVE_TYPE_ACCELEROMETER: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_ACCEL, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case EXT_SLAVE_TYPE_COMPASS: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_COMPASS, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case EXT_SLAVE_TYPE_PRESSURE: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_CONFIG_PRESSURE, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + break; + } + + return result; +} + +/** + * Request slave configuration information + * + * Use this specifically after requesting a slave configuration to see what the + * slave accually accepted. + * + * @param mldl_cfg pointer to the mldl configuration structure + * @param gyro_handle handle to the gyro sensor + * @param slave_handle handle to the slave sensor + * @param data the data being requested. + * @param slave slave description + * @param pdata slave platform data + * + * @return 0 or non-zero error code + */ +int inv_mpu_get_slave_config(struct mldl_cfg *mldl_cfg, + void *gyro_handle, + void *slave_handle, + struct ext_slave_config *data, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata) +{ + int result; + if (!mldl_cfg || !data || !slave) { + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + } + switch (slave->type) { + case EXT_SLAVE_TYPE_ACCELEROMETER: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_ACCEL, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case EXT_SLAVE_TYPE_COMPASS: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_COMPASS, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + case EXT_SLAVE_TYPE_PRESSURE: + result = ioctl((int)(uintptr_t)gyro_handle, MPU_GET_CONFIG_PRESSURE, data); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + break; + default: + LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER); + return INV_ERROR_INVALID_PARAMETER; + break; + } + return result; +} +/** + *@} + */ diff --git a/60xx/mlsdk/mllite/mldmp.c b/60xx/mlsdk/mllite/mldmp.c new file mode 100644 index 0000000..7381dd4 --- /dev/null +++ b/60xx/mlsdk/mllite/mldmp.c @@ -0,0 +1,284 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: mldmp.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +/** + * @addtogroup MLDMP + * + * @{ + * @file mldmp.c + * @brief Shared functions between all the different DMP versions +**/ + +#include + +#include "mltypes.h" +#include "mlinclude.h" +#include "mltypes.h" +#include "ml.h" +#include "mldl_cfg.h" +#include "mldl.h" +#include "compass.h" +#include "mlSetGyroBias.h" +#include "mlsl.h" +#include "mlFIFO.h" +#include "mldmp.h" +#include "mlstates.h" +#include "dmpDefault.h" +#include "mlFIFOHW.h" +#include "mlsupervisor.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-dmp" + +/** + * @brief Open the default motion sensor engine. + * This function is used to open the default MPL engine, + * featuring, for example, sensor fusion (6 axes and 9 axes), + * sensor calibration, accelerometer data byte swapping, among + * others. + * Compare with the other provided engines. + * + * @pre inv_serial_start() must have been called to instantiate the serial + * communication. + * + * Example: + * @code + * result = inv_dmp_open( ); + * if (INV_SUCCESS != result) { + * // Handle the error case + * } + * @endcode + * + * @return Zero on success; Error Code on any failure. + * + */ +inv_error_t inv_dmp_open(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + unsigned char state = inv_get_state(); + struct mldl_cfg *mldl_cfg; + unsigned long requested_sensors; + + /************************************************************* + * Common operations before calling DMPOpen + ************************************************************/ + if (state == INV_STATE_DMP_OPENED) + return INV_SUCCESS; + + if (state == INV_STATE_DMP_STARTED) { + return inv_dmp_stop(); + } + + result = inv_state_transition(INV_STATE_DMP_OPENED); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + result = inv_dl_open(inv_get_serial_handle()); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#ifdef ML_USE_DMP_SIM + do { + void setup_univ(); + setup_univ(); /* hijack the read and write paths + and re-direct them to the simulator */ + } while (0); +#endif + + result = inv_setup_dmp(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + // Init vars. + inv_init_ml(); + + result = inv_init_fifo_param(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_enable_set_bias(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_init_fifo_hardare(); + mldl_cfg = inv_get_dl_config(); + requested_sensors = INV_THREE_AXIS_GYRO; + if (mldl_cfg->accel && mldl_cfg->accel->resume) + requested_sensors |= INV_THREE_AXIS_ACCEL; + + if (mldl_cfg->compass && mldl_cfg->compass->resume) + requested_sensors |= INV_THREE_AXIS_COMPASS; + + if (mldl_cfg->pressure && mldl_cfg->pressure->resume) + requested_sensors |= INV_THREE_AXIS_PRESSURE; + + result = inv_init_requested_sensors(requested_sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_apply_calibration(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + if (NULL != mldl_cfg->accel){ + result = inv_apply_endian_accel(); + } + + return result; +} + +/** + * @brief Start the DMP. + * + * @pre inv_dmp_open() must have been called. + * + * @code + * result = inv_dmp_start(); + * if (INV_SUCCESS != result) { + * // Handle the error case + * } + * @endcode + * + * @return INV_SUCCESS if successful, or Non-zero error code otherwise. + */ +inv_error_t inv_dmp_start(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + + if (inv_get_state() == INV_STATE_DMP_STARTED) + return INV_SUCCESS; + + result = inv_state_transition(INV_STATE_DMP_STARTED); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + inv_init_sensor_fusion_supervisor(); + result = inv_dl_start(inv_get_dl_config()->requested_sensors); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* This is done after the start since it will modify DMP memory, which + * will cause a full reset is most cases */ + result = inv_reset_motion(); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/** + * @brief Stops the DMP and puts it in low power. + * + * @pre inv_dmp_start() must have been called. + * + * @return INV_SUCCESS, Non-zero error code otherwise. + */ +inv_error_t inv_dmp_stop(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + + if (inv_get_state() == INV_STATE_DMP_OPENED) + return INV_SUCCESS; + + result = inv_state_transition(INV_STATE_DMP_OPENED); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_dl_stop(INV_ALL_SENSORS); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + return result; +} + +/** + * @brief Closes the motion sensor engine. + * Does not close the serial communication. To do that, + * call inv_serial_stop(). + * After calling inv_dmp_close() another DMP module can be + * loaded in the MPL with the corresponding necessary + * intialization and configurations, via any of the + * MLDmpXXXOpen functions. + * + * @pre inv_dmp_open() must have been called. + * + * @code + * result = inv_dmp_close(); + * if (INV_SUCCESS != result) { + * // Handle the error case + * } + * @endcode + * + * @return INV_SUCCESS, Non-zero error code otherwise. + */ +inv_error_t inv_dmp_close(void) +{ + INVENSENSE_FUNC_START; + inv_error_t result; + inv_error_t firstError = INV_SUCCESS; + + if (inv_get_state() <= INV_STATE_DMP_CLOSED) + return INV_SUCCESS; + + result = inv_disable_set_bias(); + ERROR_CHECK_FIRST(firstError, result); + + result = inv_dl_stop(INV_ALL_SENSORS); + ERROR_CHECK_FIRST(firstError, result); + + result = inv_close_fifo(); + ERROR_CHECK_FIRST(firstError, result); + + result = inv_dl_close(); + ERROR_CHECK_FIRST(firstError, result); + + result = inv_state_transition(INV_STATE_SERIAL_OPENED); + ERROR_CHECK_FIRST(firstError, result); + + return result; +} + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/mldmp.h b/60xx/mlsdk/mllite/mldmp.h new file mode 100644 index 0000000..f519798 --- /dev/null +++ b/60xx/mlsdk/mllite/mldmp.h @@ -0,0 +1,96 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/***************************************************************************** * + * $Id: mldmp.h 5629 2011-06-11 03:13:08Z mcaramello $ + ******************************************************************************/ + +/** + * @defgroup MLDMP + * @brief + * + * These are the top level functions that define how to load the MPL. In order + * to use most of the features, the DMP must be loaded with some code. The + * loading procedure takes place when calling inv_dmp_open with a given DMP set + * function, after having open the serial communication with the device via + * inv_serial_start(). + * The DMP set function will load the DMP memory and enable a certain + * set of features. + * + * First select a DMP version from one of the released DMP sets. + * These could be: + * - DMP default to load and use the default DMP code featuring pedometer, + * gestures, and orientation. Use inv_dmp_open(). + * - DMP pedometer stand-alone to load and use the standalone pedometer + * implementation. Use inv_open_low_power_pedometer(). + * + * + * After inv_dmp_openXXX any number of appropriate initialization and configuration + * routines can be called. Each one of these routines will return an error code + * and will check to make sure that it is compatible with the the DMP version + * selected during the call to inv_dmp_open. + * + * Once the configuration is complete, make a call to inv_dmp_start(). This will + * finally turn on the DMP and run the code previously loaded. + * + * While the DMP is running, all data fetching, polling or other functions can + * be called and will return valid data. Some parameteres can be changed while + * the DMP is runing, while others cannot. Therefore it is important to always + * check the return code of each function. Check the error code list in mltypes + * to know what each returned error corresponds to. + * + * When no more motion processing is required, the library can be shut down and + * the DMP turned off. We can do that by calling inv_dmp_close(). Note that + * inv_dmp_close() will not close the serial communication automatically, which will + * remain open an active, in case another module needs to be loaded instead. + * If the intention is shutting down the MPL as well, an explicit call to + * inv_serial_stop() following inv_dmp_close() has to be made. + * + * The MPL additionally implements a basic state machine, whose purpose is to + * give feedback to the user on whether he is following all the required + * initialization steps. If an anomalous transition is detected, the user will + * be warned by a terminal message with the format: + * + * "Error : illegal state transition from STATE_1 to STATE_3" + * + * @{ + * @file mldmp.h + * @brief Top level entry functions to the MPL library with DMP support + */ + +#ifndef MLDMP_H +#define MLDMP_H +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mldmp_legacy.h" +#endif + +#ifdef __cplusplus +extern "C" { +#endif + + inv_error_t inv_dmp_open(void); + inv_error_t inv_dmp_start(void); + inv_error_t inv_dmp_stop(void); + inv_error_t inv_dmp_close(void); + +#ifdef __cplusplus +} +#endif +#endif /* MLDMP_H */ +/** + * @} +**/ diff --git a/60xx/mlsdk/mllite/mlinclude.h b/60xx/mlsdk/mllite/mlinclude.h new file mode 100644 index 0000000..dcbe904 --- /dev/null +++ b/60xx/mlsdk/mllite/mlinclude.h @@ -0,0 +1,50 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#ifndef INV_INCLUDE_H__ +#define INV_INCLUDE_H__ + +#define INVENSENSE_FUNC_START typedef int invensensePutFunctionCallsHere + +#ifdef COVERAGE +#include "utestCommon.h" +#endif +#ifdef PROFILE +#include "profile.h" +#endif + +#ifdef WIN32 +#ifdef COVERAGE + +extern int functionEnterLog(const char *file, const char *func); +extern int functionExitLog(const char *file, const char *func); + +#undef INVENSENSE_FUNC_START +#define INVENSENSE_FUNC_START __pragma(message(__FILE__ "|"__FUNCTION__ )) \ + int dslkQjDsd = functionEnterLog(__FILE__, __FUNCTION__) +#endif // COVERAGE +#endif // WIN32 + +#ifdef PROFILE +#undef INVENSENSE_FUNC_START +#define INVENSENSE_FUNC_START int dslkQjDsd = profileEnter(__FILE__, __FUNCTION__) +#define return if ( profileExit(__FILE__, __FUNCTION__) ) return +#endif // PROFILE + +// #define return if ( functionExitLog(__FILE__, __FUNCTION__) ) return + +#endif //INV_INCLUDE_H__ diff --git a/60xx/mlsdk/mllite/mlstates.c b/60xx/mlsdk/mllite/mlstates.c new file mode 100644 index 0000000..8ebb086 --- /dev/null +++ b/60xx/mlsdk/mllite/mlstates.c @@ -0,0 +1,269 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mlstates.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup MLSTATES + * @brief Basic state machine definition and support for the Motion Library. + * + * @{ + * @file mlstates.c + * @brief The Motion Library state machine definition. + */ + +#define ML_C + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include +#include + +#include "mlstates.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "ml.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mlstates" + +#define _stateDebug(x) //{x} + +#define MAX_STATE_CHANGE_PROCESSES (8) + +struct state_callback_obj { + int_fast8_t numStateChangeCallbacks; + HANDLE mutex; + state_change_callback_t stateChangeCallbacks[MAX_STATE_CHANGE_PROCESSES]; +}; + +static struct state_callback_obj sStateChangeCallbacks = { 0 }; + +/* --------------- */ +/* - Functions. - */ +/* --------------- */ + +static inv_error_t inv_init_state_callbacks(void) +{ + memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks)); + return inv_create_mutex(&sStateChangeCallbacks.mutex); +} + +static inv_error_t MLStateCloseCallbacks(void) +{ + inv_error_t result; + result = inv_destroy_mutex(sStateChangeCallbacks.mutex); + memset(&sStateChangeCallbacks, 0, sizeof(sStateChangeCallbacks)); + return result; +} + +/** + * @internal + * @brief return a string containing the label assigned to the given state. + * @param state The state of which the label has to be returned. + * @return A string containing the state label. +**/ +char *inv_state_name(unsigned char state) +{ + switch (state) { + case INV_STATE_SERIAL_CLOSED: + return INV_STATE_NAME(INV_STATE_SERIAL_CLOSED); + break; + case INV_STATE_SERIAL_OPENED: + return INV_STATE_NAME(INV_STATE_SERIAL_OPENED); + break; + case INV_STATE_DMP_OPENED: + return INV_STATE_NAME(INV_STATE_DMP_OPENED); + break; + case INV_STATE_DMP_STARTED: + return INV_STATE_NAME(INV_STATE_DMP_STARTED); + break; + default: + return NULL; + } +} + +/** + * @internal + * @brief Perform a transition from the current state to newState. + * Check for the correctness of the transition. + * Print out an error message if the transition is illegal . + * This routine is also called if a certain normally constant parameters + * are changed such as the FIFO Rate. + * @param newState state we are transitioning to. + * @return +**/ +inv_error_t inv_state_transition(unsigned char newState) +{ + inv_error_t result = INV_SUCCESS; + + if (newState == INV_STATE_SERIAL_CLOSED) { + // Always allow transition to closed + } else if (newState == INV_STATE_SERIAL_OPENED) { + inv_init_state_callbacks(); // Always allow first transition to start over + } else if (((newState == INV_STATE_DMP_OPENED) && + ((inv_params_obj.state == INV_STATE_SERIAL_OPENED) || + (inv_params_obj.state == INV_STATE_DMP_STARTED))) + || + ((newState == INV_STATE_DMP_STARTED) && + (inv_params_obj.state == INV_STATE_DMP_OPENED))) { + // Valid transitions but no special action required + } else { + // All other combinations are illegal + MPL_LOGE("Error : illegal state transition from %s to %s\n", + inv_state_name(inv_params_obj.state), + inv_state_name(newState)); + result = INV_ERROR_SM_TRANSITION; + } + + if (result == INV_SUCCESS) { + _stateDebug(MPL_LOGV + ("ML State transition from %s to %s\n", + inv_state_name(inv_params_obj.state), + inv_state_name(newState))); + result = inv_run_state_callbacks(newState); + if (INV_SUCCESS == result && newState == INV_STATE_SERIAL_CLOSED) { + MLStateCloseCallbacks(); + } + inv_params_obj.state = newState; + } + return result; +} + +/** + * @internal + * @brief To be moved in mlstates.c +**/ +unsigned char inv_get_state(void) +{ + return (inv_params_obj.state); +} + +/** + * @internal + * @brief This registers a function to be called each time the state + * changes. It may also be called when the FIFO Rate is changed. + * It will be called at the start of a state change before the + * state change has taken place. See Also inv_unregister_state_callback() + * The FIFO does not have to be on for this callback. + * @param func Function to be called when a DMP interrupt occurs. + * @return INV_SUCCESS or non-zero error code. + */ + +inv_error_t inv_register_state_callback(state_change_callback_t callback) +{ + INVENSENSE_FUNC_START; + int kk; + inv_error_t result; + + result = inv_lock_mutex(sStateChangeCallbacks.mutex); + if (INV_SUCCESS != result) { + return result; + } + // Make sure we have not filled up our number of allowable callbacks + if (sStateChangeCallbacks.numStateChangeCallbacks < + MAX_STATE_CHANGE_PROCESSES) { + // Make sure we haven't registered this function already + for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { + if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) { + result = INV_ERROR_INVALID_PARAMETER; + break; + } + } + + if (INV_SUCCESS == result) { + // Add new callback + sStateChangeCallbacks.stateChangeCallbacks[sStateChangeCallbacks. + numStateChangeCallbacks] + = callback; + sStateChangeCallbacks.numStateChangeCallbacks++; + } + } else { + result = INV_ERROR_MEMORY_EXAUSTED; + } + + inv_unlock_mutex(sStateChangeCallbacks.mutex); + return result; +} + +/** + * @internal + * @brief This unregisters a function to be called each time the state + * changes. See Also inv_register_state_callback() + * The FIFO does not have to be on for this callback. + * @return INV_SUCCESS or non-zero error code. + */ +inv_error_t inv_unregister_state_callback(state_change_callback_t callback) +{ + INVENSENSE_FUNC_START; + int kk, jj; + inv_error_t result; + + result = inv_lock_mutex(sStateChangeCallbacks.mutex); + if (INV_SUCCESS != result) { + return result; + } + // Make sure we haven't registered this function already + result = INV_ERROR_INVALID_PARAMETER; + for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { + if (sStateChangeCallbacks.stateChangeCallbacks[kk] == callback) { + for (jj = kk + 1; + jj < sStateChangeCallbacks.numStateChangeCallbacks; ++jj) { + sStateChangeCallbacks.stateChangeCallbacks[jj - 1] = + sStateChangeCallbacks.stateChangeCallbacks[jj]; + } + sStateChangeCallbacks.numStateChangeCallbacks--; + result = INV_SUCCESS; + break; + } + } + + inv_unlock_mutex(sStateChangeCallbacks.mutex); + return result; +} + +inv_error_t inv_run_state_callbacks(unsigned char newState) +{ + int kk; + inv_error_t result; + + result = inv_lock_mutex(sStateChangeCallbacks.mutex); + if (INV_SUCCESS != result) { + MPL_LOGE("MLOsLockMutex returned %d\n", result); + return result; + } + + for (kk = 0; kk < sStateChangeCallbacks.numStateChangeCallbacks; ++kk) { + if (sStateChangeCallbacks.stateChangeCallbacks[kk]) { + result = sStateChangeCallbacks.stateChangeCallbacks[kk] (newState); + if (INV_SUCCESS != result) { + break; // Can't return, must release mutex + } + } + } + + inv_unlock_mutex(sStateChangeCallbacks.mutex); + return result; +} diff --git a/60xx/mlsdk/mllite/mlstates.h b/60xx/mlsdk/mllite/mlstates.h new file mode 100644 index 0000000..cbaa610 --- /dev/null +++ b/60xx/mlsdk/mllite/mlstates.h @@ -0,0 +1,58 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mlstates.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#ifndef INV_STATES_H__ +#define INV_STATES_H__ + +#include "mltypes.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mlstates_legacy.h" +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/* See inv_state_transition for the transition mask */ +#define INV_STATE_SERIAL_CLOSED (0) +#define INV_STATE_SERIAL_OPENED (1) +#define INV_STATE_DMP_OPENED (2) +#define INV_STATE_DMP_STARTED (3) +#define INV_STATE_DMP_STOPPED (INV_STATE_DMP_OPENED) +#define INV_STATE_DMP_CLOSED (INV_STATE_SERIAL_OPENED) + +#define INV_STATE_NAME(x) (#x) + + typedef inv_error_t(*state_change_callback_t) (unsigned char newState); + + char *inv_state_name(unsigned char x); + inv_error_t inv_state_transition(unsigned char newState); + unsigned char inv_get_state(void); + inv_error_t inv_register_state_callback(state_change_callback_t callback); + inv_error_t inv_unregister_state_callback(state_change_callback_t callback); + inv_error_t inv_run_state_callbacks(unsigned char newState); + +#ifdef __cplusplus +} +#endif +#endif // INV_STATES_H__ diff --git a/60xx/mlsdk/mllite/mlsupervisor.c b/60xx/mlsdk/mllite/mlsupervisor.c new file mode 100644 index 0000000..139297f --- /dev/null +++ b/60xx/mlsdk/mllite/mlsupervisor.c @@ -0,0 +1,597 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $ + * + *****************************************************************************/ + +/** + * @defgroup ML_SUPERVISOR + * @brief Basic sensor fusion supervisor functionalities. + * + * @{ + * @file mlsupervisor.c + * @brief Basic sensor fusion supervisor functionalities. + */ + +#include "ml.h" +#include "mldl.h" +#include "mldl_cfg.h" +#include "mltypes.h" +#include "mlinclude.h" +#include "compass.h" +#include "pressure.h" +#include "dmpKey.h" +#include "dmpDefault.h" +#include "mlstates.h" +#include "mlFIFO.h" +#include "mlFIFOHW.h" +#include "mlMathFunc.h" +#include "mlsupervisor.h" +#include "mlmath.h" + +#include "mlsl.h" +#include "mlos.h" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-sup" + +static unsigned long lastCompassTime = 0; +static unsigned long polltime = 0; +static unsigned long coiltime = 0; +static int accCount = 0; +static int compassCalStableCount = 0; +static int compassCalCount = 0; +static int coiltimerstart = 0; +static unsigned long disturbtime = 0; +static int disturbtimerstart = 0; + +static yas_filter_if_s f; +static yas_filter_handle_t handle; + +#define SUPERVISOR_DEBUG 0 + +struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 }; + +/** + * @brief This initializes all variables that should be reset on + */ +void inv_init_sensor_fusion_supervisor(void) +{ + lastCompassTime = 0; + polltime = 0; + inv_obj.acc_state = SF_STARTUP_SETTLE; + accCount = 0; + compassCalStableCount = 0; + compassCalCount = 0; + + yas_filter_init(&f); + f.init(&handle); + +#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ + defined CONFIG_MPU_SENSORS_MPU6050B1 + if (inv_compass_present()) { + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) { + (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT); + } + } +#endif + + if (ml_supervisor_cb.supervisor_reset_func != NULL) { + ml_supervisor_cb.supervisor_reset_func(); + } +} + +static int MLUpdateCompassCalibration3DOF(int command, long *data, + unsigned long deltaTime) +{ + INVENSENSE_FUNC_START; + int retValue = INV_SUCCESS; + static float m[10][10] = { {0} }; + float mInv[10][10] = { {0} }; + float mTmp[10][10] = { {0} }; + static float xTransY[4] = { 0 }; + float magSqr = 0; + float inpData[3] = { 0 }; + int i, j; + int a, b; + float d; + + switch (command) { + case CAL_ADD_DATA: + inpData[0] = (float)data[0]; + inpData[1] = (float)data[1]; + inpData[2] = (float)data[2]; + m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]); + m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]); + m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]); + m[0][3] += (-2 * inpData[0]); + m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]); + m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]); + m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]); + m[1][3] += (-2 * inpData[1]); + m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]); + m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]); + m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]); + m[2][3] += (-2 * inpData[2]); + m[3][0] += (-2 * inpData[0]); + m[3][1] += (-2 * inpData[1]); + m[3][2] += (-2 * inpData[2]); + m[3][3] += 1.0f; + magSqr = + inpData[0] * inpData[0] + inpData[1] * inpData[1] + + inpData[2] * inpData[2]; + xTransY[0] += (-2 * inpData[0]) * magSqr; + xTransY[1] += (-2 * inpData[1]) * magSqr; + xTransY[2] += (-2 * inpData[2]) * magSqr; + xTransY[3] += magSqr; + break; + case CAL_RUN: + a = 4; + b = a; + for (i = 0; i < b; i++) { + for (j = 0; j < b; j++) { + a = b; + inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j); + mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a); + } + } + a = b; + d = inv_matrix_det(&m[0][0], &a); + if (d == 0) { + return INV_ERROR; + } + for (i = 0; i < 3; i++) { + float tmp = 0; + for (j = 0; j < 4; j++) { + tmp += mInv[j][i] / d * xTransY[j]; + } + inv_obj.compass_test_bias[i] = + -(long)(tmp * inv_obj.compass_sens / 16384.0f); + } + break; + case CAL_RESET: + for (i = 0; i < 4; i++) { + for (j = 0; j < 4; j++) { + m[i][j] = 0; + } + xTransY[i] = 0; + } + default: + break; + } + return retValue; +} + +/** + * Entry point for Sensor Fusion operations + * @internal + * @param magFB magnetormeter FB + * @param accSF accelerometer SF + */ +static inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF, + unsigned long deltaTime) +{ + static long prevCompassBias[3] = { 0 }; + static long magMax[3] = { + -1073741824L, + -1073741824L, + -1073741824L + }; + static long magMin[3] = { + 1073741824L, + 1073741824L, + 1073741824L + }; + int gyroMag; + long accMag; + inv_error_t result; + int i; + + if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) { + ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime); + } + + gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT; + accMag = inv_accel_sum_of_sqr(); + + // Scaling below assumes certain scaling. +#if ACC_MAG_SQR_SHIFT != 16 +#error +#endif + + if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) { + result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) { + //Most basic compass calibration, used only with lite MPL + if (inv_obj.resetting_compass == 1) { + for (i = 0; i < 3; i++) { + magMax[i] = -1073741824L; + magMin[i] = 1073741824L; + prevCompassBias[i] = 0; + } + compassCalStableCount = 0; + compassCalCount = 0; + inv_obj.resetting_compass = 0; + } + if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) { + if (compassCalStableCount < 1000) { + for (i = 0; i < 3; i++) { + if (inv_obj.compass_sensor_data[i] > magMax[i]) { + magMax[i] = inv_obj.compass_sensor_data[i]; + } + if (inv_obj.compass_sensor_data[i] < magMin[i]) { + magMin[i] = inv_obj.compass_sensor_data[i]; + } + } + MLUpdateCompassCalibration3DOF(CAL_ADD_DATA, + inv_obj.compass_sensor_data, + deltaTime); + compassCalStableCount += deltaTime; + for (i = 0; i < 3; i++) { + if (magMax[i] - magMin[i] < + (long long)40 * 1073741824 / inv_obj.compass_sens) { + compassCalStableCount = 0; + } + } + } else { + if (compassCalStableCount >= 1000) { + if (MLUpdateCompassCalibration3DOF + (CAL_RUN, inv_obj.compass_sensor_data, + deltaTime) == INV_SUCCESS) { + inv_obj.got_compass_bias = 1; + inv_obj.compass_accuracy = 3; + for(i=0; i<3; i++) { + inv_obj.compass_bias_error[i] = 35; + } + if (inv_obj.compass_state == SF_UNCALIBRATED) + inv_obj.compass_state = SF_STARTUP_SETTLE; + inv_set_compass_bias(inv_obj.compass_test_bias); + } + compassCalCount = 0; + compassCalStableCount = 0; + for (i = 0; i < 3; i++) { + magMax[i] = -1073741824L; + magMin[i] = 1073741824L; + prevCompassBias[i] = 0; + } + MLUpdateCompassCalibration3DOF(CAL_RESET, + inv_obj.compass_sensor_data, + deltaTime); + } + } + } + compassCalCount += deltaTime; + if (compassCalCount > 20000) { + compassCalCount = 0; + compassCalStableCount = 0; + for (i = 0; i < 3; i++) { + magMax[i] = -1073741824L; + magMin[i] = 1073741824L; + prevCompassBias[i] = 0; + } + MLUpdateCompassCalibration3DOF(CAL_RESET, + inv_obj.compass_sensor_data, + deltaTime); + } + } + + if (inv_obj.acc_state != SF_STARTUP_SETTLE) { + if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) { + inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing + accCount = 0; + } else { + if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214) + && ((inv_obj.acc_state == SF_DISTURBANCE) + || (inv_obj.acc_state == SF_SLOW_SETTLE))) { + accCount += deltaTime; + if (accCount > 0) { + inv_obj.acc_state = SF_FAST_SETTLE; + accCount = 0; + } + } else { + if (inv_obj.acc_state == SF_DISTURBANCE) { + accCount += deltaTime; + if (accCount > 500) { + inv_obj.acc_state = SF_SLOW_SETTLE; + accCount = 0; + } + } else if (inv_obj.acc_state == SF_SLOW_SETTLE) { + accCount += deltaTime; + if (accCount > 1000) { + inv_obj.acc_state = SF_NORMAL; + accCount = 0; + } + } else if (inv_obj.acc_state == SF_FAST_SETTLE) { + accCount += deltaTime; + if (accCount > 250) { + inv_obj.acc_state = SF_NORMAL; + accCount = 0; + } + } + } + } + } + if (inv_obj.acc_state == SF_STARTUP_SETTLE) { + accCount += deltaTime; + if (accCount > 50) { + *accSF = 1073741824; //Startup settling + inv_obj.acc_state = SF_NORMAL; + accCount = 0; + } + } else if ((inv_obj.acc_state == SF_NORMAL) + || (inv_obj.acc_state == SF_SLOW_SETTLE)) { + *accSF = inv_obj.accel_sens * 64; //Normal + } else if ((inv_obj.acc_state == SF_DISTURBANCE)) { + *accSF = inv_obj.accel_sens * 64; //Don't use accel (should be 0) + } else if (inv_obj.acc_state == SF_FAST_SETTLE) { + *accSF = inv_obj.accel_sens * 512; //Amplify accel + } + if (!inv_get_gyro_present()) { + *accSF = inv_obj.accel_sens * 128; + } + return INV_SUCCESS; +} + +/** + * @brief Entry point for software sensor fusion operations. + * Manages hardware interaction, calls sensor fusion supervisor for + * bias calculation. + * @return error code. INV_SUCCESS if no error occurred. + */ +inv_error_t inv_accel_compass_supervisor(void) +{ + inv_error_t result; + int adjustSensorFusion = 0; + long accSF = 1073741824; + static double magFB = 0; + long magSensorData[3]; + float fcin[3]; + float fcout[3]; + + + if (inv_compass_present()) { /* check for compass data */ + int i, j; + long long tmp[3] = { 0 }; + long long tmp64 = 0; + unsigned long ctime = inv_get_tick_count(); + if (((inv_get_compass_id() == COMPASS_ID_AK8975) && + ((ctime - polltime) > 20)) || + (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max + if (SUPERVISOR_DEBUG) { + MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n"); + MPL_LOGV("delta time = %ld\n", ctime - polltime); + } + polltime = ctime; + result = inv_get_compass_data(magSensorData); + /* external slave wants the data even if there is an error */ + if (result && !inv_obj.external_slave_callback) { + if (SUPERVISOR_DEBUG) { + MPL_LOGW("inv_get_compass_data returned %d\n", result); + } + } else { + unsigned long compassTime = inv_get_tick_count(); + unsigned long deltaTime = 1; + + if (result == INV_SUCCESS) { + if (lastCompassTime != 0) { + deltaTime = compassTime - lastCompassTime; + } + lastCompassTime = compassTime; + adjustSensorFusion = 1; + if (inv_obj.got_init_compass_bias == 0) { + inv_obj.got_init_compass_bias = 1; + for (i = 0; i < 3; i++) { + inv_obj.init_compass_bias[i] = magSensorData[i]; + } + } + for (i = 0; i < 3; i++) { + inv_obj.compass_sensor_data[i] = (long)magSensorData[i]; + inv_obj.compass_sensor_data[i] -= + inv_obj.init_compass_bias[i]; + tmp[i] = ((long long)inv_obj.compass_sensor_data[i]) + * inv_obj.compass_sens / 16384; + tmp[i] -= inv_obj.compass_bias[i]; + tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L; + } + for (i = 0; i < 3; i++) { + tmp64 = 0; + for (j = 0; j < 3; j++) { + tmp64 += (long long) tmp[j] * + inv_obj.compass_cal[i * 3 + j]; + } + inv_obj.compass_calibrated_data[i] = + (long) (tmp64 / inv_obj.compass_sens); + } + //Additions: + if ((inv_obj.compass_state == 1) && + (inv_obj.compass_overunder == 0)) { + if (disturbtimerstart == 0) { + disturbtimerstart = 1; + disturbtime = ctime; + } + } else { + disturbtimerstart = 0; + } + + if (inv_obj.compass_overunder) { + if (coiltimerstart == 0) { + coiltimerstart = 1; + coiltime = ctime; + } + } + if (coiltimerstart == 1) { + if (ctime - coiltime > 3000) { + inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; + inv_set_compass_offset(); + inv_reset_compass_calibration(); + coiltimerstart = 0; + } + } + + if (disturbtimerstart == 1) { + if (ctime - disturbtime > 10000) { + inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; + inv_set_compass_offset(); + inv_reset_compass_calibration(); + MPL_LOGI("Compass reset! inv_reset_compass_calibration \n"); + disturbtimerstart = 0; + } + } + } + if (inv_obj.external_slave_callback) { + result = inv_obj.external_slave_callback(&inv_obj); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + +#ifdef APPLY_COMPASS_FILTER + if (inv_get_compass_id() == COMPASS_ID_YAS530) + { + fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f); + fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f); + fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f); + + f.update(&handle, fcin, fcout); + + inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f); + inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f); + inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f); + } +#endif + + if (SUPERVISOR_DEBUG) { + MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n", + (float)inv_obj.compass_calibrated_data[0] / + 65536.f, + (float)inv_obj.compass_calibrated_data[1] / + 65536.f, + (float)inv_obj.compass_calibrated_data[2] / + 65536.f); + } + magFB = 1.0; + adjustSensorFusion = 1; + result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + } + } else { + //No compass, but still modify accel + unsigned long ctime = inv_get_tick_count(); + if (polltime == 0 || ((ctime - polltime) > 80)) { // at the beginning AND every 1/8 second + unsigned long deltaTime = 1; + adjustSensorFusion = 1; + magFB = 0; + if (polltime != 0) { + deltaTime = ctime - polltime; + } + MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); + polltime = ctime; + } + } + if (adjustSensorFusion == 1) { + unsigned char regs[4]; + static int prevAccSF = 1; + + if (accSF != prevAccSF) { + regs[0] = (unsigned char)((accSF >> 24) & 0xff); + regs[1] = (unsigned char)((accSF >> 16) & 0xff); + regs[2] = (unsigned char)((accSF >> 8) & 0xff); + regs[3] = (unsigned char)(accSF & 0xff); + result = inv_set_mpu_memory(KEY_D_0_96, 4, regs); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + prevAccSF = accSF; + } + } + + if (ml_supervisor_cb.accel_compass_fusion_func != NULL) + ml_supervisor_cb.accel_compass_fusion_func(magFB); + + return INV_SUCCESS; +} + +/** + * @brief Entry point for software sensor fusion operations. + * Manages hardware interaction, calls sensor fusion supervisor for + * bias calculation. + * @return INV_SUCCESS or non-zero error code on error. + */ +inv_error_t inv_pressure_supervisor(void) +{ + long pressureSensorData[1]; + static unsigned long pressurePolltime = 0; + if (inv_pressure_present()) { /* check for pressure data */ + unsigned long ctime = inv_get_tick_count(); + if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second + if (SUPERVISOR_DEBUG) { + MPL_LOGV("Fetch pressure data\n"); + MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime); + } + pressurePolltime = ctime; + if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) { + inv_obj.pressure = pressureSensorData[0]; + } + } + } + return INV_SUCCESS; +} + +/** + * @brief Resets the magnetometer calibration algorithm. + * @return INV_SUCCESS if successful, or non-zero error code otherwise. + */ +inv_error_t inv_reset_compass_calibration(void) +{ + if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) { + if (ml_supervisor_cb.reset_advanced_compass_func != NULL) + ml_supervisor_cb.reset_advanced_compass_func(); + } + MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1); + + inv_obj.compass_bias_error[0] = P_INIT; + inv_obj.compass_bias_error[1] = P_INIT; + inv_obj.compass_bias_error[2] = P_INIT; + inv_obj.compass_accuracy = 0; + + inv_obj.got_compass_bias = 0; + inv_obj.got_init_compass_bias = 0; + inv_obj.compass_state = SF_UNCALIBRATED; + inv_obj.resetting_compass = 1; + + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/mlsupervisor.h b/60xx/mlsdk/mllite/mlsupervisor.h new file mode 100644 index 0000000..62b427e --- /dev/null +++ b/60xx/mlsdk/mllite/mlsupervisor.h @@ -0,0 +1,71 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: mlsupervisor.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *****************************************************************************/ + +#ifndef __INV_SUPERVISOR_H__ +#define __INV_SUPERVISOR_H__ + +#include "mltypes.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "mlsupervisor_legacy.h" +#endif + +// The value of inv_get_gyro_sum_of_sqr is scaled such the (1 dps)^2 = 2^this_number +// this number must be >=0 and even. +#define GYRO_MAG_SQR_SHIFT 6 +// The value of inv_accel_sum_of_sqr is scaled such that (1g)^2 = 2^this_number +#define ACC_MAG_SQR_SHIFT 16 + +#define CAL_RUN 0 +#define CAL_RESET 1 +#define CAL_CHANGED_DATA 2 +#define CAL_RESET_TIME 3 +#define CAL_ADD_DATA 4 +#define CAL_COMBINE 5 + +#define P_INIT 100000 + +#define SF_NORMAL 0 +#define SF_DISTURBANCE 1 +#define SF_FAST_SETTLE 2 +#define SF_SLOW_SETTLE 3 +#define SF_STARTUP_SETTLE 4 +#define SF_UNCALIBRATED 5 + +struct inv_supervisor_cb_obj { + void (*accel_compass_fusion_func) (double magFB); + inv_error_t(*progressive_no_motion_supervisor_func) (unsigned long + deltaTime); + inv_error_t(*sensor_fusion_advanced_func) (double *magFB, + unsigned long deltaTime); + void (*reset_advanced_compass_func) (void); + void (*supervisor_reset_func) (void); +}; + +inv_error_t inv_reset_compass_calibration(void); +void inv_init_sensor_fusion_supervisor(void); +inv_error_t inv_accel_compass_supervisor(void); +inv_error_t inv_pressure_supervisor(void); + +#endif // __INV_SUPERVISOR_H__ + diff --git a/60xx/mlsdk/mllite/pressure.c b/60xx/mlsdk/mllite/pressure.c new file mode 100644 index 0000000..9c162ec --- /dev/null +++ b/60xx/mlsdk/mllite/pressure.c @@ -0,0 +1,166 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: pressure.c 4120 2010-11-21 19:56:16Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup PRESSUREDL + * @brief Motion Library - Pressure Driver Layer. + * Provides the interface to setup and handle a pressure sensor + * connected to either the primary or the seconday I2C interface + * of the gyroscope. + * + * @{ + * @file pressure.c + * @brief Pressure setup and handling methods. +**/ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ + +#include + +#include "pressure.h" + +#include "ml.h" +#include "mlinclude.h" +#include "dmpKey.h" +#include "mlFIFO.h" +#include "mldl.h" +#include "mldl_cfg.h" +#include "mlMathFunc.h" +#include "mlsl.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-pressure" + +#define _pressureDebug(x) //{x} + +/* --------------------- */ +/* - Global Variables. - */ +/* --------------------- */ + +/* --------------------- */ +/* - Static Variables. - */ +/* --------------------- */ + +/* --------------- */ +/* - Prototypes. - */ +/* --------------- */ + +/* -------------- */ +/* - Externs. - */ +/* -------------- */ + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Is a pressure configured and used by MPL? + * @return INV_SUCCESS if the pressure is present. + */ +unsigned char inv_pressure_present(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->pressure && + NULL != mldl_cfg->pressure->resume && + mldl_cfg->requested_sensors & INV_THREE_AXIS_PRESSURE) + return TRUE; + else + return FALSE; +} + +/** + * @brief Query the pressure slave address. + * @return The 7-bit pressure slave address. + */ +unsigned char inv_get_pressure_slave_addr(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->pdata) + return mldl_cfg->pdata->pressure.address; + else + return 0; +} + +/** + * @brief Get the ID of the pressure in use. + * @return ID of the pressure in use. + */ +unsigned short inv_get_pressure_id(void) +{ + INVENSENSE_FUNC_START; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + if (NULL != mldl_cfg->pressure) { + return mldl_cfg->pressure->id; + } + return ID_INVALID; +} + +/** + * @brief Get a sample of pressure data from the device. + * @param data + * the buffer to store the pressure raw data for + * X, Y, and Z axes. + * @return INV_SUCCESS or a non-zero error code. + */ +inv_error_t inv_get_pressure_data(long *data) +{ + inv_error_t result = INV_SUCCESS; + unsigned char tmp[3]; + struct mldl_cfg *mldl_cfg = inv_get_dl_config(); + + /*--- read the pressure sensor data. + The pressure read function may return an INV_ERROR_PRESSURE_* errors + when the data is not ready (read/refresh frequency mismatch) or + the internal data sampling timing of the device was not respected. + Returning the error code will make the sensor fusion supervisor + ignore this pressure data sample. ---*/ + result = (inv_error_t) inv_mpu_read_pressure(mldl_cfg, + inv_get_serial_handle(), + inv_get_serial_handle(), tmp); + if (result) { + _pressureDebug(MPL_LOGV + ("inv_mpu_read_pressure returned %d (%s)\n", result, + MLErrorCode(result))); + return result; + } + if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->pressure->endian) + data[0] = + (((long)((signed char)tmp[0])) << 16) + (((long)tmp[1]) << 8) + + ((long)tmp[2]); + else + data[0] = + (((long)((signed char)tmp[2])) << 16) + (((long)tmp[1]) << 8) + + ((long)tmp[0]); + + return INV_SUCCESS; +} + +/** + * @} + */ diff --git a/60xx/mlsdk/mllite/pressure.h b/60xx/mlsdk/mllite/pressure.h new file mode 100644 index 0000000..77c5d43 --- /dev/null +++ b/60xx/mlsdk/mllite/pressure.h @@ -0,0 +1,71 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: pressure.h 4092 2010-11-17 23:49:22Z kkeal $ + * + *******************************************************************************/ + +#ifndef PRESSURE_H +#define PRESSURE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#include "mpu.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "pressure_legacy.h" +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + +#define USE_PRESSURE_BMA 0 + +#define PRESSURE_SLAVEADDR_INVALID 0x00 +#define PRESSURE_SLAVEADDR_BMA085 0x77 + +/* + Define default pressure to use if no selection is made +*/ +#if USE_PRESSURE_BMA +#define DEFAULT_PRESSURE_TYPE PRESSURE_ID_BMA +#endif + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + + unsigned char inv_pressure_present(void); + unsigned char inv_get_pressure_slave_addr(void); + inv_error_t inv_suspend_pressure(void); + inv_error_t inv_resume_presure(void); + inv_error_t inv_get_pressure_data(long *data); + unsigned short inv_get_pressure_id(void); + +#ifdef __cplusplus +} +#endif +#endif // PRESSURE_H diff --git a/60xx/mlsdk/mlutils/checksum.c b/60xx/mlsdk/mlutils/checksum.c new file mode 100644 index 0000000..a97477d --- /dev/null +++ b/60xx/mlsdk/mlutils/checksum.c @@ -0,0 +1,16 @@ +#include "mltypes.h" + +/** bernstein hash, from public domain source */ + +uint32_t inv_checksum(unsigned char *str, int len) +{ + uint32_t hash = 5381; + int i, c; + + for (i = 0; i < len; i++) { + c = *(str + i); + hash = ((hash << 5) + hash) + c; /* hash * 33 + c */ + } + + return hash; +} diff --git a/60xx/mlsdk/mlutils/checksum.h b/60xx/mlsdk/mlutils/checksum.h new file mode 100644 index 0000000..4d3f046 --- /dev/null +++ b/60xx/mlsdk/mlutils/checksum.h @@ -0,0 +1,18 @@ +#ifndef MPLCHECKSUM_H +#define MPLCHECKSUM_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mltypes.h" +#ifdef INV_INCLUDE_LEGACY_HEADERS +#include "checksum_legacy.h" +#endif + + uint32_t inv_checksum(unsigned char *str, int len); + +#ifdef __cplusplus +} +#endif +#endif /* MPLCHECKSUM_H */ diff --git a/60xx/mlsdk/mlutils/mputest.c b/60xx/mlsdk/mlutils/mputest.c new file mode 100644 index 0000000..ac0fa30 --- /dev/null +++ b/60xx/mlsdk/mlutils/mputest.c @@ -0,0 +1,1396 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: mputest.c 5637 2011-06-14 01:13:53Z mcaramello $ + * + *****************************************************************************/ + +/* + * MPU Self Test functions + * Version 2.4 + * May 13th, 2011 + */ + +/** + * @defgroup MPU_SELF_TEST + * @brief MPU Self Test functions + * + * These functions provide an in-site test of the MPU 3xxx chips. The main + * entry point is the inv_mpu_test function. + * This runs the tests (as described in the accompanying documentation) and + * writes a configuration file containing initial calibration data. + * inv_mpu_test returns INV_SUCCESS if the chip passes the tests. + * Otherwise, an error code is returned. + * The functions in this file rely on MLSL and MLOS: refer to the MPL + * documentation for more information regarding the system interface + * files. + * + * @{ + * @file mputest.c + * @brief MPU Self Test routines for assessing gyro sensor status + * after surface mount has happened on the target host platform. + */ + +#include +#include +#include +#include +#include +#ifdef LINUX +#include +#endif + +#include "mpu.h" +#include "mldl.h" +#include "mldl_cfg.h" +#include "accel.h" +#include "mlFIFO.h" +#include "slave.h" +#include "ml.h" +#include "ml_stored_data.h" +#include "checksum.h" + +#include "mlsl.h" +#include "mlos.h" + +#include "log.h" +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mpust" + +#ifdef __cplusplus +extern "C" { +#endif + +/* + Defines +*/ + +#define VERBOSE_OUT 0 + +/*#define TRACK_IDS*/ + +/*--- Test parameters defaults. See set_test_parameters for more details ---*/ + +#define DEF_MPU_ADDR (0x68) /* I2C address of the mpu */ + +#if (USE_SENSE_PATH_TEST == 1) /* gyro full scale dps */ +#define DEF_GYRO_FULLSCALE (2000) +#else +#define DEF_GYRO_FULLSCALE (250) +#endif + +#define DEF_GYRO_SENS (32768.f / DEF_GYRO_FULLSCALE) + /* gyro sensitivity LSB/dps */ +#define DEF_PACKET_THRESH (75) /* 600 ms / 8ms / sample */ +#define DEF_TOTAL_TIMING_TOL (.03f) /* 3% = 2 pkts + 1% proc tol. */ +#define DEF_BIAS_THRESH (40 * DEF_GYRO_SENS) + /* 40 dps in LSBs */ +#define DEF_RMS_THRESH (0.4f * DEF_GYRO_SENS) + /* 0.4 dps-rms in LSB-rms */ +#define DEF_SP_SHIFT_THRESH_CUST (.05f) /* 5% */ +#define DEF_TEST_TIME_PER_AXIS (600) /* ms of time spent collecting + data for each axis, + multiple of 600ms */ +#define DEF_N_ACCEL_SAMPLES (20) /* num of accel samples to + average from, if applic. */ + +#define ML_INIT_CAL_LEN (36) /* length in bytes of + calibration data file */ + +/* + Macros +*/ + +#define CHECK_TEST_ERROR(x) \ + if (x) { \ + MPL_LOGI("error %d @ %s|%d\n", x, __func__, __LINE__); \ + return (-1); \ + } + +#define SHORT_TO_TEMP_C(shrt) (((shrt+13200.f)/280.f)+35.f) + +#define USHORT_TO_CHARS(chr,shrt) (chr)[0]=(unsigned char)(shrt>>8); \ + (chr)[1]=(unsigned char)(shrt); + +#define UINT_TO_CHARS(chr,ui) (chr)[0]=(unsigned char)(ui>>24); \ + (chr)[1]=(unsigned char)(ui>>16); \ + (chr)[2]=(unsigned char)(ui>>8); \ + (chr)[3]=(unsigned char)(ui); + +#define FLOAT_TO_SHORT(f) ( \ + (fabs(f-(short)f)>=0.5) ? ( \ + ((short)f)+(f<0?(-1):(+1))) : \ + ((short)f) \ + ) + +#define CHARS_TO_SHORT(d) ((((short)(d)[0])<<8)+(d)[1]) +#define CHARS_TO_SHORT_SWAPPED(d) ((((short)(d)[1])<<8)+(d)[0]) + +#define ACCEL_UNPACK(d) d[0], d[1], d[2], d[3], d[4], d[5] + +#define CHECK_NACKS(d) ( \ + d[0]==0xff && d[1]==0xff && \ + d[2]==0xff && d[3]==0xff && \ + d[4]==0xff && d[5]==0xff \ + ) + +/* + Prototypes +*/ + +static inv_error_t test_get_data( + void *mlsl_handle, + struct mldl_cfg *mputestCfgPtr, + short *vals); + +/* + Types +*/ +typedef struct { + float gyro_sens; + int gyro_fs; + int packet_thresh; + float total_timing_tol; + int bias_thresh; + float rms_threshSq; + float sp_shift_thresh; + unsigned int test_time_per_axis; + unsigned short accel_samples; +} tTestSetup; + +/* + Global variables +*/ +static unsigned char dataout[20]; +static unsigned char dataStore[ML_INIT_CAL_LEN]; + +static tTestSetup test_setup = { + DEF_GYRO_SENS, + DEF_GYRO_FULLSCALE, + DEF_PACKET_THRESH, + DEF_TOTAL_TIMING_TOL, + (int)DEF_BIAS_THRESH, + DEF_RMS_THRESH * DEF_RMS_THRESH, + DEF_SP_SHIFT_THRESH_CUST, + DEF_TEST_TIME_PER_AXIS, + DEF_N_ACCEL_SAMPLES +}; + +static float adjGyroSens; +static char a_name[3][2] = {"X", "Y", "Z"}; + +/* + NOTE : modify get_slave_descr parameter below to reflect + the DEFAULT accelerometer in use. The accelerometer in use + can be modified at run-time using the inv_test_setup_accel API. + NOTE : modify the expected z axis orientation (Z axis pointing + upward or downward) +*/ + +signed char g_z_sign = +1; +struct mldl_cfg *mputestCfgPtr = NULL; + +#ifndef LINUX +/** + * @internal + * @brief usec precision sleep function. + * @param number of micro seconds (us) to sleep for. + */ +static void usleep(unsigned long t) +{ + unsigned long start = inv_get_tick_count(); + while (inv_get_tick_count()-start < t / 1000); +} +#endif + +/** + * @brief Modify the self test limits from their default values. + * + * @param slave_addr + * the slave address the MPU device is setup to respond at. + * The default is DEF_MPU_ADDR = 0x68. + * @param sensitivity + * the read sensitivity of the device in LSB/dps as it is trimmed. + * NOTE : if using the self test as part of the MPL, the + * sensitivity the different sensitivity trims are already + * taken care of. + * @param p_thresh + * number of packets expected to be received in a 600 ms period. + * Depends on the sampling frequency of choice (set by default to + * 125 Hz) and low pass filter cut-off frequency selection (set + * to 42 Hz). + * The default is DEF_PACKET_THRESH = 75 packets. + * @param total_time_tol + * time skew tolerance, taking into account imprecision in turning + * the FIFO on and off and the processor time imprecision (for + * 1 GHz processor). + * The default is DEF_TOTAL_TIMING_TOL = 3 %, about 2 packets. + * @param bias_thresh + * bias level threshold, the maximun acceptable no motion bias + * for a production quality part. + * The default is DEF_BIAS_THRESH = 40 dps. + * @param rms_thresh + * the limit standard deviation (=~ RMS) set to assess whether + * the noise level on the part is acceptable. + * The default is DEF_RMS_THRESH = 0.2 dps-rms. + * @param sp_shift_thresh + * the limit shift applicable to the Sense Path self test + * calculation. + */ +void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, + int p_thresh, float total_time_tol, + int bias_thresh, float rms_thresh, + float sp_shift_thresh, + unsigned short accel_samples) +{ + mputestCfgPtr->addr = slave_addr; + test_setup.gyro_sens = sensitivity; + test_setup.gyro_fs = (int)(32768.f / sensitivity); + test_setup.packet_thresh = p_thresh; + test_setup.total_timing_tol = total_time_tol; + test_setup.bias_thresh = bias_thresh; + test_setup.rms_threshSq = rms_thresh * rms_thresh; + test_setup.sp_shift_thresh = sp_shift_thresh; + test_setup.accel_samples = accel_samples; +} + +#define X (0) +#define Y (1) +#define Z (2) + +#ifdef CONFIG_MPU_SENSORS_MPU3050 +/** + * @brief Test the gyroscope sensor. + * Implements the core logic of the MPU Self Test. + * Produces the PASS/FAIL result. Loads the calculated gyro biases + * and temperature datum into the corresponding pointers. + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @param gyro_biases + * output pointer to store the initial bias calculation provided + * by the MPU Self Test. Requires 3 elements for gyro X, Y, + * and Z. + * @param temp_avg + * output pointer to store the initial average temperature as + * provided by the MPU Self Test. + * @param perform_full_test + * If 1: + * calculates offset, drive frequency, and noise and compare it + * against set thresholds. + * Report also the final result using a bit-mask like error code + * as explained in return value description. + * When 0: + * skip the noise and drive frequency calculation and pass/fail + * assessment; simply calculates the gyro biases. + * + * @return 0 on success. + * On error, the return value is a bitmask representing: + * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively + * (decimal values will be 1, 2, 4 respectively). + * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively + * (decimal values will be 8, 16, 32 respectively). + * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively + * (decimal values will be 64, 128, 256 respectively). + * 9 If any of the RMS noise values is zero, it could be + * due to a non-functional gyro or FIFO/register failure. + * (decimal value will be 512). + * (decimal values will be 1024, 2048, 4096 respectively). + */ +int inv_test_gyro_3050(void *mlsl_handle, + short gyro_biases[3], short *temp_avg, + uint_fast8_t perform_full_test) +{ + int retVal = 0; + inv_error_t result; + + int total_count = 0; + int total_count_axis[3] = {0, 0, 0}; + int packet_count; + short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; + short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; + short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; + unsigned char regs[7]; + + int temperature; + float Avg[3]; + float RMS[3]; + int i, j, tmp; + char tmpStr[200]; + + temperature = 0; + + /* sample rate = 8ms */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_SMPLRT_DIV, 0x07); + CHECK_TEST_ERROR(result); + + regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ + switch (DEF_GYRO_FULLSCALE) { + case 2000: + regs[0] |= 0x18; + break; + case 1000: + regs[0] |= 0x10; + break; + case 500: + regs[0] |= 0x08; + break; + case 250: + default: + regs[0] |= 0x00; + break; + } + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_DLPF_FS_SYNC, regs[0]); + CHECK_TEST_ERROR(result); + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_INT_CFG, 0x00); + CHECK_TEST_ERROR(result); + + /* 1st, timing test */ + for (j = 0; j < 3; j++) { + + MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); + + /* turn on all gyros, use gyro X for clocking + Set to Y and Z for 2nd and 3rd iteration */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, j + 1); + CHECK_TEST_ERROR(result); + + /* wait for 2 ms after switching clock source */ + usleep(2000); + + /* we will enable XYZ gyro in FIFO and nothing else */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_FIFO_EN2, 0x00); + CHECK_TEST_ERROR(result); + /* enable/reset FIFO */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); + CHECK_TEST_ERROR(result); + + tmp = (int)(test_setup.test_time_per_axis / 600); + while (tmp-- > 0) { + /* enable XYZ gyro in FIFO and nothing else */ + result = inv_serial_single_write(mlsl_handle, + mputestCfgPtr->addr, MPUREG_FIFO_EN1, + BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); + CHECK_TEST_ERROR(result); + + /* wait for 600 ms for data */ + usleep(600000); + + /* stop storing gyro in the FIFO */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_FIFO_EN1, 0x00); + CHECK_TEST_ERROR(result); + + /* Getting number of bytes in FIFO */ + result = inv_serial_read( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_FIFO_COUNTH, 2, dataout); + CHECK_TEST_ERROR(result); + /* number of 6 B packets in the FIFO */ + packet_count = CHARS_TO_SHORT(dataout) / 6; + sprintf(tmpStr, "Packet Count: %d - ", packet_count); + + if ( abs(packet_count - test_setup.packet_thresh) + <= /* Within +/- total_timing_tol % range */ + test_setup.total_timing_tol * test_setup.packet_thresh) { + for (i = 0; i < packet_count; i++) { + /* getting FIFO data */ + result = inv_serial_read_fifo(mlsl_handle, + mputestCfgPtr->addr, 6, dataout); + CHECK_TEST_ERROR(result); + x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); + y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); + z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); + if (VERBOSE_OUT) { + MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", + total_count + i, x[total_count + i], + y[total_count + i], z[total_count + i]); + } + } + total_count += packet_count; + total_count_axis[j] += packet_count; + sprintf(tmpStr, "%sOK", tmpStr); + } else { + if (perform_full_test) + retVal |= 1 << j; + sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); + } + MPL_LOGI("%s\n", tmpStr); + } + + /* remove gyros from FIFO */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_FIFO_EN1, 0x00); + CHECK_TEST_ERROR(result); + + /* Read Temperature */ + result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, + MPUREG_TEMP_OUT_H, 2, dataout); + CHECK_TEST_ERROR(result); + temperature += (short)CHARS_TO_SHORT(dataout); + } + + MPL_LOGI("\n"); + MPL_LOGI("Total %d samples\n", total_count); + MPL_LOGI("\n"); + + /* 2nd, check bias from X and Y PLL clock source */ + tmp = total_count != 0 ? total_count : 1; + for (i = 0, + Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; + i < total_count; i++) { + Avg[X] += 1.f * x[i] / tmp; + Avg[Y] += 1.f * y[i] / tmp; + Avg[Z] += 1.f * z[i] / tmp; + } + MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", + Avg[X], Avg[Y], Avg[Z]); + if (VERBOSE_OUT) { + MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", + Avg[X] / adjGyroSens, + Avg[Y] / adjGyroSens, + Avg[Z] / adjGyroSens); + } + if(perform_full_test) { + for (j = 0; j < 3; j++) { + if (fabs(Avg[j]) > test_setup.bias_thresh) { + MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " + "(threshold = %d)\n", + a_name[j], Avg[j], test_setup.bias_thresh); + retVal |= 1 << (3+j); + } + } + } + + /* 3rd, check RMS */ + if (perform_full_test) { + for (i = 0, + RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; + i < total_count; i++) { + RMS[X] += (x[i] - Avg[X]) * (x[i] - Avg[X]); + RMS[Y] += (y[i] - Avg[Y]) * (y[i] - Avg[Y]); + RMS[Z] += (z[i] - Avg[Z]) * (z[i] - Avg[Z]); + } + for (j = 0; j < 3; j++) { + if (RMS[j] > test_setup.rms_threshSq * total_count) { + MPL_LOGI("%s-Gyro RMS (%.2f) exceeded threshold " + "(threshold = %.2f)\n", + a_name[j], sqrt(RMS[j] / total_count), + sqrt(test_setup.rms_threshSq)); + retVal |= 1 << (6+j); + } + } + + MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", + sqrt(RMS[X] / total_count), + sqrt(RMS[Y] / total_count), + sqrt(RMS[Z] / total_count)); + if (VERBOSE_OUT) { + MPL_LOGI("RMS ^ 2 : %+13.3f %+13.3f %+13.3f\n", + RMS[X] / total_count, + RMS[Y] / total_count, + RMS[Z] / total_count); + } + + if (RMS[X] == 0 || RMS[Y] == 0 || RMS[Z] == 0) { + /* If any of the RMS noise value returns zero, + then we might have dead gyro or FIFO/register failure, + the part is sleeping, or the part is not responsive */ + retVal |= 1 << 9; + } + } + + /* 4th, temperature average */ + temperature /= 3; + if (VERBOSE_OUT) + MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", + SHORT_TO_TEMP_C(temperature), "", ""); + + /* load into final storage */ + *temp_avg = (short)temperature; + gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); + gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); + gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); + + return retVal; +} + +#else /* CONFIG_MPU_SENSORS_MPU3050 */ + +/** + * @brief Test the gyroscope sensor. + * Implements the core logic of the MPU Self Test but does not provide + * a PASS/FAIL output as in the MPU-3050 implementation. + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @param gyro_biases + * output pointer to store the initial bias calculation provided + * by the MPU Self Test. Requires 3 elements for gyro X, Y, + * and Z. + * @param temp_avg + * output pointer to store the initial average temperature as + * provided by the MPU Self Test. + * + * @return 0 on success. + * A non-zero error code on error. + */ +int inv_test_gyro_6050(void *mlsl_handle, + short gyro_biases[3], short *temp_avg) +{ + inv_error_t result; + + int total_count = 0; + int total_count_axis[3] = {0, 0, 0}; + int packet_count; + short x[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; + short y[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; + short z[DEF_TEST_TIME_PER_AXIS / 8 * 4] = {0}; + unsigned char regs[7]; + + int temperature = 0; + float Avg[3]; + int i, j, tmp; + char tmpStr[200]; + + /* sample rate = 8ms */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_SMPLRT_DIV, 0x07); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ + switch (DEF_GYRO_FULLSCALE) { + case 2000: + regs[0] |= 0x18; + break; + case 1000: + regs[0] |= 0x10; + break; + case 500: + regs[0] |= 0x08; + break; + case 250: + default: + regs[0] |= 0x00; + break; + } + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_CONFIG, regs[0]); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_INT_ENABLE, 0x00); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* 1st, timing test */ + for (j = 0; j < 3; j++) { + MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); + + /* turn on all gyros, use gyro X for clocking + Set to Y and Z for 2nd and 3rd iteration */ +#ifdef CONFIG_MPU_SENSORS_MPU6050A2 + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGMT_1, BITS_PWRSEL | (j + 1)); +#else + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGMT_1, j + 1); +#endif + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* wait for 2 ms after switching clock source */ + usleep(2000); + + /* enable/reset FIFO */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + tmp = (int)(test_setup.test_time_per_axis / 600); + while (tmp-- > 0) { + /* enable XYZ gyro in FIFO and nothing else */ + result = inv_serial_single_write(mlsl_handle, + mputestCfgPtr->addr, MPUREG_FIFO_EN, + BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* wait for 600 ms for data */ + usleep(600000); + /* stop storing gyro in the FIFO */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_FIFO_EN, 0x00); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* Getting number of bytes in FIFO */ + result = inv_serial_read( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_FIFO_COUNTH, 2, dataout); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + /* number of 6 B packets in the FIFO */ + packet_count = CHARS_TO_SHORT(dataout) / 6; + sprintf(tmpStr, "Packet Count: %d - ", packet_count); + + if (abs(packet_count - test_setup.packet_thresh) + <= /* Within +/- total_timing_tol % range */ + test_setup.total_timing_tol * test_setup.packet_thresh) { + for (i = 0; i < packet_count; i++) { + /* getting FIFO data */ + result = inv_serial_read_fifo(mlsl_handle, + mputestCfgPtr->addr, 6, dataout); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + x[total_count + i] = CHARS_TO_SHORT(&dataout[0]); + y[total_count + i] = CHARS_TO_SHORT(&dataout[2]); + z[total_count + i] = CHARS_TO_SHORT(&dataout[4]); + if (VERBOSE_OUT) { + MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", + total_count + i, x[total_count + i], + y[total_count + i], z[total_count + i]); + } + } + total_count += packet_count; + total_count_axis[j] += packet_count; + sprintf(tmpStr, "%sOK", tmpStr); + } else { + sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); + } + MPL_LOGI("%s\n", tmpStr); + } + + /* remove gyros from FIFO */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_FIFO_EN, 0x00); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* Read Temperature */ + result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, + MPUREG_TEMP_OUT_H, 2, dataout); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + temperature += (short)CHARS_TO_SHORT(dataout); + } + + MPL_LOGI("\n"); + MPL_LOGI("Total %d samples\n", total_count); + MPL_LOGI("\n"); + + /* 2nd, check bias from X and Y PLL clock source */ + tmp = total_count != 0 ? total_count : 1; + for (i = 0, + Avg[X] = .0f, Avg[Y] = .0f, Avg[Z] = .0f; + i < total_count; i++) { + Avg[X] += 1.f * x[i] / tmp; + Avg[Y] += 1.f * y[i] / tmp; + Avg[Z] += 1.f * z[i] / tmp; + } + MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", + Avg[X], Avg[Y], Avg[Z]); + if (VERBOSE_OUT) { + MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", + Avg[X] / adjGyroSens, + Avg[Y] / adjGyroSens, + Avg[Z] / adjGyroSens); + } + + temperature /= 3; + if (VERBOSE_OUT) + MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", + SHORT_TO_TEMP_C(temperature), "", ""); + + /* load into final storage */ + *temp_avg = (short)temperature; + gyro_biases[X] = FLOAT_TO_SHORT(Avg[X]); + gyro_biases[Y] = FLOAT_TO_SHORT(Avg[Y]); + gyro_biases[Z] = FLOAT_TO_SHORT(Avg[Z]); + + return INV_SUCCESS; +} + +#endif /* CONFIG_MPU_SENSORS_MPU3050 */ + +#ifdef TRACK_IDS +/** + * @internal + * @brief Retrieve the unique MPU device identifier from the internal OTP + * bank 0 memory. + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @return 0 on success, a non-zero error code from the serial layer on error. + */ +static inv_error_t test_get_mpu_id(void *mlsl_handle) +{ + inv_error_t result; + unsigned char otp0[8]; + + + result = + inv_serial_read_mem(mlsl_handle, mputestCfgPtr->addr, + (BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 | + 0x00, 6, otp0); + if (result) + goto close; + + MPL_LOGI("\n"); + MPL_LOGI("DIE_ID : %06X\n", + ((int)otp0[1] << 8 | otp0[0]) & 0x1fff); + MPL_LOGI("WAFER_ID : %06X\n", + (((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5); + MPL_LOGI("A_LOT_ID : %06X\n", + ( ((int)otp0[4] << 16 | (int)otp0[3] << 8 | + otp0[2]) & 0x3ffff) >> 2); + MPL_LOGI("W_LOT_ID : %06X\n", + ( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2); + MPL_LOGI("WP_ID : %06X\n", + ( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7); + MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2); + MPL_LOGI("\n"); + +close: + result = + inv_serial_single_write(mlsl_handle, mputestCfgPtr->addr, MPUREG_BANK_SEL, 0x00); + return result; +} +#endif /* TRACK_IDS */ + +/** + * @brief If requested via inv_test_setup_accel(), test the accelerometer biases + * and calculate the necessary bias correction. + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @param bias + * output pointer to store the initial bias calculation provided + * by the MPU Self Test. Requires 3 elements to store accel X, Y, + * and Z axis bias. + * @param perform_full_test + * If 1: + * calculates offsets and noise and compare it against set + * thresholds. The final exist status will reflect if any of the + * value is outside of the expected range. + * When 0; + * skip the noise calculation and pass/fail assessment; simply + * calculates the accel biases. + * + * @return 0 on success. A non-zero error code on error. + */ +int inv_test_accel(void *mlsl_handle, + short *bias, long gravity, + uint_fast8_t perform_full_test) +{ + int i; + + short *p_vals; + float x = 0.f, y = 0.f, z = 0.f, zg = 0.f; + float RMS[3]; + float accelRmsThresh = 1000000.f; /* enourmous so that the test always + passes - future deployment */ + unsigned short res; + unsigned long orig_requested_sensors; + struct mpu_platform_data *mputestPData = mputestCfgPtr->pdata; + + p_vals = (short*)inv_malloc(sizeof(short) * 3 * test_setup.accel_samples); + + /* load the slave descr from the getter */ + if (mputestPData->accel.get_slave_descr == NULL) { + MPL_LOGI("\n"); + MPL_LOGI("No accelerometer configured\n"); + return 0; + } + if (mputestCfgPtr->accel == NULL) { + MPL_LOGI("\n"); + MPL_LOGI("No accelerometer configured\n"); + return 0; + } + + /* resume the accel */ + orig_requested_sensors = mputestCfgPtr->requested_sensors; + mputestCfgPtr->requested_sensors = INV_THREE_AXIS_ACCEL | INV_THREE_AXIS_GYRO; + res = inv_mpu_resume(mputestCfgPtr, + mlsl_handle, NULL, NULL, NULL, + mputestCfgPtr->requested_sensors); + if(res != INV_SUCCESS) + goto accel_error; + + /* wait at least a sample cycle for the + accel data to be retrieved by MPU */ + inv_sleep(inv_mpu_get_sampling_period_us(mputestCfgPtr) / 1000); + + /* collect the samples */ + for(i = 0; i < test_setup.accel_samples; i++) { + short *vals = &p_vals[3 * i]; + if (test_get_data(mlsl_handle, mputestCfgPtr, vals)) { + goto accel_error; + } + x += 1.f * vals[X] / test_setup.accel_samples; + y += 1.f * vals[Y] / test_setup.accel_samples; + z += 1.f * vals[Z] / test_setup.accel_samples; + } + + mputestCfgPtr->requested_sensors = orig_requested_sensors; + res = inv_mpu_suspend(mputestCfgPtr, + mlsl_handle, NULL, NULL, NULL, + INV_ALL_SENSORS); + if (res != INV_SUCCESS) + goto accel_error; + + MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (LSB)\n", x, y, z); + if (VERBOSE_OUT) { + MPL_LOGI("Accel biases : %+13.3f %+13.3f %+13.3f (gee)\n", + x / gravity, y / gravity, z / gravity); + } + + bias[0] = FLOAT_TO_SHORT(x); + bias[1] = FLOAT_TO_SHORT(y); + zg = z - g_z_sign * gravity; + bias[2] = FLOAT_TO_SHORT(zg); + + MPL_LOGI("Accel correct.: %+13d %+13d %+13d (LSB)\n", + bias[0], bias[1], bias[2]); + if (VERBOSE_OUT) { + MPL_LOGI("Accel correct.: " + "%+13.3f %+13.3f %+13.3f (gee)\n", + 1.f * bias[0] / gravity, + 1.f * bias[1] / gravity, + 1.f * bias[2] / gravity); + } + + if (perform_full_test) { + /* accel RMS - for now the threshold is only indicative */ + for (i = 0, + RMS[X] = 0.f, RMS[Y] = 0.f, RMS[Z] = 0.f; + i < test_setup.accel_samples; i++) { + short *vals = &p_vals[3 * i]; + RMS[X] += (vals[X] - x) * (vals[X] - x); + RMS[Y] += (vals[Y] - y) * (vals[Y] - y); + RMS[Z] += (vals[Z] - z) * (vals[Z] - z); + } + for (i = 0; i < 3; i++) { + if (RMS[i] > accelRmsThresh * accelRmsThresh + * test_setup.accel_samples) { + MPL_LOGI("%s-Accel RMS (%.2f) exceeded threshold " + "(threshold = %.2f)\n", + a_name[i], sqrt(RMS[i] / test_setup.accel_samples), + accelRmsThresh); + goto accel_error; + } + } + MPL_LOGI("RMS : %+13.3f %+13.3f %+13.3f (LSB-rms)\n", + sqrt(RMS[X] / DEF_N_ACCEL_SAMPLES), + sqrt(RMS[Y] / DEF_N_ACCEL_SAMPLES), + sqrt(RMS[Z] / DEF_N_ACCEL_SAMPLES)); + } + + return 0; /* success */ + +accel_error: /* error */ + bias[0] = bias[1] = bias[2] = 0; + return 1; +} + +/** + * @brief an user-space substitute of the power management function(s) + * in mldl_cfg.c for self test usage. + * Wake up and sleep the device, whether that is MPU3050, MPU6050A2, + * or MPU6050B1. + * @param mlsl_handle + * a file handle for the serial communication port used to + * communicate with the MPU device. + * @param power_level + * the power state to change the device into. Currently only 0 or + * 1 are supported, for sleep and full-power respectively. + * @return 0 on success; a non-zero error code on error. + */ +static inv_error_t inv_device_power_mgmt(void *mlsl_handle, + uint_fast8_t power_level) +{ + inv_error_t result; + static unsigned char pwr_mgm; + unsigned char b; + + if (power_level != 0 && power_level != 1) { + return INV_ERROR_INVALID_PARAMETER; + } + + if (power_level) { + result = inv_serial_read( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, 1, &pwr_mgm); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* reset */ + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, pwr_mgm | BIT_H_RESET); +#ifndef CONFIG_MPU_SENSORS_MPU6050A2 + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#endif + inv_sleep(5); + + /* re-read power mgmt reg - + it may have reset after H_RESET is applied */ + result = inv_serial_read( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, 1, &b); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + /* wake up */ +#ifdef CONFIG_MPU_SENSORS_MPU6050A2 + if ((b & BITS_PWRSEL) != BITS_PWRSEL) { + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, BITS_PWRSEL); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } +#else + if (pwr_mgm & BIT_SLEEP) { + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, 0x00); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } +#endif + inv_sleep(60); + +#if defined(CONFIG_MPU_SENSORS_MPU6050A2) || \ + defined(CONFIG_MPU_SENSORS_MPU6050B1) + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_INT_PIN_CFG, BIT_BYPASS_EN); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } +#endif + } else { + /* restore the power state the part was found in */ +#ifdef CONFIG_MPU_SENSORS_MPU6050A2 + if ((pwr_mgm & BITS_PWRSEL) != BITS_PWRSEL) { + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, pwr_mgm); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } +#else + if (pwr_mgm & BIT_SLEEP) { + result = inv_serial_single_write( + mlsl_handle, mputestCfgPtr->addr, + MPUREG_PWR_MGM, pwr_mgm); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } +#endif + } + + return INV_SUCCESS; +} + +/** + * @brief The main entry point of the MPU Self Test, triggering the run of + * the single tests, for gyros and accelerometers. + * Prepares the MPU for the test, taking the device out of low power + * state if necessary, switching the MPU secondary I2C interface into + * bypass mode and restoring the original power state at the end of + * the test. + * This function is also responsible for encoding the output of each + * test in the correct format as it is stored on the file/medium of + * choice (according to inv_serial_write_cal() function). + * The format needs to stay perfectly consistent with the one expected + * by the corresponding loader in ml_stored_data.c; currectly the + * loaded in use is inv_load_cal_V1 (record type 1 - initial + * calibration). + * + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @param provide_result + * If 1: + * perform and analyze the offset, drive frequency, and noise + * calculation and compare it against set threshouds. Report + * also the final result using a bit-mask like error code as + * described in the inv_test_gyro() function. + * When 0: + * skip the noise and drive frequency calculation and pass/fail + * assessment. It simply calculates the gyro and accel biases. + * NOTE: for MPU6050 devices, this parameter is currently + * ignored. + * + * @return 0 on success. A non-zero error code on error. + * Propagates the errors from the tests up to the caller. + */ +int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result) +{ + int result = 0; + + short temp_avg; + short gyro_biases[3] = {0, 0, 0}; + short accel_biases[3] = {0, 0, 0}; + + unsigned long testStart = inv_get_tick_count(); + long accelSens[3] = {0}; + int ptr; + int tmp; + long long lltmp; + uint32_t chk; + + MPL_LOGI("Collecting %d groups of 600 ms samples for each axis\n", + DEF_TEST_TIME_PER_AXIS / 600); + MPL_LOGI("\n"); + + result = inv_device_power_mgmt(mlsl_handle, TRUE); + +#ifdef TRACK_IDS + result = test_get_mpu_id(mlsl_handle); + if (result != INV_SUCCESS) { + MPL_LOGI("Could not read the device's unique ID\n"); + MPL_LOGI("\n"); + return result; + } +#endif + + /* adjust the gyro sensitivity according to the gyro_sens_trim value */ + adjGyroSens = test_setup.gyro_sens * mputestCfgPtr->gyro_sens_trim / 131.f; + test_setup.gyro_fs = (int)(32768.f / adjGyroSens); + + /* collect gyro and temperature data */ +#ifdef CONFIG_MPU_SENSORS_MPU3050 + result = inv_test_gyro_3050(mlsl_handle, + gyro_biases, &temp_avg, provide_result); +#else + MPL_LOGW_IF(provide_result, + "Self Test for MPU-6050 devices is for bias correction only: " + "no test PASS/FAIL result will be provided\n"); + result = inv_test_gyro_6050(mlsl_handle, gyro_biases, &temp_avg); +#endif + + MPL_LOGI("\n"); + MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - testStart); + if (result) + return result; + + /* collect accel data. if this step is skipped, + ensure the array still contains zeros. */ + if (mputestCfgPtr->accel != NULL) { + float fs; + RANGE_FIXEDPOINT_TO_FLOAT(mputestCfgPtr->accel->range, fs); + accelSens[0] = (long)(32768L / fs); + accelSens[1] = (long)(32768L / fs); + accelSens[2] = (long)(32768L / fs); +#if defined CONFIG_MPU_SENSORS_MPU6050B1 + if (MPL_PROD_KEY(mputestCfgPtr->product_id, + mputestCfgPtr->product_revision) == MPU_PRODUCT_KEY_B1_E1_5) { + accelSens[2] /= 2; + } else { + unsigned short trim_corr = 16384 / mputestCfgPtr->accel_sens_trim; + accelSens[0] /= trim_corr; + accelSens[1] /= trim_corr; + accelSens[2] /= trim_corr; + } +#endif + } else { + /* would be 0, but 1 to avoid divide-by-0 below */ + accelSens[0] = accelSens[1] = accelSens[2] = 1; + } +#ifdef CONFIG_MPU_SENSORS_MPU3050 + result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], + provide_result); +#else + result = inv_test_accel(mlsl_handle, accel_biases, accelSens[2], + FALSE); +#endif + if (result) + return result; + + result = inv_device_power_mgmt(mlsl_handle, FALSE); + if (result) + return result; + + ptr = 0; + dataStore[ptr++] = 0; /* total len of factory cal */ + dataStore[ptr++] = 0; + dataStore[ptr++] = 0; + dataStore[ptr++] = ML_INIT_CAL_LEN; + dataStore[ptr++] = 0; + dataStore[ptr++] = 5; /* record type 5 - initial calibration */ + + tmp = temp_avg; /* temperature */ + if (tmp < 0) tmp += 2 << 16; + USHORT_TO_CHARS(&dataStore[ptr], tmp); + ptr += 2; + + /* NOTE : 2 * test_setup.gyro_fs == 65536 / (32768 / test_setup.gyro_fs) */ + lltmp = (long)gyro_biases[0] * 2 * test_setup.gyro_fs; /* x gyro avg */ + if (lltmp < 0) lltmp += 1LL << 32; + UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); + ptr += 4; + lltmp = (long)gyro_biases[1] * 2 * test_setup.gyro_fs; /* y gyro avg */ + if (lltmp < 0) lltmp += 1LL << 32; + UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); + ptr += 4; + lltmp = (long)gyro_biases[2] * 2 * test_setup.gyro_fs; /* z gyro avg */ + if (lltmp < 0) lltmp += 1LL << 32; + UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); + ptr += 4; + + lltmp = (long)accel_biases[0] * 65536L / accelSens[0]; /* x accel avg */ + if (lltmp < 0) lltmp += 1LL << 32; + UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); + ptr += 4; + lltmp = (long)accel_biases[1] * 65536L / accelSens[1]; /* y accel avg */ + if (lltmp < 0) lltmp += 1LL << 32; + UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); + ptr += 4; + lltmp = (long)accel_biases[2] * 65536L / accelSens[2]; /* z accel avg */ + if (lltmp < 0) lltmp += 1LL << 32; + UINT_TO_CHARS(&dataStore[ptr], (uint32_t)lltmp); + ptr += 4; + + /* add a checksum for data */ + chk = inv_checksum( + dataStore + INV_CAL_HDR_LEN, + ML_INIT_CAL_LEN - INV_CAL_HDR_LEN - INV_CAL_CHK_LEN); + UINT_TO_CHARS(&dataStore[ptr], chk); + ptr += 4; + + if (ptr != ML_INIT_CAL_LEN) { + MPL_LOGI("Invalid calibration data length: exp %d, got %d\n", + ML_INIT_CAL_LEN, ptr); + return -1; + } + + return result; +} + +/** + * @brief The main test API. Runs the MPU Self Test and, if successful, + * stores the encoded initial calibration data on the final storage + * medium of choice (cfr. inv_serial_write_cal() and the MLCAL_FILE + * define in your mlsl implementation). + * + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @param provide_result + * If 1: + * perform and analyze the offset, drive frequency, and noise + * calculation and compare it against set threshouds. Report + * also the final result using a bit-mask like error code as + * described in the inv_test_gyro() function. + * When 0: + * skip the noise and drive frequency calculation and pass/fail + * assessment. It simply calculates the gyro and accel biases. + * + * @return 0 on success or a non-zero error code from the callees on error. + */ +inv_error_t inv_factory_calibrate(void *mlsl_handle, + uint_fast8_t provide_result) +{ + int result; + + result = inv_mpu_test(mlsl_handle, provide_result); + if (provide_result) { + MPL_LOGI("\n"); + if (result == 0) { + MPL_LOGI("Test : PASSED\n"); + } else { + MPL_LOGI("Test : FAILED %d/%04X - Biases NOT stored\n", result, result); + return result; /* abort writing the calibration if the + test is not successful */ + } + MPL_LOGI("\n"); + } else { + MPL_LOGI("\n"); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + } + + result = inv_serial_write_cal(dataStore, ML_INIT_CAL_LEN); + if (result) { + MPL_LOGI("Error : cannot write calibration on file - error %d\n", + result); + return result; + } + + return INV_SUCCESS; +} + + + +/* ----------------------------------------------------------------------- + accel interface functions + -----------------------------------------------------------------------*/ + +/** + * @internal + * @brief Reads data for X, Y, and Z axis from the accelerometer device. + * Used only if an accelerometer has been setup using the + * inv_test_setup_accel() API. + * Takes care of the accelerometer endianess according to how the + * device has been described in the corresponding accelerometer driver + * file. + * @param mlsl_handle + * serial interface handle to allow serial communication with the + * device, both gyro and accelerometer. + * @param slave + * a pointer to the descriptor of the slave accelerometer device + * in use. Contains the necessary information to operate, read, + * and communicate with the accelerometer device of choice. + * See the declaration of struct ext_slave_descr in mpu.h. + * @param pdata + * a pointer to the platform info of the slave accelerometer + * device in use. Describes how the device is oriented and + * mounted on host platform's PCB. + * @param vals + * output pointer to return the accelerometer's X, Y, and Z axis + * sensor data collected. + * @return 0 on success or a non-zero error code on error. + */ +static inv_error_t test_get_data( + void *mlsl_handle, + struct mldl_cfg *mputestCfgPtr, + short *vals) +{ + inv_error_t result; + unsigned char data[20]; + struct ext_slave_descr *slave = mputestCfgPtr->accel; +#ifndef CONFIG_MPU_SENSORS_MPU3050 + struct ext_slave_platform_data *pdata = &mputestCfgPtr->pdata->accel; +#endif + +#ifdef CONFIG_MPU_SENSORS_MPU3050 + result = inv_serial_read(mlsl_handle, mputestCfgPtr->addr, 0x23, + 6, data); +#else + result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg, + slave->read_len, data); +#endif + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } + + if (VERBOSE_OUT) { + MPL_LOGI("Accel : 0x%02X%02X 0x%02X%02X 0x%02X%02X (raw)\n", + ACCEL_UNPACK(data)); + } + + if (CHECK_NACKS(data)) { + MPL_LOGI("Error fetching data from the accelerometer : " + "all bytes read 0xff\n"); + return INV_ERROR_SERIAL_READ; + } + + if (slave->endian == EXT_SLAVE_BIG_ENDIAN) { + vals[0] = CHARS_TO_SHORT(&data[0]); + vals[1] = CHARS_TO_SHORT(&data[2]); + vals[2] = CHARS_TO_SHORT(&data[4]); + } else { + vals[0] = CHARS_TO_SHORT_SWAPPED(&data[0]); + vals[1] = CHARS_TO_SHORT_SWAPPED(&data[2]); + vals[2] = CHARS_TO_SHORT_SWAPPED(&data[4]); + } + + if (VERBOSE_OUT) { + MPL_LOGI("Accel : %+13d %+13d %+13d (LSB)\n", + vals[0], vals[1], vals[2]); + } + return INV_SUCCESS; +} + +#ifdef __cplusplus +} +#endif + +/** + * @} + */ + diff --git a/60xx/mlsdk/mlutils/mputest.h b/60xx/mlsdk/mlutils/mputest.h new file mode 100644 index 0000000..d3347c5 --- /dev/null +++ b/60xx/mlsdk/mlutils/mputest.h @@ -0,0 +1,54 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * + * $Id: mputest.h 4051 2010-11-19 04:51:58Z mcaramello $ + * + *****************************************************************************/ + +#ifndef MPUTEST_H +#define MPUTEST_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "mlsl.h" +#include "mldl_cfg.h" +#include "mputest_legacy.h" + +/* user facing APIs */ +inv_error_t inv_factory_calibrate(void *mlsl_handle, + uint_fast8_t provide_result); +void inv_set_test_parameters(unsigned int slave_addr, float sensitivity, + int p_thresh, float total_time_tol, + int bias_thresh, float rms_thresh, + float sp_shift_thresh, + unsigned short accel_samples); + +/* additional functions */ +int inv_mpu_test(void *mlsl_handle, uint_fast8_t provide_result); + + +#ifdef __cplusplus +} +#endif + +#endif /* MPUTEST_H */ + diff --git a/60xx/mlsdk/mlutils/slave.h b/60xx/mlsdk/mlutils/slave.h new file mode 100644 index 0000000..45449f6 --- /dev/null +++ b/60xx/mlsdk/mlutils/slave.h @@ -0,0 +1,188 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: slave.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#ifndef SLAVE_H +#define SLAVE_H + +/** + * @addtogroup SLAVEDL + * + * @{ + * @file slave.h + * @brief Top level descriptions for Accelerometer support + * + */ + +#include "mltypes.h" +#include "mpu.h" + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + +/*--- default accel support - selection ---*/ +#define ACCEL_ST_LIS331 0 +#define ACCEL_KIONIX_KXTF9 1 +#define ACCEL_BOSCH 0 +#define ACCEL_ADI 0 + +#define ACCEL_SLAVEADDR_INVALID 0x00 + +#define ACCEL_SLAVEADDR_LIS331 0x18 +#define ACCEL_SLAVEADDR_LSM303 0x18 +#define ACCEL_SLAVEADDR_LIS3DH 0x18 +#define ACCEL_SLAVEADDR_KXSD9 0x18 +#define ACCEL_SLAVEADDR_KXTF9 0x0F +#define ACCEL_SLAVEADDR_BMA150 0x38 +#define ACCEL_SLAVEADDR_BMA222 0x08 +#define ACCEL_SLAVEADDR_BMA250 0x18 +#define ACCEL_SLAVEADDR_ADXL34X 0x53 +#define ACCEL_SLAVEADDR_ADXL34X_ALT 0x1D /* alternative addr */ +#define ACCEL_SLAVEADDR_MMA8450 0x1C +#define ACCEL_SLAVEADDR_MMA845X 0x1C + +#define ACCEL_SLAVEADDR_INVENSENSE 0x68 +/* + Define default accelerometer to use if no selection is made +*/ +#if ACCEL_ST_LIS331 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LIS331 +#define DEFAULT_ACCEL_ID ACCEL_ID_LIS331 +#endif + +#if ACCEL_ST_LSM303 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_LSM303 +#define DEFAULT_ACCEL_ID ACCEL_ID_LSM303A +#endif + +#if ACCEL_KIONIX_KXSD9 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXSD9 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXSD9 +#endif + +#if ACCEL_KIONIX_KXTF9 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_KXTF9 +#define DEFAULT_ACCEL_ID ACCEL_ID_KXTF9 +#endif + +#if ACCEL_BOSCH +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA150 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA150 +#endif + +#if ACCEL_BMA222 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA222 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA222 +#endif + +#if ACCEL_BOSCH +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_BMA250 +#define DEFAULT_ACCEL_ID ACCEL_ID_BMA250 +#endif + +#if ACCEL_ADI +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_ADXL34X +#define DEFAULT_ACCEL_ID ACCEL_ID_ADXL34X +#endif + +#if ACCEL_MMA8450 +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA8450 +#define DEFAULT_ACCEL_ID ACCEL_ID_MMA8450 +#endif + +#if ACCEL_MMA845X +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_MMA845X +#define DEFAULT_ACCEL_ID ACCEL_ID_MMA845X +#endif + +/*--- if no default accelerometer was selected ---*/ +#ifndef DEFAULT_ACCEL_SLAVEADDR +#define DEFAULT_ACCEL_SLAVEADDR ACCEL_SLAVEADDR_INVALID +#endif + +#define USE_COMPASS_AICHI 0 +#define USE_COMPASS_AKM 0 +#define USE_COMPASS_YAS529 0 +#define USE_COMPASS_YAS530 0 +#define USE_COMPASS_HMC5883 0 +#define USE_COMPASS_MMC314X 0 +#define USE_COMPASS_HSCDTD002B 0 +#define USE_COMPASS_HSCDTD004A 0 + +#define COMPASS_SLAVEADDR_INVALID 0x00 +#define COMPASS_SLAVEADDR_AKM_BASE 0x0C +#define COMPASS_SLAVEADDR_AKM 0x0E +#define COMPASS_SLAVEADDR_AMI304 0x0E +#define COMPASS_SLAVEADDR_AMI305 0x0F /*Slave address for AMI 305/306*/ +#define COMPASS_SLAVEADDR_AMI306 0x0E /*Slave address for AMI 305/306*/ +#define COMPASS_SLAVEADDR_YAS529 0x2E +#define COMPASS_SLAVEADDR_YAS530 0x2E +#define COMPASS_SLAVEADDR_HMC5883 0x1E +#define COMPASS_SLAVEADDR_MMC314X 0x30 +#define COMPASS_SLAVEADDR_HSCDTD00XX 0x0C + +/* + Define default compass to use if no selection is made +*/ + #if USE_COMPASS_AKM + #define DEFAULT_COMPASS_TYPE COMPASS_ID_AK8975 + #endif + + #if USE_COMPASS_AICHI + #define DEFAULT_COMPASS_TYPE COMPASS_ID_AMI30X + #endif + + #if USE_COMPASS_YAS529 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS529 + #endif + + #if USE_COMPASS_YAS530 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_YAS530 + #endif + + #if USE_COMPASS_HMC5883 + #define DEFAULT_COMPASS_TYPE COMPASS_ID_HMC5883 + #endif + +#if USE_COMPASS_MMC314X +#define DEFAULT_COMPASS_TYPE COMPASS_ID_MMC314X +#endif + +#if USE_COMPASS_HSCDTD002B +#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD002B +#endif + +#if USE_COMPASS_HSCDTD004A +#define DEFAULT_COMPASS_TYPE COMPASS_ID_HSCDTD004A +#endif + +#ifndef DEFAULT_COMPASS_TYPE +#define DEFAULT_COMPASS_TYPE ID_INVALID +#endif + + +#endif // SLAVE_H + +/** + * @} + */ diff --git a/60xx/mlsdk/platform/include/i2c.h b/60xx/mlsdk/platform/include/i2c.h new file mode 100644 index 0000000..39dd255 --- /dev/null +++ b/60xx/mlsdk/platform/include/i2c.h @@ -0,0 +1,133 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: i2c.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#ifndef _I2C_H +#define _I2C_H + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ------------ */ +/* - Defines. - */ +/* ------------ */ + +/* - Error Codes. - */ + +#define I2C_SUCCESS 0 +#define I2C_ERROR 1 + +/* ---------- */ +/* - Enums. - */ +/* ---------- */ + +/* --------------- */ +/* - Structures. - */ +/* --------------- */ + +#define I2C_RBUFF_MAX 128 +#define I2C_RBUFF_SIZE 17 + +#ifdef BB_I2C +#define I2C_RBUFF_DYNAMIC 0 +#else +#define I2C_RBUFF_DYNAMIC 1 +#endif + +typedef struct { + + HANDLE i2cHndl; + HANDLE hDevice; // handle to the drive to be examined + + MLU8 readBuffer[1024]; + MLU8 writeBuffer[1024]; + + MLU16 rBuffRIndex; + MLU16 rBuffWIndex; +#if !I2C_RBUFF_DYNAMIC + MLU8 rBuff[I2C_RBUFF_MAX][I2C_RBUFF_SIZE]; +#else + MLU8 *rBuff; +#endif + MLU16 rBuffMax; + MLU16 rBuffNumBytes; + + MLU8 runThread; + MLU8 autoProcess; + +} I2C_Vars_t; + +/* --------------------- */ +/* - Function p-types. - */ +/* --------------------- */ + +#if (defined(BB_I2C)) +void set_i2c_open_bind_cb(int (*func)(unsigned int i2c_slave_addr)); +void set_i2c_open_cb(int (*func)(const char *dev, int rw)); +void set_i2c_close_cb(int (*func)(int fd)); +void set_i2c_lltransfer_cb(int (*func)(int fd, int client_addr, const char *write_buf, unsigned int write_len, + char *read_buf, unsigned int read_len )); +void set_i2c_write_register_cb(int (*func)(int fd, int client_addr, unsigned char reg, unsigned char value)); +void set_i2c_read_register_cb(unsigned char (*func)(int fd, int client_addr, unsigned char reg)); +void set_i2c_dump_register_cb(int (*func)(int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len)); + +int _i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value); +unsigned char _i2c_read_register (int fd, int client_addr, unsigned char reg); +int i2c_lltransfer (int fd, int client_addr, const char *write_buf, unsigned int write_len, + char *read_buf, unsigned int read_len ); +int i2c_write_register(int fd, int client_addr, unsigned char reg, unsigned char value); +unsigned char i2c_read_register (int fd, int client_addr, unsigned char reg); +int i2c_dump_register (int fd, int client_addr, unsigned char start_reg, unsigned char *buff, int len); +int i2c_open (const char *dev, int rw); +int i2c_close (int fd); +int i2c_open_bind (unsigned int i2c_slave_addr); +#endif + +int I2COpen (unsigned char autoProcess, unsigned char createThread); +int I2CClose (void); +int I2CDeviceIoControl(void); +int I2CRead (void); +int I2CWrite (void); +int I2CSetBufferSize (unsigned short bufferSize); +int I2CBufferUpdate (void); +int I2CHandler (void); +int I2CReadBuffer (unsigned short cnt, unsigned char bufferMode, unsigned char *rBuff); +int I2CEmptyBuffer (void); +int I2CPktsInBuffer (unsigned short *pktsInBuffer); +int I2CCreateMutex (void); +int I2CLockMutex (void); +int I2CUnlockMutex (void); + +int I2CWriteBurst (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data); +int I2CReadBurst (unsigned char slaveAddr, unsigned char registerAddr, unsigned short length, unsigned char *data); + +int I2COpenBB (void); +int I2CCloseBB (int i2cHandle); + +#ifdef __cplusplus +} +#endif + +#endif /* _TEMPLATE_H */ diff --git a/60xx/mlsdk/platform/include/linux/mpu.h b/60xx/mlsdk/platform/include/linux/mpu.h new file mode 100644 index 0000000..04fa7b6 --- /dev/null +++ b/60xx/mlsdk/platform/include/linux/mpu.h @@ -0,0 +1,335 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef __MPU_H_ +#define __MPU_H_ + +#ifdef __KERNEL__ +#include +#include +#elif defined LINUX +#include +#endif + +/* Number of axes on each sensor */ +#define GYRO_NUM_AXES (3) +#define ACCEL_NUM_AXES (3) +#define COMPASS_NUM_AXES (3) + +struct mpu_read_write { + /* Memory address or register address depending on ioctl */ + unsigned short address; + unsigned short length; + unsigned char *data; +}; + +enum mpuirq_data_type { + MPUIRQ_DATA_TYPE_MPU_IRQ, + MPUIRQ_DATA_TYPE_SLAVE_IRQ, + MPUIRQ_DATA_TYPE_PM_EVENT, + MPUIRQ_DATA_TYPE_NUM_TYPES, +}; + +/* User space PM event notification */ +#define MPU_PM_EVENT_SUSPEND_PREPARE (3) +#define MPU_PM_EVENT_POST_SUSPEND (4) + +struct mpuirq_data { + int interruptcount; + unsigned long long irqtime; + int data_type; + long data; +}; + +enum ext_slave_config_key { + MPU_SLAVE_CONFIG_ODR_SUSPEND, + MPU_SLAVE_CONFIG_ODR_RESUME, + MPU_SLAVE_CONFIG_FSR_SUSPEND, + MPU_SLAVE_CONFIG_FSR_RESUME, + MPU_SLAVE_CONFIG_MOT_THS, + MPU_SLAVE_CONFIG_NMOT_THS, + MPU_SLAVE_CONFIG_MOT_DUR, + MPU_SLAVE_CONFIG_NMOT_DUR, + MPU_SLAVE_CONFIG_IRQ_SUSPEND, + MPU_SLAVE_CONFIG_IRQ_RESUME, + MPU_SLAVE_WRITE_REGISTERS, + MPU_SLAVE_READ_REGISTERS, + /* AMI 306 specific config keys */ + MPU_SLAVE_PARAM, + MPU_SLAVE_WINDOW, + MPU_SLAVE_READWINPARAMS, + MPU_SLAVE_SEARCHOFFSET, + /* AKM specific config keys */ + MPU_SLAVE_READ_SCALE, + /* YAS specific config keys */ + MPU_SLAVE_OFFSET_VALS, + MPU_SLAVE_RANGE_CHECK, + + MPU_SLAVE_CONFIG_NUM_CONFIG_KEYS, +}; + +/* For the MPU_SLAVE_CONFIG_IRQ_SUSPEND and MPU_SLAVE_CONFIG_IRQ_RESUME */ +enum ext_slave_config_irq_type { + MPU_SLAVE_IRQ_TYPE_NONE, + MPU_SLAVE_IRQ_TYPE_MOTION, + MPU_SLAVE_IRQ_TYPE_DATA_READY, +}; + +/* Structure for the following IOCTS's + * MPU_CONFIG_ACCEL + * MPU_CONFIG_COMPASS + * MPU_CONFIG_PRESSURE + * MPU_GET_CONFIG_ACCEL + * MPU_GET_CONFIG_COMPASS + * MPU_GET_CONFIG_PRESSURE + * + * @key one of enum ext_slave_config_key + * @len length of data pointed to by data + * @apply zero if communication with the chip is not necessary, false otherwise + * This flag can be used to select cached data or to refresh cashed data + * cache data to be pushed later or push immediately. If true and the + * slave is on the secondary bus the MPU will first enger bypass mode + * before calling the slaves .config or .get_config funcion + * @data pointer to the data to confgure or get + */ +struct ext_slave_config { + int key; + int len; + int apply; + void *data; +}; + +enum ext_slave_type { + EXT_SLAVE_TYPE_GYROSCOPE, + EXT_SLAVE_TYPE_ACCELEROMETER, + EXT_SLAVE_TYPE_COMPASS, + EXT_SLAVE_TYPE_PRESSURE, + /*EXT_SLAVE_TYPE_TEMPERATURE */ + + EXT_SLAVE_NUM_TYPES +}; + +enum ext_slave_id { + ID_INVALID = 0, + + ACCEL_ID_LIS331, + ACCEL_ID_LSM303A, + ACCEL_ID_LIS3DH, + ACCEL_ID_KXSD9, + ACCEL_ID_KXTF9, + ACCEL_ID_BMA150, + ACCEL_ID_BMA222, + ACCEL_ID_BMA250, + ACCEL_ID_ADXL34X, + ACCEL_ID_MMA8450, + ACCEL_ID_MMA845X, + ACCEL_ID_MPU6050, + + COMPASS_ID_AK8975, + COMPASS_ID_AMI30X, + COMPASS_ID_AMI306, + COMPASS_ID_YAS529, + COMPASS_ID_YAS530, + COMPASS_ID_HMC5883, + COMPASS_ID_LSM303M, + COMPASS_ID_MMC314X, + COMPASS_ID_HSCDTD002B, + COMPASS_ID_HSCDTD004A, + + PRESSURE_ID_BMA085, +}; + +enum ext_slave_endian { + EXT_SLAVE_BIG_ENDIAN, + EXT_SLAVE_LITTLE_ENDIAN, + EXT_SLAVE_FS8_BIG_ENDIAN, + EXT_SLAVE_FS16_BIG_ENDIAN, +}; + +enum ext_slave_bus { + EXT_SLAVE_BUS_INVALID = -1, + EXT_SLAVE_BUS_PRIMARY = 0, + EXT_SLAVE_BUS_SECONDARY = 1 +}; + + +/** + * struct ext_slave_platform_data - Platform data for mpu3050 and mpu6050 + * slave devices + * + * @get_slave_descr: Function pointer to retrieve the struct ext_slave_descr + * for this slave + * @irq: the irq number attached to the slave if any. + * @adapt_num: the I2C adapter number. + * @bus: the bus the slave is attached to: enum ext_slave_bus + * @address: the I2C slave address of the slave device. + * @orientation: the mounting matrix of the device relative to MPU. + * @irq_data: private data for the slave irq handler + * @private_data: additional data, user customizable. Not touched by the MPU + * driver. + * + * The orientation matricies are 3x3 rotation matricies + * that are applied to the data to rotate from the mounting orientation to the + * platform orientation. The values must be one of 0, 1, or -1 and each row and + * column should have exactly 1 non-zero value. + */ +struct ext_slave_platform_data { + struct ext_slave_descr *(*get_slave_descr) (void); + int irq; + int adapt_num; + int bus; + unsigned char address; + signed char orientation[9]; + void *irq_data; + void *private_data; +}; + +struct fix_pnt_range { + long mantissa; + long fraction; +}; + +static inline long range_fixedpoint_to_long_mg(struct fix_pnt_range rng) +{ + return (long)(rng.mantissa * 1000 + rng.fraction / 10); +} + +struct ext_slave_read_trigger { + unsigned char reg; + unsigned char value; +}; + +/** + * struct ext_slave_descr - Description of the slave device for programming. + * + * @suspend: function pointer to put the device in suspended state + * @resume: function pointer to put the device in running state + * @read: function that reads the device data + * @init: function used to preallocate memory used by the driver + * @exit: function used to free memory allocated for the driver + * @config: function used to configure the device + * @get_config:function used to get the device's configuration + * + * @name: text name of the device + * @type: device type. enum ext_slave_type + * @id: enum ext_slave_id + * @reg: starting register address to retrieve data. + * @len: length in bytes of the sensor data. Should be 6. + * @endian: byte order of the data. enum ext_slave_endian + * @range: full scale range of the slave ouput: struct fix_pnt_range + * @trigger: If reading data first requires writing a register this is the + * data to write. + * + * Defines the functions and information about the slave the mpu3050 and + * mpu6050 needs to use the slave device. + */ +struct ext_slave_descr { + int (*init) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*exit) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*suspend) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*resume) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata); + int (*read) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + unsigned char *data); + int (*config) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *config); + int (*get_config) (void *mlsl_handle, + struct ext_slave_descr *slave, + struct ext_slave_platform_data *pdata, + struct ext_slave_config *config); + + char *name; + unsigned char type; + unsigned char id; + unsigned char read_reg; + unsigned int read_len; + unsigned char endian; + struct fix_pnt_range range; + struct ext_slave_read_trigger *trigger; +}; + +/** + * struct mpu_platform_data - Platform data for the mpu driver + * @int_config: Bits [7:3] of the int config register. + * @orientation: Orientation matrix of the gyroscope + * @level_shifter: 0: VLogic, 1: VDD + * @accel: Accel platform data + * @compass: Compass platform data + * @pressure: Pressure platform data + * + * Contains platform specific information on how to configure the MPU3050 to + * work on this platform. The orientation matricies are 3x3 rotation matricies + * that are applied to the data to rotate from the mounting orientation to the + * platform orientation. The values must be one of 0, 1, or -1 and each row and + * column should have exactly 1 non-zero value. + */ +struct mpu_platform_data { + unsigned char int_config; + signed char orientation[GYRO_NUM_AXES * GYRO_NUM_AXES]; + unsigned char level_shifter; + struct ext_slave_platform_data accel; + struct ext_slave_platform_data compass; + struct ext_slave_platform_data pressure; +}; + +#if defined __KERNEL__ || defined LINUX +#define MPU_IOCTL (0x81) /* Magic number for MPU Iocts */ +/* IOCTL commands for /dev/mpu */ +#define MPU_SET_MPU_CONFIG _IOWR(MPU_IOCTL, 0x00, struct mldl_cfg) +#define MPU_GET_MPU_CONFIG _IOW(MPU_IOCTL, 0x00, struct mldl_cfg) + +#define MPU_SET_PLATFORM_DATA _IOWR(MPU_IOCTL, 0x01, struct mldl_cfg) + +#define MPU_READ _IOWR(MPU_IOCTL, 0x10, struct mpu_read_write) +#define MPU_WRITE _IOW(MPU_IOCTL, 0x10, struct mpu_read_write) +#define MPU_READ_MEM _IOWR(MPU_IOCTL, 0x11, struct mpu_read_write) +#define MPU_WRITE_MEM _IOW(MPU_IOCTL, 0x11, struct mpu_read_write) +#define MPU_READ_FIFO _IOWR(MPU_IOCTL, 0x12, struct mpu_read_write) +#define MPU_WRITE_FIFO _IOW(MPU_IOCTL, 0x12, struct mpu_read_write) + +#define MPU_READ_COMPASS _IOR(MPU_IOCTL, 0x12, unsigned char) +#define MPU_READ_ACCEL _IOR(MPU_IOCTL, 0x13, unsigned char) +#define MPU_READ_PRESSURE _IOR(MPU_IOCTL, 0x14, unsigned char) + +#define MPU_CONFIG_ACCEL _IOW(MPU_IOCTL, 0x20, struct ext_slave_config) +#define MPU_CONFIG_COMPASS _IOW(MPU_IOCTL, 0x21, struct ext_slave_config) +#define MPU_CONFIG_PRESSURE _IOW(MPU_IOCTL, 0x22, struct ext_slave_config) + +#define MPU_GET_CONFIG_ACCEL _IOWR(MPU_IOCTL, 0x20, struct ext_slave_config) +#define MPU_GET_CONFIG_COMPASS _IOWR(MPU_IOCTL, 0x21, struct ext_slave_config) +#define MPU_GET_CONFIG_PRESSURE _IOWR(MPU_IOCTL, 0x22, struct ext_slave_config) + +#define MPU_SUSPEND _IO(MPU_IOCTL, 0x30) +#define MPU_RESUME _IO(MPU_IOCTL, 0x31) +/* Userspace PM Event response */ +#define MPU_PM_EVENT_HANDLED _IO(MPU_IOCTL, 0x32) + +#endif + +#endif /* __MPU_H_ */ diff --git a/60xx/mlsdk/platform/include/log.h b/60xx/mlsdk/platform/include/log.h new file mode 100644 index 0000000..8485074 --- /dev/null +++ b/60xx/mlsdk/platform/include/log.h @@ -0,0 +1,344 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/* + * This file incorporates work covered by the following copyright and + * permission notice: + * + * Copyright (C) 2005 The Android Open Source Project + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * C/C++ logging functions. See the logging documentation for API details. + * + * We'd like these to be available from C code (in case we import some from + * somewhere), so this has a C interface. + * + * The output will be correct when the log file is shared between multiple + * threads and/or multiple processes so long as the operating system + * supports O_APPEND. These calls have mutex-protected data structures + * and so are NOT reentrant. Do not use MPL_LOG in a signal handler. + */ +#ifndef _LIBS_CUTILS_MPL_LOG_H +#define _LIBS_CUTILS_MPL_LOG_H + +#include "mltypes.h" +#include + +#ifdef ANDROID +#ifdef NDK_BUILD +#include "log_macros.h" +#else +#include /* For the LOG macro */ +#endif +#endif + +#ifdef __KERNEL__ +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Normally we strip MPL_LOGV (VERBOSE messages) from release builds. + * You can modify this (for example with "#define MPL_LOG_NDEBUG 0" + * at the top of your source file) to change that behavior. + */ +#ifndef MPL_LOG_NDEBUG +#ifdef NDEBUG +#define MPL_LOG_NDEBUG 1 +#else +#define MPL_LOG_NDEBUG 0 +#endif +#endif + +#ifdef __KERNEL__ +#define MPL_LOG_UNKNOWN MPL_LOG_VERBOSE +#define MPL_LOG_DEFAULT KERN_DEFAULT +#define MPL_LOG_VERBOSE KERN_CONT +#define MPL_LOG_DEBUG KERN_NOTICE +#define MPL_LOG_INFO KERN_INFO +#define MPL_LOG_WARN KERN_WARNING +#define MPL_LOG_ERROR KERN_ERR +#define MPL_LOG_SILENT MPL_LOG_VERBOSE + +#else + /* Based off the log priorities in android + /system/core/include/android/log.h */ +#define MPL_LOG_UNKNOWN (0) +#define MPL_LOG_DEFAULT (1) +#define MPL_LOG_VERBOSE (2) +#define MPL_LOG_DEBUG (3) +#define MPL_LOG_INFO (4) +#define MPL_LOG_WARN (5) +#define MPL_LOG_ERROR (6) +#define MPL_LOG_SILENT (8) +#endif + + +/* + * This is the local tag used for the following simplified + * logging macros. You can change this preprocessor definition + * before using the other macros to change the tag. + */ +#ifndef MPL_LOG_TAG +#ifdef __KERNEL__ +#define MPL_LOG_TAG +#else +#define MPL_LOG_TAG NULL +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Simplified macro to send a verbose log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGV +#if MPL_LOG_NDEBUG +#define MPL_LOGV(fmt, ...) \ + do { \ + if (0) \ + MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__);\ + } while (0) +#else +#define MPL_LOGV(fmt, ...) MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef CONDITION +#define CONDITION(cond) ((cond) != 0) +#endif + +#ifndef MPL_LOGV_IF +#if MPL_LOG_NDEBUG +#define MPL_LOGV_IF(cond, fmt, ...) \ + do { if (0) MPL_LOG(fmt, ##__VA_ARGS__); } while (0) +#else +#define MPL_LOGV_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_VERBOSE, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif +#endif + +/* + * Simplified macro to send a debug log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGD +#define MPL_LOGD(fmt, ...) MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif + +#ifndef MPL_LOGD_IF +#define MPL_LOGD_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_DEBUG, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send an info log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGI +#define MPL_LOGI(fmt, ...) MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif + +#ifndef MPL_LOGI_IF +#define MPL_LOGI_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_INFO, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send a warning log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGW +#ifdef __KERNEL__ +#define MPL_LOGW(fmt, ...) printk(KERN_WARNING MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGW(fmt, ...) MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGW_IF +#define MPL_LOGW_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_WARN, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* + * Simplified macro to send an error log message using the current MPL_LOG_TAG. + */ +#ifndef MPL_LOGE +#ifdef __KERNEL__ +#define MPL_LOGE(fmt, ...) printk(KERN_ERR MPL_LOG_TAG fmt, ##__VA_ARGS__) +#else +#define MPL_LOGE(fmt, ...) MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) +#endif +#endif + +#ifndef MPL_LOGE_IF +#define MPL_LOGE_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? MPL_LOG(LOG_ERROR, MPL_LOG_TAG, fmt, ##__VA_ARGS__) \ + : (void)0) +#endif + +/* --------------------------------------------------------------------- */ + +/* + * Log a fatal error. If the given condition fails, this stops program + * execution like a normal assertion, but also generating the given message. + * It is NOT stripped from release builds. Note that the condition test + * is -inverted- from the normal assert() semantics. + */ +#define MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ...) \ + ((CONDITION(cond)) \ + ? ((void)android_printAssert(#cond, MPL_LOG_TAG, \ + fmt, ##__VA_ARGS__)) \ + : (void)0) + +#define MPL_LOG_ALWAYS_FATAL(fmt, ...) \ + (((void)android_printAssert(NULL, MPL_LOG_TAG, fmt, ##__VA_ARGS__))) + +/* + * Versions of MPL_LOG_ALWAYS_FATAL_IF and MPL_LOG_ALWAYS_FATAL that + * are stripped out of release builds. + */ +#if MPL_LOG_NDEBUG +#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ + do { \ + if (0) \ + MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__); \ + } while (0) +#define MPL_LOG_FATAL(fmt, ...) \ + do { \ + if (0) \ + MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) \ + } while (0) +#else +#define MPL_LOG_FATAL_IF(cond, fmt, ...) \ + MPL_LOG_ALWAYS_FATAL_IF(cond, fmt, ##__VA_ARGS__) +#define MPL_LOG_FATAL(fmt, ...) \ + MPL_LOG_ALWAYS_FATAL(fmt, ##__VA_ARGS__) +#endif + +/* + * Assertion that generates a log message when the assertion fails. + * Stripped out of release builds. Uses the current MPL_LOG_TAG. + */ +#define MPL_LOG_ASSERT(cond, fmt, ...) \ + MPL_LOG_FATAL_IF(!(cond), fmt, ##__VA_ARGS__) + +/* --------------------------------------------------------------------- */ + +/* + * Basic log message macro. + * + * Example: + * MPL_LOG(MPL_LOG_WARN, NULL, "Failed with error %d", errno); + * + * The second argument may be NULL or "" to indicate the "global" tag. + */ +#ifndef MPL_LOG +#define MPL_LOG(priority, tag, fmt, ...) \ + MPL_LOG_PRI(priority, tag, fmt, ##__VA_ARGS__) +#endif + +/* + * Log macro that allows you to specify a number for the priority. + */ +#ifndef MPL_LOG_PRI +#ifdef ANDROID +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + ALOG(priority, tag, fmt, ##__VA_ARGS__) +#elif defined __KERNEL__ +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + pr_debug(MPL_##priority tag fmt, ##__VA_ARGS__) +#else +#define MPL_LOG_PRI(priority, tag, fmt, ...) \ + _MLPrintLog(MPL_##priority, tag, fmt, ##__VA_ARGS__) +#endif +#endif + +/* + * Log macro that allows you to pass in a varargs ("args" is a va_list). + */ +#ifndef MPL_LOG_PRI_VA +#ifdef ANDROID +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + android_vprintLog(priority, NULL, tag, fmt, args) +#elif defined __KERNEL__ +/* not allowed in the Kernel because there is no dev_dbg that takes a va_list */ +#else +#define MPL_LOG_PRI_VA(priority, tag, fmt, args) \ + _MLPrintVaLog(priority, NULL, tag, fmt, args) +#endif +#endif + +/* --------------------------------------------------------------------- */ + +/* + * =========================================================================== + * + * The stuff in the rest of this file should not be used directly. + */ + +#ifndef ANDROID +int _MLPrintLog(int priority, const char *tag, const char *fmt, ...); +int _MLPrintVaLog(int priority, const char *tag, const char *fmt, va_list args); +/* Final implementation of actual writing to a character device */ +int _MLWriteLog(const char *buf, int buflen); +#endif + +static inline void __print_result_location(int result, + const char *file, + const char *func, int line) +{ + MPL_LOGE("%s|%s|%d returning %d\n", file, func, line, result); +} + +#define LOG_RESULT_LOCATION(condition) \ + do { \ + __print_result_location((int)(condition), __FILE__, \ + __func__, __LINE__); \ + } while (0) + + +#ifdef __cplusplus +} +#endif +#endif /* _LIBS_CUTILS_MPL_LOG_H */ diff --git a/60xx/mlsdk/platform/include/mlmath.h b/60xx/mlsdk/platform/include/mlmath.h new file mode 100644 index 0000000..bf54870 --- /dev/null +++ b/60xx/mlsdk/platform/include/mlmath.h @@ -0,0 +1,107 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mlmath.h 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +#ifndef _ML_MATH_H_ +#define _ML_MATH_H_ + +#ifndef MLMATH +// This define makes Microsoft pickup things like M_PI +#define _USE_MATH_DEFINES +#include + +#ifdef WIN32 +// Microsoft doesn't follow standards +#define round(x)(((double)((long long)((x)>0?(x)+.5:(x)-.5)))) +#define roundf(x)(((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) +#endif + +#else // MLMATH + +#ifdef __cplusplus +extern "C" { +#endif +/* MPL needs below functions */ +double ml_asin(double); +double ml_atan(double); +double ml_atan2(double, double); +double ml_log(double); +double ml_sqrt(double); +double ml_ceil(double); +double ml_floor(double); +double ml_cos(double); +double ml_sin(double); +double ml_acos(double); +#ifdef __cplusplus +} // extern "C" +#endif + +/* + * We rename functions here to provide the hook for other + * customized math functions. + */ +#define sqrt(x) ml_sqrt(x) +#define log(x) ml_log(x) +#define asin(x) ml_asin(x) +#define atan(x) ml_atan(x) +#define atan2(x,y) ml_atan2(x,y) +#define ceil(x) ml_ceil(x) +#define floor(x) ml_floor(x) +#define fabs(x) (((x)<0)?-(x):(x)) +#define round(x) (((double)((long long)((x)>0?(x)+.5:(x)-.5)))) +#define roundf(x) (((float )((long long)((x)>0?(x)+.5f:(x)-.5f)))) +#define cos(x) ml_cos(x) +#define sin(x) ml_sin(x) +#define acos(x) ml_acos(x) + +#define pow(x,y) ml_pow(x,y) + +#ifdef LINUX +/* stubs for float version of math functions */ +#define cosf(x) ml_cos(x) +#define sinf(x) ml_sin(x) +#define atan2f(x,y) ml_atan2(x,y) +#define sqrtf(x) ml_sqrt(x) +#endif + + + +#endif // MLMATH + +#ifndef M_PI +#define M_PI 3.14159265358979 +#endif + +#ifndef ABS +#define ABS(x) (((x)>=0)?(x):-(x)) +#endif + +#ifndef MIN +#define MIN(x,y) (((x)<(y))?(x):(y)) +#endif + +#ifndef MAX +#define MAX(x,y) (((x)>(y))?(x):(y)) +#endif + +/*---------------------------*/ +#endif /* !_ML_MATH_H_ */ diff --git a/60xx/mlsdk/platform/include/mlos.h b/60xx/mlsdk/platform/include/mlos.h new file mode 100644 index 0000000..0aeda96 --- /dev/null +++ b/60xx/mlsdk/platform/include/mlos.h @@ -0,0 +1,109 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef _MLOS_H +#define _MLOS_H + +#ifndef __KERNEL__ +#include +#endif + +#include "mltypes.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#if defined(LINUX) || defined(__KERNEL__) +#include +typedef uintptr_t HANDLE; +#endif + + /* ------------ */ + /* - Defines. - */ + /* ------------ */ + + /* - MLOSCreateFile defines. - */ + +#define MLOS_GENERIC_READ ((unsigned int)0x80000000) +#define MLOS_GENERIC_WRITE ((unsigned int)0x40000000) +#define MLOS_FILE_SHARE_READ ((unsigned int)0x00000001) +#define MLOS_FILE_SHARE_WRITE ((unsigned int)0x00000002) +#define MLOS_OPEN_EXISTING ((unsigned int)0x00000003) + + /* ---------- */ + /* - Enums. - */ + /* ---------- */ + + /* --------------- */ + /* - Structures. - */ + /* --------------- */ + + /* --------------------- */ + /* - Function p-types. - */ + /* --------------------- */ + +#ifndef __KERNEL__ +#include + void *inv_malloc(unsigned int numBytes); + inv_error_t inv_free(void *ptr); + inv_error_t inv_create_mutex(HANDLE *mutex); + inv_error_t inv_lock_mutex(HANDLE mutex); + inv_error_t inv_unlock_mutex(HANDLE mutex); + FILE *inv_fopen(char *filename); + void inv_fclose(FILE *fp); + + inv_error_t inv_destroy_mutex(HANDLE handle); + + void inv_sleep(int mSecs); + unsigned long inv_get_tick_count(void); + + /* Kernel implmentations */ +#define GFP_KERNEL (0x70) + static inline void *kmalloc(size_t size, + unsigned int gfp_flags) + { + return inv_malloc((unsigned int)size); + } + static inline void *kzalloc(size_t size, unsigned int gfp_flags) + { + void *tmp = inv_malloc((unsigned int)size); + if (tmp) + memset(tmp, 0, size); + return tmp; + } + static inline void kfree(void *ptr) + { + inv_free(ptr); + } + static inline void msleep(long msecs) + { + inv_sleep(msecs); + } + static inline void udelay(unsigned long usecs) + { + inv_sleep((usecs + 999) / 1000); + } +#else +#include +#endif + +#ifdef __cplusplus +} +#endif +#endif /* _MLOS_H */ diff --git a/60xx/mlsdk/platform/include/mlsl.h b/60xx/mlsdk/platform/include/mlsl.h new file mode 100644 index 0000000..535d117 --- /dev/null +++ b/60xx/mlsdk/platform/include/mlsl.h @@ -0,0 +1,290 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef __MLSL_H__ +#define __MLSL_H__ + +/** + * @defgroup MLSL + * @brief Motion Library - Serial Layer. + * The Motion Library System Layer provides the Motion Library + * with the communication interface to the hardware. + * + * The communication interface is assumed to support serial + * transfers in burst of variable length up to + * SERIAL_MAX_TRANSFER_SIZE. + * The default value for SERIAL_MAX_TRANSFER_SIZE is 128 bytes. + * Transfers of length greater than SERIAL_MAX_TRANSFER_SIZE, will + * be subdivided in smaller transfers of length <= + * SERIAL_MAX_TRANSFER_SIZE. + * The SERIAL_MAX_TRANSFER_SIZE definition can be modified to + * overcome any host processor transfer size limitation down to + * 1 B, the minimum. + * An higher value for SERIAL_MAX_TRANSFER_SIZE will favor + * performance and efficiency while requiring higher resource usage + * (mostly buffering). A smaller value will increase overhead and + * decrease efficiency but allows to operate with more resource + * constrained processor and master serial controllers. + * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the + * mlsl.h header file and master serial controllers. + * The SERIAL_MAX_TRANSFER_SIZE definition can be found in the + * mlsl.h header file. + * + * @{ + * @file mlsl.h + * @brief The Motion Library System Layer. + * + */ + +#include "mltypes.h" +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * NOTE : to properly support Yamaha compass reads, + * the max transfer size should be at least 9 B. + * Length in bytes, typically a power of 2 >= 2 + */ +#define SERIAL_MAX_TRANSFER_SIZE 128 + +#ifndef __KERNEL__ +/** + * inv_serial_open() - used to open the serial port. + * @port The COM port specification associated with the device in use. + * @sl_handle a pointer to the file handle to the serial device to be open + * for the communication. + * This port is used to send and receive data to the device. + * + * This function is called by inv_serial_start(). + * Unlike previous MPL Software releases, explicitly calling + * inv_serial_start() is mandatory to instantiate the communication + * with the device. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_open(char const *port, void **sl_handle); + +/** + * inv_serial_close() - used to close the serial port. + * @sl_handle a file handle to the serial device used for the communication. + * + * This port is used to send and receive data to the device. + * + * This function is called by inv_serial_stop(). + * Unlike previous MPL Software releases, explicitly calling + * inv_serial_stop() is mandatory to properly shut-down the + * communication with the device. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_close(void *sl_handle); + +/** + * inv_serial_reset() - used to reset any buffering the driver may be doing + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_reset(void *sl_handle); +#endif + +/** + * inv_serial_single_write() - used to write a single byte of data. + * @sl_handle pointer to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to write. + * @data Single byte of data to write. + * + * It is called by the MPL to write a single byte of data to the MPU. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_single_write( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned char data); + +/** + * inv_serial_write() - used to write multiple bytes of data to registers. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to write. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_write( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char const *data); + +/** + * inv_serial_read() - used to read multiple bytes of data from registers. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @register_addr Register address to read. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +inv_error_t inv_serial_read( + void *sl_handle, + unsigned char slave_addr, + unsigned char register_addr, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_read_mem() - used to read multiple bytes of data from the memory. + * This should be sent by I2C or SPI. + * + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @mem_addr The location in the memory to read from. + * @length Length of burst data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +inv_error_t inv_serial_read_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_write_mem() - used to write multiple bytes of data to the memory. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @mem_addr The location in the memory to write to. + * @length Length of burst data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +inv_error_t inv_serial_write_mem( + void *sl_handle, + unsigned char slave_addr, + unsigned short mem_addr, + unsigned short length, + unsigned char const *data); + +/** + * inv_serial_read_fifo() - used to read multiple bytes of data from the fifo. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +inv_error_t inv_serial_read_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char *data); + +/** + * inv_serial_write_fifo() - used to write multiple bytes of data to the fifo. + * @sl_handle a file handle to the serial device used for the communication. + * @slave_addr I2C slave address of device. + * @length Length of burst of data. + * @data Pointer to block of data. + * + * returns INV_SUCCESS == 0 if successful; a non-zero error code otherwise. + */ +inv_error_t inv_serial_write_fifo( + void *sl_handle, + unsigned char slave_addr, + unsigned short length, + unsigned char const *data); + +#ifndef __KERNEL__ +/** + * inv_serial_read_cfg() - used to get the configuration data. + * @cfg Pointer to the configuration data. + * @len Length of the configuration data. + * + * Is called by the MPL to get the configuration data + * used by the motion library. + * This data would typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len); + +/** + * inv_serial_write_cfg() - used to save the configuration data. + * @cfg Pointer to the configuration data. + * @len Length of the configuration data. + * + * Is called by the MPL to save the configuration data used by the + * motion library. + * This data would typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len); + +/** + * inv_serial_read_cal() - used to get the calibration data. + * @cfg Pointer to the calibration data. + * @len Length of the calibration data. + * + * It is called by the MPL to get the calibration data used by the + * motion library. + * This data is typically be saved in non-volatile memory. + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len); + +/** + * inv_serial_write_cal() - used to save the calibration data. + * + * @cfg Pointer to the calibration data. + * @len Length of the calibration data. + * + * It is called by the MPL to save the calibration data used by the + * motion library. + * This data is typically be saved in non-volatile memory. + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len); + +/** + * inv_serial_get_cal_length() - Get the calibration length from the storage. + * @len lenght to be returned + * + * returns INV_SUCCESS if successful, a non-zero error code otherwise. + */ +inv_error_t inv_serial_get_cal_length(unsigned int *len); +#endif +#ifdef __cplusplus +} +#endif +/** + * @} + */ +#endif /* __MLSL_H__ */ diff --git a/60xx/mlsdk/platform/include/mltypes.h b/60xx/mlsdk/platform/include/mltypes.h new file mode 100644 index 0000000..90a126b --- /dev/null +++ b/60xx/mlsdk/platform/include/mltypes.h @@ -0,0 +1,265 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/** + * @defgroup MLERROR + * @brief Motion Library - Error definitions. + * Definition of the error codes used within the MPL and + * returned to the user. + * Every function tries to return a meaningful error code basing + * on the occuring error condition. The error code is numeric. + * + * The available error codes and their associated values are: + * - (0) INV_SUCCESS + * - (1) INV_ERROR + * - (2) INV_ERROR_INVALID_PARAMETER + * - (3) INV_ERROR_FEATURE_NOT_ENABLED + * - (4) INV_ERROR_FEATURE_NOT_IMPLEMENTED + * - (6) INV_ERROR_DMP_NOT_STARTED + * - (7) INV_ERROR_DMP_STARTED + * - (8) INV_ERROR_NOT_OPENED + * - (9) INV_ERROR_OPENED + * - (10) INV_ERROR_INVALID_MODULE + * - (11) INV_ERROR_MEMORY_EXAUSTED + * - (12) INV_ERROR_DIVIDE_BY_ZERO + * - (13) INV_ERROR_ASSERTION_FAILURE + * - (14) INV_ERROR_FILE_OPEN + * - (15) INV_ERROR_FILE_READ + * - (16) INV_ERROR_FILE_WRITE + * - (17) INV_ERROR_INVALID_CONFIGURATION + * - (20) INV_ERROR_SERIAL_CLOSED + * - (21) INV_ERROR_SERIAL_OPEN_ERROR + * - (22) INV_ERROR_SERIAL_READ + * - (23) INV_ERROR_SERIAL_WRITE + * - (24) INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED + * - (25) INV_ERROR_SM_TRANSITION + * - (26) INV_ERROR_SM_IMPROPER_STATE + * - (30) INV_ERROR_FIFO_OVERFLOW + * - (31) INV_ERROR_FIFO_FOOTER + * - (32) INV_ERROR_FIFO_READ_COUNT + * - (33) INV_ERROR_FIFO_READ_DATA + * - (40) INV_ERROR_MEMORY_SET + * - (50) INV_ERROR_LOG_MEMORY_ERROR + * - (51) INV_ERROR_LOG_OUTPUT_ERROR + * - (60) INV_ERROR_OS_BAD_PTR + * - (61) INV_ERROR_OS_BAD_HANDLE + * - (62) INV_ERROR_OS_CREATE_FAILED + * - (63) INV_ERROR_OS_LOCK_FAILED + * - (70) INV_ERROR_COMPASS_DATA_OVERFLOW + * - (71) INV_ERROR_COMPASS_DATA_UNDERFLOW + * - (72) INV_ERROR_COMPASS_DATA_NOT_READY + * - (73) INV_ERROR_COMPASS_DATA_ERROR + * - (75) INV_ERROR_CALIBRATION_LOAD + * - (76) INV_ERROR_CALIBRATION_STORE + * - (77) INV_ERROR_CALIBRATION_LEN + * - (78) INV_ERROR_CALIBRATION_CHECKSUM + * - (79) INV_ERROR_ACCEL_DATA_OVERFLOW + * - (80) INV_ERROR_ACCEL_DATA_UNDERFLOW + * - (81) INV_ERROR_ACCEL_DATA_NOT_READY + * - (82) INV_ERROR_ACCEL_DATA_ERROR + * + * @{ + * @file mltypes.h + * @} + */ + +#ifndef MLTYPES_H +#define MLTYPES_H + +#ifdef __KERNEL__ +#include +#else +#include "stdint_invensense.h" +#endif + +/*--------------------------- + ML Types +---------------------------*/ + +/** + * @struct inv_error_t mltypes.h "mltypes" + * @brief The MPL Error Code return type. + * + * @code + * typedef unsigned char inv_error_t; + * @endcode + */ +typedef unsigned char inv_error_t; + +#ifndef __cplusplus +#ifndef __KERNEL__ +typedef int_fast8_t bool; +#endif +#endif + +/*--------------------------- + ML Defines +---------------------------*/ + +#ifndef NULL +#define NULL 0 +#endif + +#ifndef TRUE +#define TRUE 1 +#endif + +#ifndef FALSE +#define FALSE 0 +#endif + +#ifndef __KERNEL__ +#ifndef ARRAY_SIZE +/* Dimension of an array */ +#define ARRAY_SIZE(array) (sizeof(array)/sizeof((array)[0])) +#endif +#endif +/* - ML Errors. - */ +#define ERROR_NAME(x) (#x) +#define ERROR_CHECK_FIRST(first, x) \ + { if (INV_SUCCESS == first) first = x; } + +#define INV_SUCCESS (0) +/* Generic Error code. Proprietary Error Codes only */ +#define INV_ERROR (1) + +/* Compatibility and other generic error codes */ +#define INV_ERROR_INVALID_PARAMETER (2) +#define INV_ERROR_FEATURE_NOT_ENABLED (3) +#define INV_ERROR_FEATURE_NOT_IMPLEMENTED (4) +#define INV_ERROR_DMP_NOT_STARTED (6) +#define INV_ERROR_DMP_STARTED (7) +#define INV_ERROR_NOT_OPENED (8) +#define INV_ERROR_OPENED (9) +#define INV_ERROR_INVALID_MODULE (10) +#define INV_ERROR_MEMORY_EXAUSTED (11) +#define INV_ERROR_DIVIDE_BY_ZERO (12) +#define INV_ERROR_ASSERTION_FAILURE (13) +#define INV_ERROR_FILE_OPEN (14) +#define INV_ERROR_FILE_READ (15) +#define INV_ERROR_FILE_WRITE (16) +#define INV_ERROR_INVALID_CONFIGURATION (17) + +/* Serial Communication */ +#define INV_ERROR_SERIAL_CLOSED (20) +#define INV_ERROR_SERIAL_OPEN_ERROR (21) +#define INV_ERROR_SERIAL_READ (22) +#define INV_ERROR_SERIAL_WRITE (23) +#define INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED (24) + +/* SM = State Machine */ +#define INV_ERROR_SM_TRANSITION (25) +#define INV_ERROR_SM_IMPROPER_STATE (26) + +/* Fifo */ +#define INV_ERROR_FIFO_OVERFLOW (30) +#define INV_ERROR_FIFO_FOOTER (31) +#define INV_ERROR_FIFO_READ_COUNT (32) +#define INV_ERROR_FIFO_READ_DATA (33) + +/* Memory & Registers, Set & Get */ +#define INV_ERROR_MEMORY_SET (40) + +#define INV_ERROR_LOG_MEMORY_ERROR (50) +#define INV_ERROR_LOG_OUTPUT_ERROR (51) + +/* OS interface errors */ +#define INV_ERROR_OS_BAD_PTR (60) +#define INV_ERROR_OS_BAD_HANDLE (61) +#define INV_ERROR_OS_CREATE_FAILED (62) +#define INV_ERROR_OS_LOCK_FAILED (63) + +/* Compass errors */ +#define INV_ERROR_COMPASS_DATA_OVERFLOW (70) +#define INV_ERROR_COMPASS_DATA_UNDERFLOW (71) +#define INV_ERROR_COMPASS_DATA_NOT_READY (72) +#define INV_ERROR_COMPASS_DATA_ERROR (73) + +/* Load/Store calibration */ +#define INV_ERROR_CALIBRATION_LOAD (75) +#define INV_ERROR_CALIBRATION_STORE (76) +#define INV_ERROR_CALIBRATION_LEN (77) +#define INV_ERROR_CALIBRATION_CHECKSUM (78) + +/* Accel errors */ +#define INV_ERROR_ACCEL_DATA_OVERFLOW (79) +#define INV_ERROR_ACCEL_DATA_UNDERFLOW (80) +#define INV_ERROR_ACCEL_DATA_NOT_READY (81) +#define INV_ERROR_ACCEL_DATA_ERROR (82) + +#ifdef INV_USE_LEGACY_NAMES +#define ML_SUCCESS INV_SUCCESS +#define ML_ERROR INV_ERROR +#define ML_ERROR_INVALID_PARAMETER INV_ERROR_INVALID_PARAMETER +#define ML_ERROR_FEATURE_NOT_ENABLED INV_ERROR_FEATURE_NOT_ENABLED +#define ML_ERROR_FEATURE_NOT_IMPLEMENTED INV_ERROR_FEATURE_NOT_IMPLEMENTED +#define ML_ERROR_DMP_NOT_STARTED INV_ERROR_DMP_NOT_STARTED +#define ML_ERROR_DMP_STARTED INV_ERROR_DMP_STARTED +#define ML_ERROR_NOT_OPENED INV_ERROR_NOT_OPENED +#define ML_ERROR_OPENED INV_ERROR_OPENED +#define ML_ERROR_INVALID_MODULE INV_ERROR_INVALID_MODULE +#define ML_ERROR_MEMORY_EXAUSTED INV_ERROR_MEMORY_EXAUSTED +#define ML_ERROR_DIVIDE_BY_ZERO INV_ERROR_DIVIDE_BY_ZERO +#define ML_ERROR_ASSERTION_FAILURE INV_ERROR_ASSERTION_FAILURE +#define ML_ERROR_FILE_OPEN INV_ERROR_FILE_OPEN +#define ML_ERROR_FILE_READ INV_ERROR_FILE_READ +#define ML_ERROR_FILE_WRITE INV_ERROR_FILE_WRITE +#define ML_ERROR_INVALID_CONFIGURATION INV_ERROR_INVALID_CONFIGURATION +#define ML_ERROR_SERIAL_CLOSED INV_ERROR_SERIAL_CLOSED +#define ML_ERROR_SERIAL_OPEN_ERROR INV_ERROR_SERIAL_OPEN_ERROR +#define ML_ERROR_SERIAL_READ INV_ERROR_SERIAL_READ +#define ML_ERROR_SERIAL_WRITE INV_ERROR_SERIAL_WRITE +#define ML_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED \ + INV_ERROR_SERIAL_DEVICE_NOT_RECOGNIZED +#define ML_ERROR_SM_TRANSITION INV_ERROR_SM_TRANSITION +#define ML_ERROR_SM_IMPROPER_STATE INV_ERROR_SM_IMPROPER_STATE +#define ML_ERROR_FIFO_OVERFLOW INV_ERROR_FIFO_OVERFLOW +#define ML_ERROR_FIFO_FOOTER INV_ERROR_FIFO_FOOTER +#define ML_ERROR_FIFO_READ_COUNT INV_ERROR_FIFO_READ_COUNT +#define ML_ERROR_FIFO_READ_DATA INV_ERROR_FIFO_READ_DATA +#define ML_ERROR_MEMORY_SET INV_ERROR_MEMORY_SET +#define ML_ERROR_LOG_MEMORY_ERROR INV_ERROR_LOG_MEMORY_ERROR +#define ML_ERROR_LOG_OUTPUT_ERROR INV_ERROR_LOG_OUTPUT_ERROR +#define ML_ERROR_OS_BAD_PTR INV_ERROR_OS_BAD_PTR +#define ML_ERROR_OS_BAD_HANDLE INV_ERROR_OS_BAD_HANDLE +#define ML_ERROR_OS_CREATE_FAILED INV_ERROR_OS_CREATE_FAILED +#define ML_ERROR_OS_LOCK_FAILED INV_ERROR_OS_LOCK_FAILED +#define ML_ERROR_COMPASS_DATA_OVERFLOW INV_ERROR_COMPASS_DATA_OVERFLOW +#define ML_ERROR_COMPASS_DATA_UNDERFLOW INV_ERROR_COMPASS_DATA_UNDERFLOW +#define ML_ERROR_COMPASS_DATA_NOT_READY INV_ERROR_COMPASS_DATA_NOT_READY +#define ML_ERROR_COMPASS_DATA_ERROR INV_ERROR_COMPASS_DATA_ERROR +#define ML_ERROR_CALIBRATION_LOAD INV_ERROR_CALIBRATION_LOAD +#define ML_ERROR_CALIBRATION_STORE INV_ERROR_CALIBRATION_STORE +#define ML_ERROR_CALIBRATION_LEN INV_ERROR_CALIBRATION_LEN +#define ML_ERROR_CALIBRATION_CHECKSUM INV_ERROR_CALIBRATION_CHECKSUM +#define ML_ERROR_ACCEL_DATA_OVERFLOW INV_ERROR_ACCEL_DATA_OVERFLOW +#define ML_ERROR_ACCEL_DATA_UNDERFLOW INV_ERROR_ACCEL_DATA_UNDERFLOW +#define ML_ERROR_ACCEL_DATA_NOT_READY INV_ERROR_ACCEL_DATA_NOT_READY +#define ML_ERROR_ACCEL_DATA_ERROR INV_ERROR_ACCEL_DATA_ERROR +#endif + +/* For Linux coding compliance */ +#ifndef __KERNEL__ +#define EXPORT_SYMBOL(x) +#endif + +/*--------------------------- + p-Types +---------------------------*/ + +#endif /* MLTYPES_H */ diff --git a/60xx/mlsdk/platform/include/mpu3050.h b/60xx/mlsdk/platform/include/mpu3050.h new file mode 100644 index 0000000..c363a00 --- /dev/null +++ b/60xx/mlsdk/platform/include/mpu3050.h @@ -0,0 +1,255 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef __MPU_H_ +#error Do not include this file directly. Include mpu.h instead. +#endif + +#ifndef __MPU3050_H_ +#define __MPU3050_H_ + +#ifdef __KERNEL__ +#include +#endif + +#if !defined CONFIG_MPU_SENSORS_MPU3050 +#error MPU6000 build including MPU3050 header +#endif + +#define MPU_NAME "mpu3050" +#define DEFAULT_MPU_SLAVEADDR 0x68 + +/*==== MPU REGISTER SET ====*/ +enum mpu_register { + MPUREG_WHO_AM_I = 0, /* 00 0x00 */ + MPUREG_PRODUCT_ID, /* 01 0x01 */ + MPUREG_02_RSVD, /* 02 0x02 */ + MPUREG_03_RSVD, /* 03 0x03 */ + MPUREG_04_RSVD, /* 04 0x04 */ + MPUREG_XG_OFFS_TC, /* 05 0x05 */ + MPUREG_06_RSVD, /* 06 0x06 */ + MPUREG_07_RSVD, /* 07 0x07 */ + MPUREG_YG_OFFS_TC, /* 08 0x08 */ + MPUREG_09_RSVD, /* 09 0x09 */ + MPUREG_0A_RSVD, /* 10 0x0a */ + MPUREG_ZG_OFFS_TC, /* 11 0x0b */ + MPUREG_X_OFFS_USRH, /* 12 0x0c */ + MPUREG_X_OFFS_USRL, /* 13 0x0d */ + MPUREG_Y_OFFS_USRH, /* 14 0x0e */ + MPUREG_Y_OFFS_USRL, /* 15 0x0f */ + MPUREG_Z_OFFS_USRH, /* 16 0x10 */ + MPUREG_Z_OFFS_USRL, /* 17 0x11 */ + MPUREG_FIFO_EN1, /* 18 0x12 */ + MPUREG_FIFO_EN2, /* 19 0x13 */ + MPUREG_AUX_SLV_ADDR, /* 20 0x14 */ + MPUREG_SMPLRT_DIV, /* 21 0x15 */ + MPUREG_DLPF_FS_SYNC, /* 22 0x16 */ + MPUREG_INT_CFG, /* 23 0x17 */ + MPUREG_ACCEL_BURST_ADDR,/* 24 0x18 */ + MPUREG_19_RSVD, /* 25 0x19 */ + MPUREG_INT_STATUS, /* 26 0x1a */ + MPUREG_TEMP_OUT_H, /* 27 0x1b */ + MPUREG_TEMP_OUT_L, /* 28 0x1c */ + MPUREG_GYRO_XOUT_H, /* 29 0x1d */ + MPUREG_GYRO_XOUT_L, /* 30 0x1e */ + MPUREG_GYRO_YOUT_H, /* 31 0x1f */ + MPUREG_GYRO_YOUT_L, /* 32 0x20 */ + MPUREG_GYRO_ZOUT_H, /* 33 0x21 */ + MPUREG_GYRO_ZOUT_L, /* 34 0x22 */ + MPUREG_23_RSVD, /* 35 0x23 */ + MPUREG_24_RSVD, /* 36 0x24 */ + MPUREG_25_RSVD, /* 37 0x25 */ + MPUREG_26_RSVD, /* 38 0x26 */ + MPUREG_27_RSVD, /* 39 0x27 */ + MPUREG_28_RSVD, /* 40 0x28 */ + MPUREG_29_RSVD, /* 41 0x29 */ + MPUREG_2A_RSVD, /* 42 0x2a */ + MPUREG_2B_RSVD, /* 43 0x2b */ + MPUREG_2C_RSVD, /* 44 0x2c */ + MPUREG_2D_RSVD, /* 45 0x2d */ + MPUREG_2E_RSVD, /* 46 0x2e */ + MPUREG_2F_RSVD, /* 47 0x2f */ + MPUREG_30_RSVD, /* 48 0x30 */ + MPUREG_31_RSVD, /* 49 0x31 */ + MPUREG_32_RSVD, /* 50 0x32 */ + MPUREG_33_RSVD, /* 51 0x33 */ + MPUREG_34_RSVD, /* 52 0x34 */ + MPUREG_DMP_CFG_1, /* 53 0x35 */ + MPUREG_DMP_CFG_2, /* 54 0x36 */ + MPUREG_BANK_SEL, /* 55 0x37 */ + MPUREG_MEM_START_ADDR, /* 56 0x38 */ + MPUREG_MEM_R_W, /* 57 0x39 */ + MPUREG_FIFO_COUNTH, /* 58 0x3a */ + MPUREG_FIFO_COUNTL, /* 59 0x3b */ + MPUREG_FIFO_R_W, /* 60 0x3c */ + MPUREG_USER_CTRL, /* 61 0x3d */ + MPUREG_PWR_MGM, /* 62 0x3e */ + MPUREG_3F_RSVD, /* 63 0x3f */ + NUM_OF_MPU_REGISTERS /* 64 0x40 */ +}; + +/*==== BITS FOR MPU ====*/ + +/*---- MPU 'FIFO_EN1' register (12) ----*/ +#define BIT_TEMP_OUT 0x80 +#define BIT_GYRO_XOUT 0x40 +#define BIT_GYRO_YOUT 0x20 +#define BIT_GYRO_ZOUT 0x10 +#define BIT_ACCEL_XOUT 0x08 +#define BIT_ACCEL_YOUT 0x04 +#define BIT_ACCEL_ZOUT 0x02 +#define BIT_AUX_1OUT 0x01 +/*---- MPU 'FIFO_EN2' register (13) ----*/ +#define BIT_AUX_2OUT 0x02 +#define BIT_AUX_3OUT 0x01 +/*---- MPU 'DLPF_FS_SYNC' register (16) ----*/ +#define BITS_EXT_SYNC_NONE 0x00 +#define BITS_EXT_SYNC_TEMP 0x20 +#define BITS_EXT_SYNC_GYROX 0x40 +#define BITS_EXT_SYNC_GYROY 0x60 +#define BITS_EXT_SYNC_GYROZ 0x80 +#define BITS_EXT_SYNC_ACCELX 0xA0 +#define BITS_EXT_SYNC_ACCELY 0xC0 +#define BITS_EXT_SYNC_ACCELZ 0xE0 +#define BITS_EXT_SYNC_MASK 0xE0 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 +#define BITS_DLPF_CFG_MASK 0x07 +/*---- MPU 'INT_CFG' register (17) ----*/ +#define BIT_ACTL 0x80 +#define BIT_ACTL_LOW 0x80 +#define BIT_ACTL_HIGH 0x00 +#define BIT_OPEN 0x40 +#define BIT_OPEN_DRAIN 0x40 +#define BIT_PUSH_PULL 0x00 +#define BIT_LATCH_INT_EN 0x20 +#define BIT_INT_PULSE_WIDTH_50US 0x00 +#define BIT_INT_ANYRD_2CLEAR 0x10 +#define BIT_INT_STAT_READ_2CLEAR 0x00 +#define BIT_MPU_RDY_EN 0x04 +#define BIT_DMP_INT_EN 0x02 +#define BIT_RAW_RDY_EN 0x01 +/*---- MPU 'INT_STATUS' register (1A) ----*/ +#define BIT_INT_STATUS_FIFO_OVERLOW 0x80 +#define BIT_MPU_RDY 0x04 +#define BIT_DMP_INT 0x02 +#define BIT_RAW_RDY 0x01 +/*---- MPU 'BANK_SEL' register (37) ----*/ +#define BIT_PRFTCH_EN 0x20 +#define BIT_CFG_USER_BANK 0x10 +#define BITS_MEM_SEL 0x0f +/*---- MPU 'USER_CTRL' register (3D) ----*/ +#define BIT_DMP_EN 0x80 +#define BIT_FIFO_EN 0x40 +#define BIT_AUX_IF_EN 0x20 +#define BIT_AUX_RD_LENG 0x10 +#define BIT_AUX_IF_RST 0x08 +#define BIT_DMP_RST 0x04 +#define BIT_FIFO_RST 0x02 +#define BIT_GYRO_RST 0x01 +/*---- MPU 'PWR_MGM' register (3E) ----*/ +#define BIT_H_RESET 0x80 +#define BIT_SLEEP 0x40 +#define BIT_STBY_XG 0x20 +#define BIT_STBY_YG 0x10 +#define BIT_STBY_ZG 0x08 +#define BITS_CLKSEL 0x07 + +/*---- MPU Silicon Revision ----*/ +#define MPU_SILICON_REV_A4 1 /* MPU A4 Device */ +#define MPU_SILICON_REV_B1 2 /* MPU B1 Device */ +#define MPU_SILICON_REV_B4 3 /* MPU B4 Device */ +#define MPU_SILICON_REV_B6 4 /* MPU B6 Device */ + +/*---- MPU Memory ----*/ +#define MPU_MEM_BANK_SIZE (256) +#define FIFO_HW_SIZE (512) + +enum MPU_MEMORY_BANKS { + MPU_MEM_RAM_BANK_0 = 0, + MPU_MEM_RAM_BANK_1, + MPU_MEM_RAM_BANK_2, + MPU_MEM_RAM_BANK_3, + MPU_MEM_NUM_RAM_BANKS, + MPU_MEM_OTP_BANK_0 = MPU_MEM_NUM_RAM_BANKS, + /* This one is always last */ + MPU_MEM_NUM_BANKS +}; + +/*---- structure containing control variables used by MLDL ----*/ +/*---- MPU clock source settings ----*/ +/*---- MPU filter selections ----*/ +enum mpu_filter { + MPU_FILTER_256HZ_NOLPF2 = 0, + MPU_FILTER_188HZ, + MPU_FILTER_98HZ, + MPU_FILTER_42HZ, + MPU_FILTER_20HZ, + MPU_FILTER_10HZ, + MPU_FILTER_5HZ, + MPU_FILTER_2100HZ_NOLPF, + NUM_MPU_FILTER +}; + +enum mpu_fullscale { + MPU_FS_250DPS = 0, + MPU_FS_500DPS, + MPU_FS_1000DPS, + MPU_FS_2000DPS, + NUM_MPU_FS +}; + +enum mpu_clock_sel { + MPU_CLK_SEL_INTERNAL = 0, + MPU_CLK_SEL_PLLGYROX, + MPU_CLK_SEL_PLLGYROY, + MPU_CLK_SEL_PLLGYROZ, + MPU_CLK_SEL_PLLEXT32K, + MPU_CLK_SEL_PLLEXT19M, + MPU_CLK_SEL_RESERVED, + MPU_CLK_SEL_STOP, + NUM_CLK_SEL +}; + +enum mpu_ext_sync { + MPU_EXT_SYNC_NONE = 0, + MPU_EXT_SYNC_TEMP, + MPU_EXT_SYNC_GYROX, + MPU_EXT_SYNC_GYROY, + MPU_EXT_SYNC_GYROZ, + MPU_EXT_SYNC_ACCELX, + MPU_EXT_SYNC_ACCELY, + MPU_EXT_SYNC_ACCELZ, + NUM_MPU_EXT_SYNC +}; + +#define DLPF_FS_SYNC_VALUE(ext_sync, full_scale, lpf) \ + ((ext_sync << 5) | (full_scale << 3) | lpf) + +#endif /* __MPU3050_H_ */ diff --git a/60xx/mlsdk/platform/include/stdint_invensense.h b/60xx/mlsdk/platform/include/stdint_invensense.h new file mode 100644 index 0000000..d238813 --- /dev/null +++ b/60xx/mlsdk/platform/include/stdint_invensense.h @@ -0,0 +1,51 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +#ifndef STDINT_INVENSENSE_H +#define STDINT_INVENSENSE_H + +#ifndef WIN32 + +#ifdef __KERNEL__ +#include +#else +#include +#endif + +#else + +#include + +typedef char int8_t; +typedef short int16_t; +typedef long int32_t; + +typedef unsigned char uint8_t; +typedef unsigned short uint16_t; +typedef unsigned long uint32_t; + +typedef int int_fast8_t; +typedef int int_fast16_t; +typedef long int_fast32_t; + +typedef unsigned int uint_fast8_t; +typedef unsigned int uint_fast16_t; +typedef unsigned long uint_fast32_t; + +#endif + +#endif // STDINT_INVENSENSE_H diff --git a/60xx/mlsdk/platform/linux/kernel/mpuirq.h b/60xx/mlsdk/platform/linux/kernel/mpuirq.h new file mode 100644 index 0000000..352170b --- /dev/null +++ b/60xx/mlsdk/platform/linux/kernel/mpuirq.h @@ -0,0 +1,41 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef __MPUIRQ__ +#define __MPUIRQ__ + +#ifdef __KERNEL__ +#include +#include +#include "mldl_cfg.h" +#else +#include +#include +#endif + +#define MPUIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x40, unsigned long) +#define MPUIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x41, unsigned long) +#define MPUIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x42, struct timeval) +#define MPUIRQ_SET_FREQUENCY_DIVIDER _IOW(MPU_IOCTL, 0x43, unsigned long) + +#ifdef __KERNEL__ +void mpuirq_exit(void); +int mpuirq_init(struct i2c_client *mpu_client, struct mldl_cfg *mldl_cfg); +#endif + +#endif diff --git a/60xx/mlsdk/platform/linux/kernel/slaveirq.h b/60xx/mlsdk/platform/linux/kernel/slaveirq.h new file mode 100644 index 0000000..beb352b --- /dev/null +++ b/60xx/mlsdk/platform/linux/kernel/slaveirq.h @@ -0,0 +1,35 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef __SLAVEIRQ__ +#define __SLAVEIRQ__ + +#include +#include "mpuirq.h" + +#define SLAVEIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x50, unsigned long) +#define SLAVEIRQ_GET_INTERRUPT_CNT _IOR(MPU_IOCTL, 0x51, unsigned long) +#define SLAVEIRQ_GET_IRQ_TIME _IOR(MPU_IOCTL, 0x52, unsigned long) + + +void slaveirq_exit(struct ext_slave_platform_data *pdata); +int slaveirq_init(struct i2c_adapter *slave_adapter, + struct ext_slave_platform_data *pdata, char *name); + + +#endif diff --git a/60xx/mlsdk/platform/linux/kernel/timerirq.h b/60xx/mlsdk/platform/linux/kernel/timerirq.h new file mode 100644 index 0000000..dc3eea2 --- /dev/null +++ b/60xx/mlsdk/platform/linux/kernel/timerirq.h @@ -0,0 +1,29 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +#ifndef __TIMERIRQ__ +#define __TIMERIRQ__ + +#include + +#define TIMERIRQ_SET_TIMEOUT _IOW(MPU_IOCTL, 0x60, unsigned long) +#define TIMERIRQ_GET_INTERRUPT_CNT _IOW(MPU_IOCTL, 0x61, unsigned long) +#define TIMERIRQ_START _IOW(MPU_IOCTL, 0x62, unsigned long) +#define TIMERIRQ_STOP _IO(MPU_IOCTL, 0x63) + +#endif diff --git a/60xx/mlsdk/platform/linux/log_linux.c b/60xx/mlsdk/platform/linux/log_linux.c new file mode 100644 index 0000000..8e75753 --- /dev/null +++ b/60xx/mlsdk/platform/linux/log_linux.c @@ -0,0 +1,114 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/****************************************************************************** + * $Id: log_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + ******************************************************************************/ + +/** + * @defgroup MPL_LOG + * @brief Logging facility for the MPL + * + * @{ + * @file log.c + * @brief Core logging facility functions. + * + * +**/ + +#include +#include +#include "log.h" +#include "mltypes.h" + +#define LOG_BUFFER_SIZE (256) + +#ifdef WIN32 +#define snprintf _snprintf +#define vsnprintf _vsnprintf +#endif + +int _MLPrintLog (int priority, const char* tag, const char* fmt, ...) +{ + va_list ap; + int result; + + va_start(ap, fmt); + result = _MLPrintVaLog(priority,tag,fmt,ap); + va_end(ap); + + return result; +} + +int _MLPrintVaLog(int priority, const char* tag, const char* fmt, va_list args) +{ + int result; + char buf[LOG_BUFFER_SIZE]; + char new_fmt[LOG_BUFFER_SIZE]; + char priority_char; + + if (NULL == fmt) { + fmt = ""; + } + + switch (priority) { + case MPL_LOG_UNKNOWN: + priority_char = 'U'; + break; + case MPL_LOG_VERBOSE: + priority_char = 'V'; + break; + case MPL_LOG_DEBUG: + priority_char = 'D'; + break; + case MPL_LOG_INFO: + priority_char = 'I'; + break; + case MPL_LOG_WARN: + priority_char = 'W'; + break; + case MPL_LOG_ERROR: + priority_char = 'E'; + break; + case MPL_LOG_SILENT: + priority_char = 'S'; + break; + case MPL_LOG_DEFAULT: + default: + priority_char = 'D'; + break; + }; + + result = snprintf(new_fmt, sizeof(new_fmt), "%c/%s:%s", + priority_char, tag, fmt); + if (result <= 0) { + return INV_ERROR_LOG_MEMORY_ERROR; + } + result = vsnprintf(buf,sizeof(buf),new_fmt, args); + if (result <= 0) { + return INV_ERROR_LOG_OUTPUT_ERROR; + } + + result = _MLWriteLog(buf, strlen(buf)); + return INV_SUCCESS; +} + +/** + * @} +**/ + + diff --git a/60xx/mlsdk/platform/linux/log_printf_linux.c b/60xx/mlsdk/platform/linux/log_printf_linux.c new file mode 100644 index 0000000..744a223 --- /dev/null +++ b/60xx/mlsdk/platform/linux/log_printf_linux.c @@ -0,0 +1,43 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: log_printf_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + ******************************************************************************/ + +/** + * @addtogroup MPL_LOG + * + * @{ + * @file log_printf.c + * @brief printf replacement for _MLWriteLog. + */ + +#include +#include "log.h" + +int _MLWriteLog (const char * buf, int buflen) +{ + return fputs(buf, stdout); +} + +/** + * @} + */ + diff --git a/60xx/mlsdk/platform/linux/mlos_linux.c b/60xx/mlsdk/platform/linux/mlos_linux.c new file mode 100644 index 0000000..fd3b002 --- /dev/null +++ b/60xx/mlsdk/platform/linux/mlos_linux.c @@ -0,0 +1,206 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ +/******************************************************************************* + * + * $Id: mlos_linux.c 5629 2011-06-11 03:13:08Z mcaramello $ + * + *******************************************************************************/ + +/** + * @defgroup MLOS + * @brief OS Interface. + * + * @{ + * @file mlos.c + * @brief OS Interface. +**/ + +/* ------------- */ +/* - Includes. - */ +/* ------------- */ + +#include +#include +#include +#include + +#include "stdint_invensense.h" + +#include "mlos.h" +#include + + +/* -------------- */ +/* - Functions. - */ +/* -------------- */ + +/** + * @brief Allocate space + * @param numBytes number of bytes + * @return pointer to allocated space +**/ +void *inv_malloc(unsigned int numBytes) +{ + // Allocate space. + void *allocPtr = malloc(numBytes); + return allocPtr; +} + + +/** + * @brief Free allocated space + * @param ptr pointer to space to deallocate + * @return error code. +**/ +inv_error_t inv_free(void *ptr) +{ + // Deallocate space. + free(ptr); + + return INV_SUCCESS; +} + + +/** + * @brief Mutex create function + * @param mutex pointer to mutex handle + * @return error code. + */ +inv_error_t inv_create_mutex(HANDLE *mutex) +{ + int res; + pthread_mutex_t *pm = malloc(sizeof(pthread_mutex_t)); + if(pm == NULL) + return INV_ERROR; + + res = pthread_mutex_init(pm, NULL); + if(res == -1) { + free(pm); + return INV_ERROR_OS_CREATE_FAILED; + } + + *mutex = (HANDLE)pm; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex lock function + * @param mutex Mutex handle + * @return error code. + */ +inv_error_t inv_lock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_lock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief Mutex unlock function + * @param mutex mutex handle + * @return error code. +**/ +inv_error_t inv_unlock_mutex(HANDLE mutex) +{ + int res; + pthread_mutex_t *pm = (pthread_mutex_t*)mutex; + + res = pthread_mutex_unlock(pm); + if(res == -1) + return INV_ERROR_OS_LOCK_FAILED; + + return INV_SUCCESS; +} + + +/** + * @brief open file + * @param filename name of the file to open. + * @return error code. + */ +FILE *inv_fopen(char *filename) +{ + FILE *fp = fopen(filename,"r"); + return fp; +} + + +/** + * @brief close the file. + * @param fp handle to file to close. + * @return error code. + */ +void inv_fclose(FILE *fp) +{ + fclose(fp); +} + +/** + * @brief Close Handle + * @param handle handle to the resource. + * @return Zero if success, an error code otherwise. + */ +inv_error_t inv_destroy_mutex(HANDLE handle) +{ + int error; + pthread_mutex_t *pm = (pthread_mutex_t*)handle; + error = pthread_mutex_destroy(pm); + if (error) { + return errno; + } + free((void*) handle); + + return INV_SUCCESS;} + + +/** + * @brief Sleep function. + */ +void inv_sleep(int mSecs) +{ + usleep(mSecs*1000); +} + + +/** + * @brief get system's internal tick count. + * Used for time reference. + * @return current tick count. + */ +unsigned long inv_get_tick_count() +{ + struct timeval tv; + + if (gettimeofday(&tv, NULL) !=0) + return 0; + + return (long)((tv.tv_sec * 1000000LL + tv.tv_usec) / 1000LL); +} + + /**********************/ + /** @} */ /* defgroup */ +/**********************/ + diff --git a/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c new file mode 100644 index 0000000..1bd71de --- /dev/null +++ b/60xx/mlsdk/platform/linux/mlsl_linux_mpu.c @@ -0,0 +1,497 @@ +/* + $License: + Copyright 2011 InvenSense, Inc. + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. + $ + */ + +/****************************************************************************** + * $Id: mlsl_linux_mpu.c 5653 2011-06-16 21:06:55Z nroyer $ + *****************************************************************************/ + +/** + * @defgroup MLSL (Motion Library - Serial Layer) + * @brief The Motion Library System Layer provides the Motion Library the + * interface to the system functions. + * + * @{ + * @file mlsl_linux_mpu.c + * @brief The Motion Library System Layer. + * + */ + +/* ------------------ */ +/* - Include Files. - */ +/* ------------------ */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mpu.h" +#if defined CONFIG_MPU_SENSORS_MPU6050A2 +# include "mpu6050a2.h" +#elif defined CONFIG_MPU_SENSORS_MPU6050B1 +# include "mpu6050b1.h" +#elif defined CONFIG_MPU_SENSORS_MPU3050 +# include "mpu3050.h" +#else +#error Invalid or undefined CONFIG_MPU_SENSORS_MPUxxxx +#endif + +#include "mlsl.h" +#include "mlos.h" +#include "mlmath.h" +#include "mlinclude.h" + +#define MLCAL_ID (0x0A0B0C0DL) +#define MLCAL_FILE "/data/cal.bin" +#define MLCFG_ID (0x01020304L) +#define MLCFG_FILE "/data/cfg.bin" + +#include +#undef MPL_LOG_TAG +#define MPL_LOG_TAG "MPL-mlsl" + +#ifndef I2CDEV +#define I2CDEV "/dev/mpu" +#endif + +#define SERIAL_FULL_DEBUG (0) + +/* --------------- */ +/* - Prototypes. - */ +/* --------------- */ + +/* ----------------------- */ +/* - Function Pointers. - */ +/* ----------------------- */ + +/* --------------------------- */ +/* - Global and Static vars. - */ +/* --------------------------- */ + +/* ---------------- */ +/* - Definitions. - */ +/* ---------------- */ + +inv_error_t inv_serial_read_cfg(unsigned char *cfg, unsigned int len) +{ + FILE *fp; + int bytesRead; + + fp = fopen(MLCFG_FILE, "rb"); + if (fp == NULL) { + MPL_LOGE("Unable to open \"%s\" for read\n", MLCFG_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesRead = fread(cfg, 1, len, fp); + if (bytesRead != len) { + MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", + bytesRead, len); + return INV_ERROR_FILE_READ; + } + fclose(fp); + + if (((unsigned int)cfg[0] << 24 | cfg[1] << 16 | cfg[2] << 8 | cfg[3]) + != MLCFG_ID) { + return INV_ERROR_ASSERTION_FAILURE; + } + + return INV_SUCCESS; +} + +inv_error_t inv_serial_write_cfg(unsigned char *cfg, unsigned int len) +{ + FILE *fp; + int bytesWritten; + unsigned char cfgId[4]; + + fp = fopen(MLCFG_FILE,"wb"); + if (fp == NULL) { + MPL_LOGE("Unable to open \"%s\" for write\n", MLCFG_FILE); + return INV_ERROR_FILE_OPEN; + } + + cfgId[0] = (unsigned char)(MLCFG_ID >> 24); + cfgId[1] = (unsigned char)(MLCFG_ID >> 16); + cfgId[2] = (unsigned char)(MLCFG_ID >> 8); + cfgId[3] = (unsigned char)(MLCFG_ID); + bytesWritten = fwrite(cfgId, 1, 4, fp); + if (bytesWritten != 4) { + MPL_LOGE("CFG ID could not be written on file\n"); + return INV_ERROR_FILE_WRITE; + } + + bytesWritten = fwrite(cfg, 1, len, fp); + if (bytesWritten != len) { + MPL_LOGE("bytes write (%d) don't match requested length (%d)\n", + bytesWritten, len); + return INV_ERROR_FILE_WRITE; + } + + fclose(fp); + + return INV_SUCCESS; +} + +inv_error_t inv_serial_read_cal(unsigned char *cal, unsigned int len) +{ + FILE *fp; + int bytesRead; + inv_error_t result = INV_SUCCESS; + + fp = fopen(MLCAL_FILE,"rb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesRead = fread(cal, 1, len, fp); + if (bytesRead != len) { + MPL_LOGE("bytes read (%d) don't match requested length (%d)\n", + bytesRead, len); + result = INV_ERROR_FILE_READ; + goto read_cal_end; + } + + /* MLCAL_ID not used + if (((unsigned int)cal[0] << 24 | cal[1] << 16 | cal[2] << 8 | cal[3]) + != MLCAL_ID) { + result = INV_ERROR_ASSERTION_FAILURE; + goto read_cal_end; + } + */ +read_cal_end: + fclose(fp); + return result; +} + +inv_error_t inv_serial_write_cal(unsigned char *cal, unsigned int len) +{ + FILE *fp; + int bytesWritten; + inv_error_t result = INV_SUCCESS; + + fp = fopen(MLCAL_FILE,"wb"); + if (fp == NULL) { + MPL_LOGE("Cannot open file \"%s\" for write\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + bytesWritten = fwrite(cal, 1, len, fp); + if (bytesWritten != len) { + MPL_LOGE("bytes written (%d) don't match requested length (%d)\n", + bytesWritten, len); + result = INV_ERROR_FILE_WRITE; + } + fclose(fp); + return result; +} + +inv_error_t inv_serial_get_cal_length(unsigned int *len) +{ + FILE *calFile; + *len = 0; + + calFile = fopen(MLCAL_FILE, "rb"); + if (calFile == NULL) { + MPL_LOGE("Cannot open file \"%s\" for read\n", MLCAL_FILE); + return INV_ERROR_FILE_OPEN; + } + + *len += (unsigned char)fgetc(calFile) << 24; + *len += (unsigned char)fgetc(calFile) << 16; + *len += (unsigned char)fgetc(calFile) << 8; + *len += (unsigned char)fgetc(calFile); + + fclose(calFile); + + if (*len <= 0) + return INV_ERROR_FILE_READ; + + return INV_SUCCESS; +} + +inv_error_t inv_serial_open(char const *port, void **sl_handle) +{ + INVENSENSE_FUNC_START; + + if (NULL == port) { + port = I2CDEV; + } + *sl_handle = (void*)(uintptr_t) open(port, O_RDWR); + if((intptr_t)*sl_handle < 0) { + /* ERROR HANDLING; you can check errno to see what went wrong */ + MPL_LOGE("inv_serial_open\n"); + MPL_LOGE("I2C Error %d: Cannot open Adapter %s\n", errno, port); + return INV_ERROR_SERIAL_OPEN_ERROR; + } else { + MPL_LOGI("inv_serial_open: %s\n", port); + } + + return INV_SUCCESS; +} + +inv_error_t inv_serial_close(void *sl_handle) +{ + INVENSENSE_FUNC_START; + + close((int)(uintptr_t)sl_handle); + + return INV_SUCCESS; +} + +inv_error_t inv_serial_reset(void *sl_handle) +{ + return INV_ERROR_FEATURE_NOT_IMPLEMENTED; +} + +inv_error_t inv_serial_single_write(void *sl_handle, + unsigned char slaveAddr, + unsigned char registerAddr, + unsigned char data) +{ + unsigned char buf[2]; + buf[0] = registerAddr; + buf[1] = data; + return inv_serial_write(sl_handle, slaveAddr, 2, buf); +} + +inv_error_t inv_serial_write(void *sl_handle, + unsigned char slaveAddr, + unsigned short length, + unsigned char const *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + inv_error_t result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = 0; /* not used */ + msg.length = length; + msg.data = (unsigned char*)data; + + if ((result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE, &msg))) { + MPL_LOGE("I2C Error: could not write: R:%02x L:%d %d \n", + data[0], length, result); + return result; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, sizeof(data_buff)); + } + MPL_LOGI("I2C Write Success %02x %02x: %s \n", + data[0], length, data_buff); + } + + return INV_SUCCESS; +} + +inv_error_t inv_serial_read(void *sl_handle, + unsigned char slaveAddr, + unsigned char registerAddr, + unsigned short length, + unsigned char *data) +{ + INVENSENSE_FUNC_START; + int result = INV_SUCCESS; + struct mpu_read_write msg; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = registerAddr; + msg.length = length; + msg.data = data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_READ, &msg); + + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error %08x: could not read: R:%02x L:%d\n", + result, registerAddr, length); + result = INV_ERROR_SERIAL_READ; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, sizeof(data_buff)); + } + MPL_LOGI("I2C Read Success %02x %02x: %s \n", + registerAddr, length, data_buff); + } + + return (inv_error_t) result; +} + +inv_error_t inv_serial_write_mem(void *sl_handle, + unsigned char mpu_addr, + unsigned short memAddr, + unsigned short length, + const unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + msg.address = memAddr; + msg.length = length; + msg.data = (unsigned char *)data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_MEM, &msg); + if (result) { + LOG_RESULT_LOCATION(result); + return result; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, sizeof(data_buff)); + } + MPL_LOGI("I2C WriteMem Success %04x %04x: %s \n", + memAddr, length, data_buff); + } + return INV_SUCCESS; +} + +inv_error_t inv_serial_read_mem(void *sl_handle, + unsigned char mpu_addr, + unsigned short memAddr, + unsigned short length, + unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = memAddr; + msg.length = length; + msg.data = data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_MEM, &msg); + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error %08x: could not read memory: A:%04x L:%d\n", + result, memAddr, length); + return INV_ERROR_SERIAL_READ; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, sizeof(data_buff)); + } + MPL_LOGI("I2C ReadMem Success %04x %04x: %s\n", + memAddr, length, data_buff); + } + return INV_SUCCESS; +} + +inv_error_t inv_serial_write_fifo(void *sl_handle, + unsigned char mpu_addr, + unsigned short length, + const unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = 0; /* Not used */ + msg.length = length; + msg.data = (unsigned char *)data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_WRITE_FIFO, &msg); + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error: could not write fifo: %02x %02x\n", + MPUREG_FIFO_R_W, length); + return INV_ERROR_SERIAL_WRITE; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, sizeof(data_buff)); + } + MPL_LOGI("I2C Write Success %02x %02x: %s\n", + MPUREG_FIFO_R_W, length, data_buff); + } + return (inv_error_t) result; +} + +inv_error_t inv_serial_read_fifo(void *sl_handle, + unsigned char mpu_addr, + unsigned short length, + unsigned char *data) +{ + INVENSENSE_FUNC_START; + struct mpu_read_write msg; + int result; + + if (NULL == data) { + return INV_ERROR_INVALID_PARAMETER; + } + + msg.address = MPUREG_FIFO_R_W; /* Not used */ + msg.length = length; + msg.data = data; + + result = ioctl((int)(uintptr_t)sl_handle, MPU_READ_FIFO, &msg); + if (result != INV_SUCCESS) { + MPL_LOGE("I2C Error %08x: could not read fifo: R:%02x L:%d\n", + result, MPUREG_FIFO_R_W, length); + return INV_ERROR_SERIAL_READ; + } else if (SERIAL_FULL_DEBUG) { + char data_buff[4096] = ""; + char strchar[3]; + int ii; + for (ii = 0; ii < length; ii++) { + snprintf(strchar, sizeof(strchar), "%02x", data[0]); + strncat(data_buff, strchar, sizeof(data_buff)); + } + MPL_LOGI("I2C ReadFifo Success %02x %02x: %s\n", + MPUREG_FIFO_R_W, length, data_buff); + } + return INV_SUCCESS; +} + +/** + * @} + */ + + diff --git a/Android.mk b/Android.mk index ff26264..65c9e4f 100644 --- a/Android.mk +++ b/Android.mk @@ -2,7 +2,14 @@ ifneq ($(filter hammerhead, $(TARGET_DEVICE)),) # hammerhead expects 65xx sensors. include $(call all-named-subdir-makefiles,65xx) -else ifneq ($(filter guppy dory, $(TARGET_DEVICE)),) +else +ifneq ($(filter guppy dory, $(TARGET_DEVICE)),) # dory and guppy expect 6515 sensors. include $(call all-named-subdir-makefiles,6515) +else +ifneq ($(filter manta grouper tuna mako, $(TARGET_DEVICE)),) +# manta, grouper, tuna, and mako expect 60xx sensors. +include $(call all-named-subdir-makefiles,60xx) +endif +endif endif -- cgit v1.2.3