DO NOT MERGE
Merge pie-platform-release (PPRL.181105.017, history only) into master
Bug: 118454372
Change-Id: I622f5c591e963b7d45ac3f372e53f864826190e8
diff --git a/contexthubhal/system_comms.cpp b/contexthubhal/system_comms.cpp
index 6edd0cb..358e2f5 100644
--- a/contexthubhal/system_comms.cpp
+++ b/contexthubhal/system_comms.cpp
@@ -418,7 +418,7 @@
int SystemComm::AppMgmtSession::setupMgmt(const hub_message_t *appMsg, uint32_t transactionId, uint32_t cmd, AppManager &appManager)
{
- int32_t result = 0;
+ int32_t result = -1; // results are only directly returned on failure
const hub_app_name_t &appName = *static_cast<const hub_app_name_t*>(appMsg->message);
if (appMsg->message_len != sizeof(appName)) {
return -EINVAL;
diff --git a/contexthubhal/test/Android.bp b/contexthubhal/test/Android.bp
new file mode 100644
index 0000000..9ab543c
--- /dev/null
+++ b/contexthubhal/test/Android.bp
@@ -0,0 +1,39 @@
+//
+// Copyright (C) 2016 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.
+//
+
+cc_test {
+ name: "nanohub_hal_test",
+ gtest: false,
+
+ srcs: ["main.cpp"],
+
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+ "-DHOST_BUILD",
+ "-DBOOTLOADER=",
+ "-DBOOTLOADER_RO=",
+ ],
+
+ static_libs: ["libnanohub_common"],
+
+ shared_libs: [
+ "libhardware",
+ "liblog",
+ "libcutils",
+ ],
+}
diff --git a/contexthubhal/test/Android.mk b/contexthubhal/test/Android.mk
deleted file mode 100644
index 44d2744..0000000
--- a/contexthubhal/test/Android.mk
+++ /dev/null
@@ -1,48 +0,0 @@
-#
-# Copyright (C) 2016 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.
-#
-
-LOCAL_PATH := $(call my-dir)
-
-include $(CLEAR_VARS)
-
-
-LOCAL_SRC_FILES := main.cpp \
- ../../lib/nanohub/nanoapp.c \
-
-
-LOCAL_CFLAGS := \
- -Wall \
- -Werror \
- -Wextra \
- -DHOST_BUILD \
- -DBOOTLOADER= \
- -DBOOTLOADER_RO= \
-
-
-LOCAL_C_INCLUDES := \
- device/google/contexthub/lib/include \
- hardware/libhardware/include \
-
-LOCAL_SHARED_LIBRARIES := \
- libhardware \
- liblog \
- libcutils \
-
-LOCAL_MODULE := nanohub_hal_test
-
-LOCAL_MODULE_TAGS := optional
-
-include $(BUILD_EXECUTABLE)
diff --git a/firmware/argonkey_aux_variant_config.mk b/firmware/argonkey_aux_variant_config.mk
index e677395..6e244f6 100644
--- a/firmware/argonkey_aux_variant_config.mk
+++ b/firmware/argonkey_aux_variant_config.mk
@@ -54,25 +54,26 @@
device/google/contexthub/firmware/variant/$(my_variant)/inc \
# this is relative to NANOHUB_OS_PATH
-NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
- os/algos/calibration/accelerometer/accel_cal.c \
- os/algos/calibration/gyroscope/gyro_cal.c \
- os/algos/calibration/gyroscope/gyro_stillness_detect.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/calibration/common/diversity_checker.c \
- os/algos/calibration/over_temp/over_temp_cal.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c \
- os/algos/fusion.c \
- os/algos/time_sync.c \
- os/drivers/hall/hall.c \
- os/drivers/intersil_isl29034/isl29034.c \
- os/drivers/leds/leds_gpio.c \
- os/drivers/leds/leds_lp3943.c \
- os/drivers/orientation/orientation.c \
- os/drivers/st_hts221/hts221.c \
- os/drivers/st_lps22hb/lps22hb.c \
- os/drivers/st_lsm6dsm/st_lsm6dsm.c \
- os/drivers/tilt_detection/tilt_detection.c \
- os/drivers/window_orientation/window_orientation.c \
+NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
+ os/algos/calibration/accelerometer/accel_cal.c \
+ os/algos/calibration/gyroscope/gyro_cal.c \
+ os/algos/calibration/gyroscope/gyro_stillness_detect.c \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/calibration/over_temp/over_temp_cal.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c \
+ os/drivers/hall/hall.c \
+ os/drivers/intersil_isl29034/isl29034.c \
+ os/drivers/leds/leds_gpio.c \
+ os/drivers/leds/leds_lp3943.c \
+ os/drivers/orientation/orientation.c \
+ os/drivers/st_hts221/hts221.c \
+ os/drivers/st_lps22hb/lps22hb.c \
+ os/drivers/st_lsm6dsm/st_lsm6dsm.c \
+ os/drivers/tilt_detection/tilt_detection.c \
+ os/drivers/window_orientation/window_orientation.c \
diff --git a/firmware/lunchbox_aux_variant_config.mk b/firmware/lunchbox_aux_variant_config.mk
index e0228c9..b4e0285 100644
--- a/firmware/lunchbox_aux_variant_config.mk
+++ b/firmware/lunchbox_aux_variant_config.mk
@@ -59,29 +59,30 @@
#'Lu' -> lunchbox
NANO_VARIANT_CFLAGS_$(my_variant) += -DPLATFORM_HW_TYPE=0x4C75
-NANO_VARIANT_C_INCLUDES_$(my_variant) := \
- device/google/contexthub/firmware/variant/lunchbox/inc \
+NANO_VARIANT_C_INCLUDES_$(my_variant) := \
+ device/google/contexthub/firmware/variant/lunchbox/inc \
# this is relative to NANOHUB_OS_PATH
-NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
- os/algos/calibration/accelerometer/accel_cal.c \
- os/algos/calibration/gyroscope/gyro_cal.c \
- os/algos/calibration/gyroscope/gyro_stillness_detect.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/calibration/common/diversity_checker.c \
- os/algos/calibration/over_temp/over_temp_cal.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c \
- os/algos/fusion.c \
- os/algos/time_sync.c \
- os/drivers/ams_tmd2772/ams_tmd2772.c \
- os/drivers/bosch_bmi160/bosch_bmi160.c \
- os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
- os/drivers/bosch_bmp280/bosch_bmp280.c \
- os/drivers/hall/hall.c \
- os/drivers/orientation/orientation.c \
- os/drivers/rohm_rpr0521/rohm_rpr0521.c \
- os/drivers/tilt_detection/tilt_detection.c \
- os/drivers/vsync/vsync.c \
- os/drivers/window_orientation/window_orientation.c \
+NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
+ os/algos/calibration/accelerometer/accel_cal.c \
+ os/algos/calibration/gyroscope/gyro_cal.c \
+ os/algos/calibration/gyroscope/gyro_stillness_detect.c \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/calibration/over_temp/over_temp_cal.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c \
+ os/drivers/ams_tmd2772/ams_tmd2772.c \
+ os/drivers/bosch_bmi160/bosch_bmi160.c \
+ os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
+ os/drivers/bosch_bmp280/bosch_bmp280.c \
+ os/drivers/hall/hall.c \
+ os/drivers/orientation/orientation.c \
+ os/drivers/rohm_rpr0521/rohm_rpr0521.c \
+ os/drivers/tilt_detection/tilt_detection.c \
+ os/drivers/vsync/vsync.c \
+ os/drivers/window_orientation/window_orientation.c \
diff --git a/firmware/neonkey_aux_variant_config.mk b/firmware/neonkey_aux_variant_config.mk
index 6698c7c..d53ad8c 100644
--- a/firmware/neonkey_aux_variant_config.mk
+++ b/firmware/neonkey_aux_variant_config.mk
@@ -55,26 +55,27 @@
device/google/contexthub/firmware/variant/neonkey/inc \
# this is relative to NANOHUB_OS_PATH
-NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
- os/algos/calibration/accelerometer/accel_cal.c \
- os/algos/calibration/gyroscope/gyro_cal.c \
- os/algos/calibration/gyroscope/gyro_stillness_detect.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/calibration/common/diversity_checker.c \
- os/algos/calibration/over_temp/over_temp_cal.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c \
- os/algos/fusion.c \
- os/algos/time_sync.c \
- os/drivers/bosch_bmi160/bosch_bmi160.c \
- os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
- os/drivers/bosch_bmp280/bosch_bmp280.c \
- os/drivers/hall/hall.c \
- os/drivers/leds/leds_gpio.c \
- os/drivers/leds/leds_lp3943.c \
- os/drivers/orientation/orientation.c \
- os/drivers/rohm_rpr0521/rohm_rpr0521.c \
- os/drivers/si_si7034/si7034a10.c \
- os/drivers/tilt_detection/tilt_detection.c \
- os/drivers/window_orientation/window_orientation.c \
+NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
+ os/algos/calibration/accelerometer/accel_cal.c \
+ os/algos/calibration/gyroscope/gyro_cal.c \
+ os/algos/calibration/gyroscope/gyro_stillness_detect.c \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/calibration/over_temp/over_temp_cal.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c \
+ os/drivers/bosch_bmi160/bosch_bmi160.c \
+ os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
+ os/drivers/bosch_bmp280/bosch_bmp280.c \
+ os/drivers/hall/hall.c \
+ os/drivers/leds/leds_gpio.c \
+ os/drivers/leds/leds_lp3943.c \
+ os/drivers/orientation/orientation.c \
+ os/drivers/rohm_rpr0521/rohm_rpr0521.c \
+ os/drivers/si_si7034/si7034a10.c \
+ os/drivers/tilt_detection/tilt_detection.c \
+ os/drivers/window_orientation/window_orientation.c \
diff --git a/firmware/nucleo_aux_variant_config.mk b/firmware/nucleo_aux_variant_config.mk
index 06b01a5..c5b6d6b 100644
--- a/firmware/nucleo_aux_variant_config.mk
+++ b/firmware/nucleo_aux_variant_config.mk
@@ -55,25 +55,26 @@
device/google/contexthub/firmware/variant/nucleo/inc \
# this is relative to NANOHUB_OS_PATH
-NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
- os/algos/calibration/accelerometer/accel_cal.c \
- os/algos/calibration/gyroscope/gyro_cal.c \
- os/algos/calibration/gyroscope/gyro_stillness_detect.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/calibration/common/diversity_checker.c \
- os/algos/calibration/over_temp/over_temp_cal.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c \
- os/algos/fusion.c \
- os/algos/time_sync.c \
- os/drivers/ams_tmd2772/ams_tmd2772.c \
- os/drivers/bosch_bmi160/bosch_bmi160.c \
- os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
- os/drivers/bosch_bmp280/bosch_bmp280.c \
- os/drivers/hall/hall.c \
- os/drivers/orientation/orientation.c \
- os/drivers/rohm_rpr0521/rohm_rpr0521.c \
- os/drivers/tilt_detection/tilt_detection.c \
- os/drivers/vsync/vsync.c \
- os/drivers/window_orientation/window_orientation.c \
+NANO_VARIANT_OSCFG_SRC_FILES_$(my_variant) := \
+ os/algos/calibration/accelerometer/accel_cal.c \
+ os/algos/calibration/gyroscope/gyro_cal.c \
+ os/algos/calibration/gyroscope/gyro_stillness_detect.c \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/calibration/over_temp/over_temp_cal.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c \
+ os/drivers/ams_tmd2772/ams_tmd2772.c \
+ os/drivers/bosch_bmi160/bosch_bmi160.c \
+ os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
+ os/drivers/bosch_bmp280/bosch_bmp280.c \
+ os/drivers/hall/hall.c \
+ os/drivers/orientation/orientation.c \
+ os/drivers/rohm_rpr0521/rohm_rpr0521.c \
+ os/drivers/tilt_detection/tilt_detection.c \
+ os/drivers/vsync/vsync.c \
+ os/drivers/window_orientation/window_orientation.c \
diff --git a/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c b/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c
new file mode 100644
index 0000000..dc85f55
--- /dev/null
+++ b/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c
@@ -0,0 +1,3272 @@
+/*
+ * Copyright (C) 2018 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 <algos/time_sync.h>
+#include <stdlib.h>
+#include <string.h>
+#include <timer.h>
+#include <sensors.h>
+#include <heap.h>
+#include <spi.h>
+#include <slab.h>
+#include <limits.h>
+#include <atomic.h>
+#include <plat/rtc.h>
+#include <plat/gpio.h>
+#include <plat/exti.h>
+#include <plat/syscfg.h>
+#include <seos.h>
+#include <gpio.h>
+#include <isr.h>
+#include <halIntf.h>
+#include <hostIntf.h>
+#include <nanohubPacket.h>
+#include <cpu/cpuMath.h>
+#include <nanohub_math.h>
+#include <variant/sensType.h>
+#include <variant/variant.h>
+#include <common/math/macros.h>
+
+#ifdef ACCEL_CAL_ENABLED
+#include <calibration/accelerometer/accel_cal.h>
+#endif
+#ifdef GYRO_CAL_ENABLED
+#include <calibration/gyroscope/gyro_cal.h>
+#endif
+
+#define INFO_PRINT(fmt, ...) do { \
+ osLog(LOG_INFO, "%s " fmt, "[ICM40600]", ##__VA_ARGS__); \
+ } while (0);
+
+#define ERROR_PRINT(fmt, ...) do { \
+ osLog(LOG_ERROR, "%s " fmt, "[ICM40600] ERROR:", ##__VA_ARGS__); \
+ } while (0);
+
+#define DEBUG_PRINT(fmt, ...) do { \
+ if (DBG_ENABLE) { \
+ INFO_PRINT(fmt, ##__VA_ARGS__); \
+ } \
+ } while (0);
+
+#define DEBUG_PRINT_IF(cond, fmt, ...) do { \
+ if ((cond) && DBG_ENABLE) { \
+ INFO_PRINT(fmt, ##__VA_ARGS__); \
+ } \
+ } while (0);
+
+#define DBG_ENABLE 0
+#define DBG_STATE 0
+#define DBG_TIMESTAMP 0
+
+#define ICM40600_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_INVENSENSE, 0x2)
+#define ICM40600_APP_VERSION 1
+
+#define ICM40600_SPI_WRITE 0x00
+#define ICM40600_SPI_READ 0x80
+
+// SPI speeds officialy supported: 5MHz (low speed), 17MHz (high speed)
+#define ICM40600_SPI_LOW_SPEED_HZ 5000000
+#define ICM40600_SPI_HIGH_SPEED_HZ 17000000
+#define ICM40600_SPI_DEFAULT_SPEED_HZ ICM40600_SPI_LOW_SPEED_HZ
+#define ICM40600_SPI_MODE 3
+
+#ifndef ICM40600_SPI_BUS_ID
+#define ICM40600_SPI_BUS_ID 1
+#endif
+#ifndef ICM40600_INT1_PIN
+#define ICM40600_INT1_PIN GPIO_PB(6)
+#endif
+#ifndef ICM40600_INT1_IRQ
+#define ICM40600_INT1_IRQ EXTI9_5_IRQn
+#endif
+#define ICM40600_SPI_SPEED_HZ ICM40600_SPI_HIGH_SPEED_HZ
+
+// set SPI speed register value
+#if ICM40600_SPI_SPEED_HZ == ICM40600_SPI_LOW_SPEED_HZ
+#define ICM40600_SPI_SPEED_REG_VALUE BIT_SPI_SPEED_5MHZ
+#elif ICM40600_SPI_SPEED_HZ == ICM40600_SPI_HIGH_SPEED_HZ
+#define ICM40600_SPI_SPEED_REG_VALUE BIT_SPI_SPEED_17MHZ
+#else
+#error "Invalid ICM40600_SPI_SPEED_HZ setting: valid values are 5MHz or 17MHz"
+#endif
+
+/*
+ **********************
+ * Chip configuration
+ **********************
+ */
+/* Full-scale range */
+// Default accel range is 8g
+#ifndef ICM40600_ACC_RANGE_G
+#define ICM40600_ACC_RANGE_G 8
+#endif
+// Default gyro range is 2000dps
+#ifndef ICM40600_GYR_RANGE_DPS
+#define ICM40600_GYR_RANGE_DPS 2000
+#endif
+// 0 -> +-16g, 1 -> +-8g, 2 -> +-4g, 3 -> +-2g, 4 -> +-1g
+#if ICM40600_ACC_RANGE_G == 16
+#define ICM40600_ACC_FS 0
+#elif ICM40600_ACC_RANGE_G == 8
+#define ICM40600_ACC_FS 1
+#elif ICM40600_ACC_RANGE_G == 4
+#define ICM40600_ACC_FS 2
+#else
+#error "Invalid ICM40600_ACC_RANGE_G setting: valid values are 16, 8, 4 (no HiFi)"
+#endif
+// 0 -> +-2000dps, 1 -> +-1000dps, 2 -> +-500dps, 3 -> +-250dps, 4 -> +-125dps, 5 -> +-62.5dps, 6 -> +-31.25dps, 7 -> +-15.6225dps
+#if ICM40600_GYR_RANGE_DPS == 2000
+#define ICM40600_GYR_FS 0
+#elif ICM40600_GYR_RANGE_DPS == 1000
+#define ICM40600_GYR_FS 1
+#else
+#error "Invalid ICM40600_GYR_RANGE_DPS setting: valid values are 2000, 1000"
+#endif
+// Bandwidth for low-pass filters, using ODR/2 FIR
+#define ICM40600_ACC_BW_IND BIT_ACCEL_UI_BW_2_FIR
+#define ICM40600_GYR_BW_IND BIT_GYRO_UI_BW_2_FIR
+
+/* INT1 configuration */
+// Polarity: 0 -> Active Low, 1 -> Active High
+#define INT1_POLARITY 1
+// Drive circuit: 0 -> Open Drain, 1 -> Push-Pull, fixed for INT1 do not change!
+#define INT1_DRIVE_CIRCUIT 1
+// Mode: 0 -> Pulse, 1 -> Latch
+#define INT1_MODE 0
+
+/* Wake-on-Motion/No-Motion */
+#define ICM40600_WOM_THRESHOLD_MG 160 // 160mg @ 25Hz
+#define ICM40600_WOM_COMPUTE(val_mg) ((256 * val_mg) / 1000)
+// No-Motion duration: 5s
+#define ICM40600_NOM_DURATION_NS (5 * 1000000000ULL)
+
+/*
+ **********************
+ * Factory calibration
+ **********************
+ */
+#define CALIBRATION_ODR 7 // 200Hz (support both LPM and LNM)
+#define CALIBRATION_ACC_FS 0 // +-16g (= resolution of offset register)
+#define CALIBRATION_GYR_FS 1 // +-1000dps (= resolution of offset register)
+#define CALIBRATION_ACC_1G 2048 // LSB/g @+-16g
+#define CALIBRATION_ACC_BW_IND BIT_ACCEL_UI_BW_4_IIR
+#define CALIBRATION_GYR_BW_IND BIT_GYRO_UI_BW_4_IIR
+
+#define CALIBRATION_READ_INTERVAL_US (200 * 1000) // 200ms (200/5ms=40 packets -> 40 * 16 = 640 bytes)
+#define CALIBRATION_SAMPLE_NB 200 // 200/40packets = 5 loops
+#define RETRY_CNT_CALIBRATION 10 // > 5 loops
+
+/*
+ **********************
+ * Selftest
+ **********************
+ */
+#define SELF_TEST_ODR 6 // 1000Hz
+#define SELF_TEST_ACC_FS 3 // +-2g
+#define SELF_TEST_GYR_FS 3 // +-250dps
+#define SELF_TEST_ACC_BW_IND BIT_ACCEL_UI_BW_10_IIR
+#define SELF_TEST_GYR_BW_IND BIT_GYRO_UI_BW_10_IIR
+#define SELF_TEST_MIN_ACC_MG 225 // mg
+#define SELF_TEST_MAX_ACC_MG 675 // mg
+#define SELF_TEST_MIN_GYR_DPS 60 // dps
+#define SELF_TEST_MAX_GYR_OFF_DPS 20 // dps
+#define SELF_TEST_ACC_SHIFT_DELTA 500 // = 0.5
+#define SELF_TEST_GYR_SHIFT_DELTA 500 // = 0.5
+
+#define SELF_TEST_ACC_SCALE 32768 / (16 >> SELF_TEST_ACC_FS) / 1000
+#define SELF_TEST_GYR_SCALE 32768 / (2000 >> SELF_TEST_GYR_FS)
+
+#define SELF_TEST_READ_INTERVAL_US (20 * 1000) // 20ms (20/1ms=20 packets -> 20 * 16 = 320 bytes)
+#define SELF_TEST_SAMPLE_NB 200 // 200/20packets = 10 loops
+#define RETRY_CNT_SELF_TEST 20 // > 10 loops
+#define SELF_TEST_PRECISION 1000
+#define SELF_TEST_SETTLE_TIME_MS 25
+
+#define SELF_TEST_MIN_ACC (SELF_TEST_MIN_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION)
+#define SELF_TEST_MAX_ACC (SELF_TEST_MAX_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION)
+#define SELF_TEST_MIN_GYR (SELF_TEST_MIN_GYR_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)
+#define SELF_TEST_MAX_GYR (SELF_TEST_MAX_GYR_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)
+#define SELF_TEST_MAX_GYR_OFFSET (SELF_TEST_MAX_GYR_OFF_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)
+
+/*
+ ****************
+ * register map *
+ ****************
+ */
+/* Bank 0 */
+#define REG_DEVICE_CONFIG 0x11
+#define REG_SPI_SPEED 0x13
+#define REG_INT_CONFIG 0x14
+#define REG_FIFO_CONFIG 0x16
+#define REG_INT_STATUS 0x2D
+#define REG_FIFO_BYTE_COUNT1 0x2E
+#define REG_FIFO_BYTE_COUNT2 0x2F
+#define REG_FIFO_DATA 0x30
+#define REG_SIGNAL_PATH_RESET 0x4B
+#define REG_INTF_CONFIG0 0x4C
+#define REG_PWR_MGMT_0 0x4E
+#define REG_GYRO_CONFIG0 0x4F
+#define REG_ACCEL_CONFIG0 0x50
+#define REG_GYRO_CONFIG1 0x51
+#define REG_GYRO_ACCEL_CONFIG0 0x52
+#define REG_ACCEL_CONFIG1 0x53
+#define REG_ACCEL_WOM_X_THR 0x54
+#define REG_ACCEL_WOM_Y_THR 0x55
+#define REG_ACCEL_WOM_Z_THR 0x56
+#define REG_SMD_CONFIG 0x57
+#define REG_INT_STATUS2 0x59
+#define REG_TMST_CONFIG 0x5A
+#define REG_FIFO_CONFIG1 0x5F
+#define REG_FIFO_CONFIG2 0x60
+#define REG_FIFO_CONFIG3 0x61
+#define REG_INT_CONFIG0 0x63
+#define REG_INT_CONFIG1 0x64
+#define REG_INT_SOURCE0 0x65
+#define REG_INT_SOURCE1 0x66
+#define REG_SELF_TEST_CONFIG 0x70
+#define REG_WHO_AM_I 0x75
+#define REG_REG_BANK_SEL 0x76
+#define REG_GOS_USER0 0x77
+#define REG_GOS_USER1 0x78
+#define REG_GOS_USER2 0x79
+#define REG_GOS_USER3 0x7A
+#define REG_GOS_USER4 0x7B
+#define REG_GOS_USER5 0x7C
+#define REG_GOS_USER6 0x7D
+#define REG_GOS_USER7 0x7E
+#define REG_GOS_USER8 0x7F
+/* Bank 1 */
+#define REG_XG_ST_DATA 0x5F
+#define REG_YG_ST_DATA 0x60
+#define REG_ZG_ST_DATA 0x61
+/* Bank 2 */
+#define REG_XA_ST_DATA 0x3B
+#define REG_YA_ST_DATA 0x3C
+#define REG_ZA_ST_DATA 0x3D
+
+/* REG_WHO_AM_I */
+#define WHO_AM_I_ICM40604 0x32
+#define WHO_AM_I_ICM40605 0x33
+/* REG_REG_BANK_SEL */
+#define BIT_BANK_SEL_0 0x00
+#define BIT_BANK_SEL_1 0x01
+#define BIT_BANK_SEL_2 0x02
+/* REG_DEVICE_CONFIG */
+#define BIT_SOFT_RESET 0x01
+/* REG_SPI_SPEED */
+#define BIT_SPI_SPEED_5MHZ 0x03
+#define BIT_SPI_SPEED_17MHZ 0x05
+/* REG_GYRO_CONFIG0/REG_ACCEL_CONFIG0 */
+#define SHIFT_GYRO_FS_SEL 5
+#define SHIFT_ACCEL_FS_SEL 5
+#define SHIFT_ODR_CONF 0
+/* REG_INT_CONFIG */
+#define SHIFT_INT1_POLARITY 0
+#define SHIFT_INT1_DRIVE_CIRCUIT 1
+#define SHIFT_INT1_MODE 2
+/* REG_PWR_MGMT_0 */
+#define BIT_TEMP_DIS 0x20
+#define BIT_IDLE 0x10
+#define BIT_GYRO_MODE_OFF 0x00
+#define BIT_GYRO_MODE_STBY 0x04
+#define BIT_GYRO_MODE_LN 0x0C
+#define BIT_ACCEL_MODE_OFF 0x00
+#define BIT_ACCEL_MODE_LN 0x03
+/* REG_SIGNAL_PATH_RESET */
+#define BIT_FIFO_FLUSH 0x02
+/* REG_INTF_CONFIG0 */
+#define BIT_FIFO_COUNT_REC 0x40
+#define BIT_COUNT_BIG_ENDIAN 0x20
+#define BIT_SENS_DATA_BIG_ENDIAN 0x10
+#define BIT_UI_SIFS_DISABLE_SPI 0x02
+#define BIT_UI_SIFS_DISABLE_I2C 0x03
+/* REG_FIFO_CONFIG1 */
+#define BIT_FIFO_ACCEL_EN 0x01
+#define BIT_FIFO_GYRO_EN 0x02
+#define BIT_FIFO_TEMP_EN 0x04
+#define BIT_FIFO_TMST_FSYNC_EN 0x08
+#define BIT_FIFO_HIRES_EN 0x10
+#define BIT_FIFO_WM_TH 0x20
+#define BIT_FIFO_RESUME_PART_RD 0x40
+/* REG_INT_CONFIG1 */
+#define BIT_INT_ASY_RST_DISABLE 0x10
+/* REG_INT_SOURCE0 */
+#define BIT_INT_UI_AGC_RDY_INT1_EN 0x01
+#define BIT_INT_FIFO_FULL_INT1_EN 0x02
+#define BIT_INT_FIFO_THS_INT1_EN 0x04
+#define BIT_INT_UI_DRDY_INT1_EN 0x08
+#define BIT_INT_RESET_DONE_INT1_EN 0x10
+#define BIT_INT_PLL_RDY_INT1_EN 0x20
+#define BIT_INT_UI_FSYNC_INT1_EN 0x40
+/* REG_INT_SOURCE1 */
+#define BIT_INT_WOM_X_INT1_EN 0x01
+#define BIT_INT_WOM_Y_INT1_EN 0x02
+#define BIT_INT_WOM_Z_INT1_EN 0x04
+#define BIT_INT_SMD_INT1_EN 0x08
+#define BIT_INT_WOM_XYZ_INT1_EN (BIT_INT_WOM_X_INT1_EN | BIT_INT_WOM_Y_INT1_EN | BIT_INT_WOM_Z_INT1_EN)
+/* REG_SELF_TEST_CONFIG */
+#define BIT_SELF_TEST_REGULATOR_EN 0x40
+#define BIT_TEST_AZ_EN 0x20
+#define BIT_TEST_AY_EN 0x10
+#define BIT_TEST_AX_EN 0x08
+#define BIT_TEST_GZ_EN 0x04
+#define BIT_TEST_GY_EN 0x02
+#define BIT_TEST_GX_EN 0x01
+/* REG_INT_STATUS */
+#define BIT_INT_STATUS_AGC_RDY 0x01
+#define BIT_INT_STATUS_FIFO_FULL 0x02
+#define BIT_INT_STATUS_FIFO_THS 0x04
+#define BIT_INT_STATUS_DRDY 0x08
+#define BIT_INT_STATUS_RESET_DONE 0x10
+#define BIT_INT_STATUS_PLL_DRY 0x20
+#define BIT_INT_STATUS_UI_FSYNC 0x40
+/* REG_INT_STATUS2 */
+#define BIT_INT_STATUS_WOM_X 0x01
+#define BIT_INT_STATUS_WOM_Y 0x02
+#define BIT_INT_STATUS_WOM_Z 0x04
+#define BIT_INT_STATUS_SMD 0x08
+#define BIT_INT_STATUS_WOM_XYZ (BIT_INT_STATUS_WOM_X | BIT_INT_STATUS_WOM_Y | BIT_INT_STATUS_WOM_Z)
+/* REG_FIFO_CONFIG */
+#define BIT_FIFO_MODE_BYPASS 0x00
+#define BIT_FIFO_MODE_STREAM 0x40
+#define BIT_FIFO_MODE_STOP_FULL 0x80
+/* REG_GYRO_ACCEL_CONFIG0 */
+#define BIT_ACCEL_UI_BW_2_FIR 0x00
+#define BIT_ACCEL_UI_BW_4_IIR 0x10
+#define BIT_ACCEL_UI_BW_5_IIR 0x20
+#define BIT_ACCEL_UI_BW_8_IIR 0x30
+#define BIT_ACCEL_UI_BW_10_IIR 0x40
+#define BIT_ACCEL_UI_BW_16_IIR 0x50
+#define BIT_ACCEL_UI_BW_20_IIR 0x60
+#define BIT_ACCEL_UI_BW_40_IIR 0x70
+#define BIT_GYRO_UI_BW_2_FIR 0x00
+#define BIT_GYRO_UI_BW_4_IIR 0x01
+#define BIT_GYRO_UI_BW_5_IIR 0x02
+#define BIT_GYRO_UI_BW_8_IIR 0x03
+#define BIT_GYRO_UI_BW_10_IIR 0x04
+#define BIT_GYRO_UI_BW_16_IIR 0x05
+#define BIT_GYRO_UI_BW_20_IIR 0x06
+#define BIT_GYRO_UI_BW_40_IIR 0x07
+/* fifo data packet header */
+#define BIT_FIFO_HEAD_MSG 0x80
+#define BIT_FIFO_HEAD_ACCEL 0x40
+#define BIT_FIFO_HEAD_GYRO 0x20
+#define BIT_FIFO_HEAD_TMSP_ODR 0x08
+#define BIT_FIFO_HEAD_TMSP_NO_ODR 0x04
+#define BIT_FIFO_HEAD_TMSP_FSYNC 0x0C
+#define BIT_FIFO_HEAD_ODR_ACCEL 0x02
+#define BIT_FIFO_HEAD_ODR_GYRO 0x01
+/* REG_SMD_CONFIG */
+#define BIT_WOM_INT_MODE_OR 0x00
+#define BIT_WOM_INT_MODE_AND 0x08
+#define BIT_WOM_MODE_INITIAL 0x00
+#define BIT_WOM_MODE_PREV 0x04
+#define BIT_SMD_MODE_OFF 0x00
+#define BIT_SMD_MODE_OLD 0x01
+#define BIT_SMD_MODE_SHORT 0x02
+#define BIT_SMD_MODE_LONG 0x03
+/* REG_TMST_CONFIG */
+#define BIT_EN_DREG_FIFO_D2A 0x20
+#define BIT_TMST_TO_REGS_EN 0x10
+#define BIT_TMST_RESOL 0x08
+#define BIT_TMST_DELTA_EN 0x04
+#define BIT_TMST_FSYNC_EN 0x02
+#define BIT_TMST_EN 0x01
+
+/* FIFO data definitions */
+#define FIFO_PACKET_SIZE 16
+#define FIFO_MAX_PACKET_NB 65
+#define FIFO_MIN_PACKET_NB 62
+
+/* sensor startup time */
+#define ICM40600_GYRO_START_TIME_MS 40
+#define ICM40600_ACCEL_START_TIME_MS 10
+
+/* temperature sensor */
+#define TEMP_SCALE (128.0f / 115.49f) // scale, #9447
+#define TEMP_OFFSET 25 // 25 degC offset
+
+#define SPI_WRITE_0(addr, data) spiQueueWrite(addr, data, 2)
+#define SPI_WRITE_1(addr, data, delay) spiQueueWrite(addr, data, delay)
+#define GET_SPI_WRITE_MACRO(_1,_2,_3,NAME,...) NAME
+#define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__)
+
+#define SPI_READ_0(addr, size, buf) spiQueueRead(addr, size, buf, 0)
+#define SPI_READ_1(addr, size, buf, delay) spiQueueRead(addr, size, buf, delay)
+#define GET_SPI_READ_MACRO(_1,_2,_3,_4,NAME,...) NAME
+#define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__)
+
+#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG)
+#define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION)
+#define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION)
+
+#define MAX_NUM_COMMS_EVENT_SAMPLES 15
+
+#define SPI_PACKET_SIZE 30
+#define FIFO_READ_SIZE (FIFO_MAX_PACKET_NB * FIFO_PACKET_SIZE)
+#define BUF_MARGIN 32 // some extra buffer for additional reg RW when a FIFO read happens
+#define SPI_BUF_SIZE (FIFO_READ_SIZE + BUF_MARGIN)
+
+#define ACC_FS_RANGE (16 >> ICM40600_ACC_FS)
+#define GYR_FS_RANGE (2000 >> ICM40600_GYR_FS)
+#define GRAVITY_NORM 9.80665f
+#define kScale_acc (GRAVITY_NORM * ACC_FS_RANGE / 32768.0f)
+#define kScale_gyr (NANO_PI / 180.0f * GYR_FS_RANGE / 32768.0f)
+
+#define RATE_TO_HZ SENSOR_HZ(1.0f)
+#define DECIMATION_HIGH_RATE SENSOR_HZ(1000.0f)
+#define DECIMATION_LOW_RATE SENSOR_HZ(25.0f)
+#define NO_DECIMATION_MAX_RATE SENSOR_HZ(200.0f) // will set ODR to DECIMATION_HIGH_RATE if more than this rate
+#define NO_DECIMATION_MIN_RATE SENSOR_HZ(25.0f) // will set ODR to DECIMATION_LOW_RATE if less than this rate
+#define MAX_BATCH_SIZE (((FIFO_MIN_PACKET_NB * 90) / 100) * FIFO_PACKET_SIZE) // 90% of FIFO size
+
+/* skip first some samples */
+#define ACC_SKIP_SAMPLE_NB 0
+#define GYR_SKIP_SAMPLE_NB 3
+
+#define CHIP_TIME_RES_US 1
+#define CHIP_TIME_OFFSET_US 64000000ULL // 64sec
+#define MIN_INCREMENT_TIME_NS 1000000ULL // 1ms for 1000Hz
+
+enum SensorIndex {
+ FIRST_CONT_SENSOR = 0,
+ ACC = FIRST_CONT_SENSOR,
+ GYR,
+ NUM_CONT_SENSOR,
+ FIRST_ONESHOT_SENSOR = NUM_CONT_SENSOR,
+ WOM = FIRST_ONESHOT_SENSOR,
+ NOMO,
+ NUM_OF_SENSOR,
+};
+
+enum SensorEvents {
+ NO_EVT = -1,
+ EVT_SPI_DONE = EVT_APP_START + 1,
+ EVT_SENSOR_INTERRUPT_1,
+};
+
+enum IntState {
+ INT_READ_STATUS,
+ INT_PROCESS_STATUS,
+ INT_READ_FIFO_DATA,
+ INT_DONE,
+};
+
+enum InitState {
+ RESET_ICM40600,
+ INIT_ICM40600,
+ INIT_DONE,
+};
+
+enum CalibrationState {
+ CALIBRATION_START,
+ CALIBRATION_READ_STATUS,
+ CALIBRATION_READ_DATA,
+ CALIBRATION_SET_OFFSET,
+ CALIBRATION_DONE
+};
+
+enum SelfTestState {
+ TEST_START,
+ TEST_READ_STATUS,
+ TEST_READ_DATA,
+ TEST_READ_OTP,
+ TEST_REPORT,
+ TEST_DONE
+};
+
+enum SensorState {
+ // keep this in sync with getStateName
+ SENSOR_BOOT,
+ SENSOR_VERIFY_ID,
+ SENSOR_INITIALIZING,
+ SENSOR_IDLE,
+ SENSOR_POWERING_UP,
+ SENSOR_POWERING_DOWN,
+ SENSOR_CONFIG_CHANGING,
+ SENSOR_INT_1_HANDLING,
+ SENSOR_CALIBRATING,
+ SENSOR_TESTING,
+ SENSOR_SAVE_CALIBRATION,
+ SENSOR_NUM_OF_STATE
+};
+#if DBG_STATE
+#define PRI_STATE "s"
+static const char * getStateName(int32_t s) {
+ // keep this in sync with SensorState
+ static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP",
+ "PWR_DN", "CFG_CHANGE", "INT1", "CALIB", "TEST", "SAVE_CALIB"};
+ if (s >= 0 && s < SENSOR_NUM_OF_STATE) {
+ return l[s];
+ }
+ return "???";
+#else
+#define PRI_STATE PRIi32
+static int32_t getStateName(int32_t s) {
+ return s;
+#endif
+}
+
+#if DBG_TIMESTAMP
+struct StatisticsSet {
+ uint32_t sync_reset_count;
+ uint32_t sync_count;
+ uint32_t sync_adjust_plus;
+ uint32_t sync_adjust_minus;
+ uint32_t sync_truncate;
+};
+#endif
+
+struct ConfigStat {
+ uint64_t latency;
+ uint32_t rate;
+ bool enable;
+};
+
+struct CalibrationData {
+ struct HostHubRawPacket header;
+ struct SensorAppEventHeader data_header;
+ int32_t xBias;
+ int32_t yBias;
+ int32_t zBias;
+} __attribute__((packed));
+
+struct TestResultData {
+ struct HostHubRawPacket header;
+ struct SensorAppEventHeader data_header;
+} __attribute__((packed));
+
+struct ICM40600Sensor {
+ struct ConfigStat pConfig; // pending config status request
+ struct TripleAxisDataEvent *data_evt;
+ uint32_t handle;
+ uint32_t rate;
+ uint64_t latency;
+ uint64_t prev_rtc_time;
+ int16_t offset[3];
+ int16_t data[3];
+ bool updated;
+ bool powered; // activate status
+ bool configed; // configure status
+ bool wait_for_odr;
+ enum SensorIndex idx;
+ uint8_t flush;
+ uint8_t decimator;
+ uint8_t data_cnt;
+ uint8_t skip_sample_cnt;
+};
+
+struct FifoPacketData {
+ int16_t accel[3];
+ int16_t gyro[3];
+ uint16_t timestamp;
+ bool odr_accel;
+ bool odr_gyro;
+ bool valid_accel;
+ bool valid_gyro;
+ int8_t temp;
+};
+
+struct ICM40600FactoryCal {
+ int32_t data[3];
+ int32_t data_count;
+};
+
+struct ICM40600SelfTest {
+ int32_t data[3];
+ int32_t data_st_on[3];
+ int32_t data_st_off[3];
+ int32_t data_count;
+ bool st_mode;
+ uint8_t otp_st_data_gyro[3];
+ uint8_t otp_st_data_accel[3];
+};
+
+struct ICM40600Config {
+ uint32_t accel_rate;
+ uint32_t gyro_rate;
+ uint32_t wom_threshold;
+ uint32_t fifo_rate;
+ uint16_t fifo_watermark;
+ uint8_t fifo_sample_size;
+ bool accel_on;
+ bool gyro_on;
+ bool wom_on;
+};
+
+struct ICM40600Task {
+ uint32_t tid;
+ struct ICM40600Sensor sensors[NUM_OF_SENSOR];
+
+ // spi and interrupt
+ spi_cs_t cs;
+ struct SpiMode mode;
+ struct SpiPacket packets[SPI_PACKET_SIZE];
+ struct SpiDevice *spiDev;
+ struct Gpio *Int1;
+ IRQn_Type Irq1;
+ struct ChainedIsr Isr1;
+ bool Int1_EN;
+
+ // spi buffers
+ uint8_t *dataBuffer[3];
+ uint8_t txrxBuffer[SPI_BUF_SIZE];
+
+ // states
+ volatile uint8_t state; //task state, type enum SensorState, do NOT change this directly
+ enum InitState init_state;
+ enum IntState int_state;
+ enum CalibrationState calibration_state;
+ enum SelfTestState selftest_state;
+
+ // pending config
+ bool pending_int[1];
+ bool pending_flush;
+ bool pending_config[NUM_OF_SENSOR];
+ bool pending_calibration_save;
+
+ struct ICM40600Config config;
+ uint32_t noMotionTimerHandle;
+ uint16_t fifo_count;
+ bool powered;
+ bool flush;
+
+ // calibration
+#ifdef ACCEL_CAL_ENABLED
+ struct AccelCal accel_cal;
+#endif
+#ifdef GYRO_CAL_ENABLED
+ struct GyroCal gyro_cal;
+#endif
+
+ // timestamping
+ time_sync_t gSensorTime2RTC;
+ uint64_t data_timestamp;
+ uint64_t chip_time_us;
+ uint64_t last_sync_time;
+ uint64_t last_sync_data_ts;
+ uint16_t chip_timestamp;
+ bool fifo_start_sync;
+
+ // temperature data from chip in degC
+ float chip_temperature;
+
+ // spi rw
+ struct SlabAllocator *mDataSlab;
+ uint16_t mWbufCnt;
+ uint8_t mRegCnt;
+ uint8_t mRetryLeft;
+ bool spiInUse;
+
+ struct ICM40600FactoryCal factory_cal;
+
+ struct ICM40600SelfTest self_test;
+
+#if DBG_TIMESTAMP
+ struct StatisticsSet statistics_set;
+#endif
+};
+
+static uint32_t AccRates[] = {
+ SENSOR_HZ(25.0f/8.0f),
+ SENSOR_HZ(25.0f/4.0f),
+ SENSOR_HZ(25.0f/2.0f),
+ SENSOR_HZ(25.0f),
+ SENSOR_HZ(50.0f),
+ SENSOR_HZ(100.0f),
+ SENSOR_HZ(200.0f),
+ SENSOR_HZ(500.0f),
+ 0,
+};
+
+static uint32_t GyrRates[] = {
+ SENSOR_HZ(25.0f/8.0f),
+ SENSOR_HZ(25.0f/4.0f),
+ SENSOR_HZ(25.0f/2.0f),
+ SENSOR_HZ(25.0f),
+ SENSOR_HZ(50.0f),
+ SENSOR_HZ(100.0f),
+ SENSOR_HZ(200.0f),
+ SENSOR_HZ(500.0f),
+ 0,
+};
+
+static struct ICM40600Task mTask;
+
+#define DEC_INFO(name, type, axis, inter, samples) \
+ .sensorName = name, \
+ .sensorType = type, \
+ .numAxis = axis, \
+ .interrupt = inter, \
+ .minSamples = samples
+
+#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
+ DEC_INFO(name, type, axis, inter, samples), \
+ .supportedRates = rates
+
+#define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \
+ DEC_INFO(name, type, axis, inter, samples), \
+ .supportedRates = rates, \
+ .flags1 = SENSOR_INFO_FLAGS1_RAW, \
+ .rawType = raw, \
+ .rawScale = scale
+
+#define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \
+ DEC_INFO(name, type, axis, inter, samples), \
+ .supportedRates = rates, \
+ .flags1 = SENSOR_INFO_FLAGS1_BIAS, \
+ .biasType = bias
+
+#define DEC_INFO_RATE_RAW_BIAS(name, rates, type, axis, inter, samples, raw, scale, bias) \
+ DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale), \
+ .flags1 = SENSOR_INFO_FLAGS1_RAW | SENSOR_INFO_FLAGS1_BIAS, \
+ .biasType = bias
+
+typedef struct ICM40600Task _Task;
+#define TASK _Task* const _task
+
+// To get rid of static variables all task functions should have a task structure pointer input.
+// This is an intermediate step.
+#define TDECL() TASK = &mTask; (void)_task
+
+// Access task variables without explicitly specify the task structure pointer.
+#define T(v) (_task->v)
+
+// Atomic get state
+#define GET_STATE() (atomicReadByte(&(_task->state)))
+
+// Atomic set state, this set the state to arbitrary value, use with caution
+#define SET_STATE(s) do{\
+ DEBUG_PRINT_IF(DBG_STATE, "set state %" PRI_STATE "\n", getStateName(s));\
+ atomicWriteByte(&(_task->state), (s));\
+ }while(0)
+
+// Atomic switch state from IDLE to desired state.
+static bool trySwitchState_(TASK, enum SensorState newState) {
+#if DBG_STATE
+ bool ret = atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
+ uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE();
+ DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n",
+ getStateName(prevState), getStateName(newState), ret ? "ok" : "failed");
+ return ret;
+#else
+ return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
+#endif
+}
+// Short-hand
+#define trySwitchState(s) trySwitchState_(_task, (s))
+
+static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] =
+{
+#ifdef ACCEL_CAL_ENABLED
+ { DEC_INFO_RATE_RAW_BIAS("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0 / kScale_acc, SENS_TYPE_ACCEL_BIAS) },
+#else
+ { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0 / kScale_acc) },
+#endif
+#ifdef GYRO_CAL_ENABLED
+ { DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
+#else
+ { DEC_INFO_RATE("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
+ NANOHUB_INT_NONWAKEUP, 20) },
+#endif
+ { DEC_INFO("Wake-on-Motion", SENS_TYPE_ANY_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
+ { DEC_INFO("No-Motion", SENS_TYPE_NO_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
+};
+
+static void time_init(TASK)
+{
+ time_sync_init(&T(gSensorTime2RTC));
+}
+
+static bool sensortime_to_rtc_time(TASK, uint64_t sensor_time, uint64_t *rtc_time_ns)
+{
+ return time_sync_estimate_time1(&T(gSensorTime2RTC), sensor_time, rtc_time_ns);
+}
+
+static void map_sensortime_to_rtc_time(TASK, uint64_t sensor_time, uint64_t rtc_time_ns)
+{
+#if DBG_TIMESTAMP
+ T(statistics_set).sync_count++;
+#endif
+ time_sync_add(&T(gSensorTime2RTC), rtc_time_ns, sensor_time);
+}
+
+static void invalidate_sensortime_to_rtc_time(TASK)
+{
+#if DBG_TIMESTAMP
+ T(statistics_set).sync_reset_count++;
+#endif
+ time_sync_reset(&T(gSensorTime2RTC));
+}
+
+static void dataEvtFree(void *ptr)
+{
+ TDECL();
+ struct TripleAxisDataEvent *ev = (struct TripleAxisDataEvent *)ptr;
+
+ slabAllocatorFree(T(mDataSlab), ev);
+}
+
+static void spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
+{
+ TDECL();
+
+ if (T(spiInUse)) {
+ ERROR_PRINT("SPI in use, cannot queue write\n");
+ return;
+ }
+ T(packets[T(mRegCnt)]).size = 2;
+ T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).rxBuf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).delay = delay * 1000;
+ T(txrxBuffer[T(mWbufCnt++)]) = ICM40600_SPI_WRITE | addr;
+ T(txrxBuffer[T(mWbufCnt++)]) = data;
+ T(mRegCnt)++;
+}
+
+/*
+ * need to be sure size of buf is larger than read size
+ */
+static void spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
+{
+ TDECL();
+
+ if (T(spiInUse)) {
+ ERROR_PRINT("SPI in use, cannot queue read %d %d\n", (int)addr, (int)size);
+ return;
+ }
+
+ *buf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).size = size + 1; // first byte will not contain valid data
+ T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
+ T(packets[T(mRegCnt)]).rxBuf = *buf;
+ T(packets[T(mRegCnt)]).delay = delay * 1000;
+ T(txrxBuffer[T(mWbufCnt)++]) = ICM40600_SPI_READ | addr;
+ T(mWbufCnt) += size;
+ T(mRegCnt)++;
+}
+
+static void spiBatchTxRx(struct SpiMode *mode,
+ SpiCbkF callback, void *cookie, const char * src)
+{
+ TDECL();
+
+ if (T(mRegCnt) == 0) {
+ callback(cookie, 0);
+ return;
+ }
+
+ if (T(mWbufCnt) > SPI_BUF_SIZE) {
+ ERROR_PRINT("NO enough SPI buffer space, dropping transaction.\n");
+ return;
+ }
+ if (T(mRegCnt) > SPI_PACKET_SIZE) {
+ ERROR_PRINT("spiBatchTxRx too many packets!\n");
+ return;
+ }
+
+ T(spiInUse) = true;
+ T(mWbufCnt) = 0;
+
+ // Reset variables before issuing SPI transaction.
+ // SPI may finish before spiMasterRxTx finish
+ uint8_t regCount = T(mRegCnt);
+ T(mRegCnt) = 0;
+
+ if (spiMasterRxTx(T(spiDev), T(cs), T(packets), regCount, mode, callback, cookie) < 0) {
+ ERROR_PRINT("spiMasterRxTx failed!\n");
+ }
+}
+
+static bool icm40600Isr1(struct ChainedIsr *isr)
+{
+ TASK = container_of(isr, struct ICM40600Task, Isr1);
+
+ if (!extiIsPendingGpio(T(Int1))) {
+ return false;
+ }
+
+ /* better to use atomic read for the pending flag but not serious even not atomic */
+ if (!T(pending_int[0])) {
+ osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid));
+ }
+ extiClearPendingGpio(T(Int1));
+ return true;
+}
+
+static void sensorSpiCallback(void *cookie, int err)
+{
+ TDECL();
+
+ T(spiInUse) = false;
+
+ if (!osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, T(tid)))
+ ERROR_PRINT("sensorSpiCallback: osEnqueuePrivateEvt() failed\n");
+}
+
+static void noMotionCallback(uint32_t timerId, void *data)
+{
+ const struct ICM40600Sensor *sensor = (const struct ICM40600Sensor *)data;
+
+ if (!sensor->configed)
+ return;
+
+ osEnqueueEvt(EVT_SENSOR_NO_MOTION, NULL, NULL);
+}
+
+static void setOffsetReg(TASK)
+{
+ uint8_t val;
+ const int16_t * const acc_offset = T(sensors[ACC]).offset;
+ const int16_t * const gyr_offset = T(sensors[GYR]).offset;
+
+ DEBUG_PRINT("Set ACC hw offset %d %d %d\n",
+ -acc_offset[0], -acc_offset[1], -acc_offset[2]);
+ DEBUG_PRINT("Set GYR hw offset %d %d %d\n",
+ -gyr_offset[0], -gyr_offset[1], -gyr_offset[2]);
+
+ // GX_L
+ val = (-gyr_offset[0]) & 0xff;
+ SPI_WRITE(REG_GOS_USER0, val);
+ // GY_H / GX_H
+ val = (-gyr_offset[1] >> 4) & 0xf0;
+ val |= ((-gyr_offset[0] >> 8) & 0x0f);
+ SPI_WRITE(REG_GOS_USER1, val);
+ // GY_L
+ val = (-gyr_offset[1]) & 0xff;
+ SPI_WRITE(REG_GOS_USER2, val);
+ // GZ_L
+ val = (-gyr_offset[2]) & 0xff;
+ SPI_WRITE(REG_GOS_USER3, val);
+ // AX_H / GZ_H
+ val = (-acc_offset[0] >> 4) & 0xf0;
+ val |= ((-gyr_offset[2] >> 8) & 0x0f);
+ SPI_WRITE(REG_GOS_USER4, val);
+ // AX_H / GZ_H
+ val = (-acc_offset[0] >> 4) & 0xf0;
+ val |= ((-gyr_offset[2] >> 8) & 0x0f);
+ SPI_WRITE(REG_GOS_USER4, val);
+ // AX_L
+ val = (-acc_offset[0]) & 0xff;
+ SPI_WRITE(REG_GOS_USER5, val);
+ // AY_L
+ val = (-acc_offset[1]) & 0xff;
+ SPI_WRITE(REG_GOS_USER6, val);
+ // AZ_H / AY_H
+ val = (-acc_offset[2] >> 4) & 0xf0;
+ val |= ((-acc_offset[1] >> 8) & 0x0f);
+ SPI_WRITE(REG_GOS_USER7, val);
+ // AZ_L
+ val = (-acc_offset[2]) & 0xff;
+ SPI_WRITE(REG_GOS_USER8, val);
+}
+
+static void resetOffsetReg(TASK, enum SensorIndex idx)
+{
+ uint8_t val;
+
+ DEBUG_PRINT("Reset offset registers sensor=%d\n", idx);
+
+ if (idx == ACC) {
+ val = (-T(sensors[GYR]).offset[2] >> 8) & 0x0f;
+ SPI_WRITE(REG_GOS_USER4, val); // AX_H / GZ_H
+ SPI_WRITE(REG_GOS_USER5, 0x00); // AX_L
+ SPI_WRITE(REG_GOS_USER6, 0x00); // AY_L
+ SPI_WRITE(REG_GOS_USER7, 0x00); // AZ_H / AY_H
+ SPI_WRITE(REG_GOS_USER8, 0x00); // AZ_L
+ } else if (idx == GYR) {
+ SPI_WRITE(REG_GOS_USER0, 0x00); // GX_L
+ SPI_WRITE(REG_GOS_USER1, 0x00); // GY_H / GX_H
+ SPI_WRITE(REG_GOS_USER2, 0x00); // GY_L
+ SPI_WRITE(REG_GOS_USER3, 0x00); // GZ_L
+ val = (-T(sensors[ACC]).offset[0] >> 4) & 0xf0;
+ SPI_WRITE(REG_GOS_USER4, val); // AX_H / GZ_H
+ }
+}
+
+static bool saveCalibration(TASK)
+{
+ if (trySwitchState(SENSOR_SAVE_CALIBRATION)) {
+ setOffsetReg(_task);
+ spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
+ return true;
+ } else {
+ return false;
+ }
+}
+
+static bool accCfgData(void *data, void *cookie)
+{
+ TDECL();
+ struct ICM40600Sensor *sensor = &T(sensors[ACC]);
+ struct CfgData {
+ int32_t hw[3]; // chip frame (hw unit @FSR=factory calibration)
+ float sw[3]; // body frame
+ };
+ struct CfgData *values = data;
+ int i;
+
+ for (i = 0; i < 3; i++) {
+ // offset register is 12bit
+ if (values->hw[i] > 2047) {
+ sensor->offset[i] = 2047;
+ } else if (values->hw[i] < -2048) {
+ sensor->offset[i] = -2048;
+ } else {
+ sensor->offset[i] = values->hw[i];
+ }
+ }
+#ifdef ACCEL_CAL_ENABLED
+ accelCalBiasSet(&T(accel_cal), values->sw[0], values->sw[1], values->sw[2]);
+#endif
+ if (!saveCalibration(_task)) {
+ T(pending_calibration_save) = true;
+ }
+ INFO_PRINT("accCfgData hw bias: data=%d, %d, %d\n",
+ sensor->offset[0], sensor->offset[1], sensor->offset[2]);
+
+ return true;
+}
+
+static bool gyrCfgData(void *data, void *cookie)
+{
+ TDECL();
+ struct ICM40600Sensor *sensor = &T(sensors[GYR]);
+ const struct AppToSensorHalDataPayload *p = data;
+ int i;
+
+ if (p->type == HALINTF_TYPE_GYRO_CAL_BIAS && p->size == sizeof(struct GyroCalBias)) {
+ const struct GyroCalBias *bias = p->gyroCalBias;
+ for (i = 0; i < 3; i++) {
+ // offset register is 12bit
+ if (bias->hardwareBias[i] > 2047) {
+ sensor->offset[i] = 2047;
+ } else if (bias->hardwareBias[i] < -2048) {
+ sensor->offset[i] = -2048;
+ } else {
+ sensor->offset[i] = bias->hardwareBias[i];
+ }
+ }
+#ifdef GYRO_CAL_ENABLED
+ gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1],
+ bias->softwareBias[2], sensorGetTime());
+#endif
+ if (!saveCalibration(_task)) {
+ T(pending_calibration_save) = true;
+ }
+ INFO_PRINT("gyrCfgData hw bias: data=%d, %d, %d\n",
+ sensor->offset[0], sensor->offset[1], sensor->offset[2]);
+ } else if (p->type == HALINTF_TYPE_GYRO_OTC_DATA && p->size == sizeof(struct GyroOtcData)) {
+ // Over-temperature gyro calibration not supported
+ } else {
+ ERROR_PRINT("Unknown gyro config data type 0x%04x, size %d\n", p->type, p->size);
+ }
+
+ return true;
+}
+
+static bool validateFifoData(const int16_t data[3])
+{
+ bool ret = true;
+
+ if ((data[0] == -32768) && (data[1] == -32768) && (data[2] == -32768))
+ ret = false;
+
+ return ret;
+}
+
+static void getFifoData(const uint8_t *data, int idx, struct FifoPacketData *out)
+{
+ /* invalidate all flags */
+ out->valid_accel = false;
+ out->valid_gyro = false;
+
+ /* Header : 1 byte
+ * Accel : 6 byte
+ * Gyro : 6 byte
+ * Temp : 1 byte
+ * Timestamp : 2 byte */
+ if (data[idx] & BIT_FIFO_HEAD_ACCEL) {
+ out->accel[0] = (data[idx + 2] << 8) | data[idx + 1];
+ out->accel[1] = (data[idx + 4] << 8) | data[idx + 3];
+ out->accel[2] = (data[idx + 6] << 8) | data[idx + 5];
+ out->valid_accel = validateFifoData(out->accel);
+ }
+ out->odr_accel = (data[idx] & BIT_FIFO_HEAD_ODR_ACCEL) ? true : false;
+
+ if (data[idx] & BIT_FIFO_HEAD_GYRO) {
+ out->gyro[0] = (data[idx + 8] << 8) | data[idx + 7];
+ out->gyro[1] = (data[idx + 10] << 8) | data[idx + 9];
+ out->gyro[2] = (data[idx + 12] << 8) | data[idx + 11];
+ out->valid_gyro = validateFifoData(out->gyro);
+ }
+ out->odr_gyro = (data[idx] & BIT_FIFO_HEAD_ODR_GYRO) ? true : false;
+
+ out->temp = (int8_t)data[idx + 13];
+ out->timestamp = (data[idx + 15] << 8) | data[idx + 14];
+}
+
+static bool calSelftestGetOneData(enum SensorIndex sensor_idx, const uint8_t *data, int idx, int16_t raw_data[3])
+{
+ struct FifoPacketData fifo_data;
+ bool ret = false;
+
+ getFifoData(data, idx, &fifo_data);
+
+ switch (sensor_idx) {
+ case ACC:
+ if (fifo_data.valid_accel) {
+ raw_data[0] = fifo_data.accel[0];
+ raw_data[1] = fifo_data.accel[1];
+ raw_data[2] = fifo_data.accel[2];
+ ret = true;
+ }
+ break;
+ case GYR:
+ if (fifo_data.valid_gyro) {
+ raw_data[0] = fifo_data.gyro[0];
+ raw_data[1] = fifo_data.gyro[1];
+ raw_data[2] = fifo_data.gyro[2];
+ ret = true;
+ }
+ break;
+ default:
+ break;
+ }
+
+ return ret;
+}
+
+static void calSelftestFifoEnable(TASK, enum SensorIndex idx, bool en, uint32_t delay)
+{
+ uint8_t val;
+
+ if (en) {
+ // enable FIFO output
+ T(config).fifo_sample_size = FIFO_PACKET_SIZE;
+ val = BIT_FIFO_ACCEL_EN | BIT_FIFO_GYRO_EN |
+ BIT_FIFO_TEMP_EN | BIT_FIFO_TMST_FSYNC_EN;
+ SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_STREAM);
+ SPI_WRITE(REG_FIFO_CONFIG1, val, delay); // with wait
+ } else {
+ // disable FIFO output
+ T(config).fifo_sample_size = 0;
+ SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_BYPASS);
+ SPI_WRITE(REG_FIFO_CONFIG1, 0);
+ }
+}
+
+/*
+ * Factory calibration
+ */
+static void sendCalibrationResult(uint8_t status, uint8_t sensorType,
+ int32_t xBias, int32_t yBias, int32_t zBias)
+{
+ struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
+ if (!data) {
+ ERROR_PRINT("Couldn't alloc cal result pkt");
+ return;
+ }
+
+ data->header.appId = ICM40600_APP_ID;
+ data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket));
+ data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT;
+ data->data_header.sensorType = sensorType;
+ data->data_header.status = status;
+
+ data->xBias = xBias;
+ data->yBias = yBias;
+ data->zBias = zBias;
+
+ if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
+ ERROR_PRINT("Couldn't send cal result evt");
+}
+
+static void calibrationInit(TASK, enum SensorIndex idx)
+{
+ uint8_t val;
+
+ // Disable Interrupt
+ SPI_WRITE(REG_INT_SOURCE0, 0);
+ SPI_WRITE(REG_INT_SOURCE1, 0);
+
+ // reset offset registers
+ resetOffsetReg(_task, idx);
+
+ // stop FIFO
+ calSelftestFifoEnable(_task, idx, false, 0);
+
+ // set rate
+ SPI_WRITE(REG_GYRO_CONFIG0, (CALIBRATION_GYR_FS << SHIFT_GYRO_FS_SEL) |
+ (CALIBRATION_ODR << SHIFT_ODR_CONF));
+ SPI_WRITE(REG_ACCEL_CONFIG0, (CALIBRATION_ACC_FS << SHIFT_ACCEL_FS_SEL) |
+ (CALIBRATION_ODR << SHIFT_ODR_CONF));
+
+ // set filter
+ SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, CALIBRATION_ACC_BW_IND | CALIBRATION_GYR_BW_IND);
+
+ // turn on sensors
+ switch (idx) {
+ case ACC:
+ val = BIT_ACCEL_MODE_LN;
+ break;
+ case GYR:
+ val = BIT_GYRO_MODE_LN;
+ break;
+ default:
+ val = 0;
+ break;
+ }
+ SPI_WRITE(REG_PWR_MGMT_0, val, 200 * 1000);
+
+ calSelftestFifoEnable(_task, idx, true, CALIBRATION_READ_INTERVAL_US);
+}
+
+static void calibrationDeinit(TASK, enum SensorIndex idx)
+{
+ calSelftestFifoEnable(_task, idx, false, 0);
+
+ SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND);
+ SPI_WRITE(REG_PWR_MGMT_0, 0, 200); // 9136
+
+ // make register accesses happen when sensor is enabled next time
+ T(config).gyro_rate = 0;
+ T(config).accel_rate = 0;
+ T(config).fifo_rate = 0;
+ T(config).fifo_watermark = 0;
+ T(config).accel_on = false;
+ T(config).gyro_on = false;
+ T(config).wom_on = false;
+}
+
+static void calibrationHandling(TASK, enum SensorIndex idx)
+{
+ const uint8_t *buf, *data;
+ uint8_t int_status;
+ uint16_t fifo_count;
+ int i, t;
+ bool r;
+ int16_t raw_data[3] = { 0, 0, 0 };
+
+ if (idx != ACC && idx != GYR) {
+ ERROR_PRINT("Invalid sensor index\n");
+ return;
+ }
+
+ switch (T(calibration_state)) {
+ case CALIBRATION_START:
+ T(mRetryLeft) = RETRY_CNT_CALIBRATION;
+ calibrationInit(_task, idx);
+ for (i = 0; i < 3; i++) {
+ T(factory_cal).data[i] = 0;
+ }
+ SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
+ SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
+ T(calibration_state) = CALIBRATION_READ_STATUS;
+ T(factory_cal).data_count = CALIBRATION_SAMPLE_NB;
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ case CALIBRATION_READ_STATUS:
+ buf = T(dataBuffer[0]);
+ int_status = buf[1];
+ buf = T(dataBuffer[1]);
+ fifo_count = buf[2] << 8 | buf[1];
+ fifo_count = fifo_count - fifo_count % T(config).fifo_sample_size;
+ T(fifo_count) = fifo_count;
+ DEBUG_PRINT("fifo_count = %d\n", fifo_count);
+ if (int_status & BIT_INT_STATUS_FIFO_FULL) {
+ ERROR_PRINT("fifo overflow\n");
+ calibrationDeinit(_task, idx);
+ T(calibration_state) = CALIBRATION_DONE;
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0);
+ } else {
+ T(calibration_state) = CALIBRATION_READ_DATA;
+ SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0]));
+ }
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ case CALIBRATION_READ_DATA:
+ buf = T(dataBuffer[0]);
+ data = &buf[1];
+ for (i = 0; i < T(fifo_count); i += T(config).fifo_sample_size) {
+ r = calSelftestGetOneData(idx, data, i, raw_data);
+ if (r == false) {
+ ERROR_PRINT("invalid data packet\n");
+ calibrationDeinit(_task, idx);
+ T(calibration_state) = CALIBRATION_DONE;
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0);
+ break;
+ }
+ for (t = 0; t < 3; t++) {
+ T(factory_cal).data[t] += raw_data[t];
+ }
+ T(factory_cal).data_count--;
+ if (T(factory_cal).data_count == 0) {
+ break;
+ }
+ }
+ if (T(factory_cal).data_count > 0) {
+ if (--T(mRetryLeft) == 0) {
+ ERROR_PRINT("calibration timeout\n");
+ calibrationDeinit(_task, idx);
+ T(calibration_state) = CALIBRATION_DONE;
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0);
+ } else {
+ calSelftestFifoEnable(_task, idx, false, 0); // toggle FIFO to reset
+ calSelftestFifoEnable(_task, idx, true, CALIBRATION_READ_INTERVAL_US);
+ SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
+ SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
+ T(calibration_state) = CALIBRATION_READ_STATUS;
+ }
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ }
+ T(calibration_state) = CALIBRATION_SET_OFFSET;
+ // fall-through
+ case CALIBRATION_SET_OFFSET:
+ DEBUG_PRINT("calibration total: %ld, %ld, %ld, data count=%d\n",
+ T(factory_cal).data[0],
+ T(factory_cal).data[1],
+ T(factory_cal).data[2],
+ CALIBRATION_SAMPLE_NB);
+ for (i = 0; i < 3; i++) {
+ T(factory_cal).data[i] /= CALIBRATION_SAMPLE_NB;
+ }
+ DEBUG_PRINT("average: %ld, %ld, %ld\n",
+ T(factory_cal).data[0],
+ T(factory_cal).data[1],
+ T(factory_cal).data[2]);
+ if (idx == ACC) {
+ // assume the largest data axis shows +1 or -1 gee
+ t = 0;
+ for (i = 0; i < 3; i++) {
+ if (abs(T(factory_cal).data[i]) > abs(T(factory_cal).data[t]))
+ t = i;
+ }
+ if (T(factory_cal).data[t] > 0) {
+ DEBUG_PRINT("assume axis %d is %d\n", t, CALIBRATION_ACC_1G);
+ T(factory_cal).data[t] -= CALIBRATION_ACC_1G;
+ } else {
+ DEBUG_PRINT("assume axis %d is %d\n", t, -CALIBRATION_ACC_1G);
+ T(factory_cal).data[t] += CALIBRATION_ACC_1G;
+ }
+ }
+ // set bias to offset registers
+ for (i = 0; i < 3; i++) {
+ T(sensors[idx]).offset[i] = T(factory_cal).data[i];
+ }
+ setOffsetReg(_task);
+ calibrationDeinit(_task, idx);
+
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS,
+ mSensorInfo[idx].sensorType,
+ T(factory_cal).data[0],
+ T(factory_cal).data[1],
+ T(factory_cal).data[2]);
+ INFO_PRINT("reported: %ld, %ld, %ld\n",
+ T(factory_cal).data[0],
+ T(factory_cal).data[1],
+ T(factory_cal).data[2]);
+ T(calibration_state) = CALIBRATION_DONE;
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ default:
+ break;
+ }
+}
+
+static bool accCalibration(void *cookie)
+{
+ TDECL();
+
+ INFO_PRINT("Accel Calibration\n");
+
+ if (!T(powered) && trySwitchState(SENSOR_CALIBRATING)) {
+ T(calibration_state) = CALIBRATION_START;
+ calibrationHandling(_task, ACC);
+ return true;
+ } else {
+ ERROR_PRINT("Cannot run calibration because sensor is busy\n");
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0);
+ return false;
+ }
+}
+
+static bool gyrCalibration(void *cookie)
+{
+ TDECL();
+
+ INFO_PRINT("Gyro Calibration\n");
+
+ if (!T(powered) && trySwitchState(SENSOR_CALIBRATING)) {
+ T(calibration_state) = CALIBRATION_START;
+ calibrationHandling(_task, GYR);
+ return true;
+ } else {
+ ERROR_PRINT("Cannot run calibration because sensor is busy\n");
+ sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO, 0, 0, 0);
+ return false;
+ }
+}
+
+/*
+ * Selftest
+ */
+static void sendTestResult(uint8_t status, uint8_t sensorType)
+{
+ struct TestResultData *data = heapAlloc(sizeof(struct TestResultData));
+ if (!data) {
+ ERROR_PRINT("Couldn't alloc test result packet");
+ return;
+ }
+
+ data->header.appId = ICM40600_APP_ID;
+ data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket));
+ data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT;
+ data->data_header.sensorType = sensorType;
+ data->data_header.status = status;
+
+ if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
+ ERROR_PRINT("Couldn't send test result packet");
+}
+
+static const uint16_t SelfTestEquation[256] = {
+ 2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
+ 2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
+ 3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
+ 3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
+ 3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
+ 3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
+ 4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
+ 4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
+ 4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
+ 5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
+ 5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
+ 6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
+ 6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
+ 7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
+ 7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
+ 8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
+ 9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
+ 10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
+ 10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
+ 11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
+ 12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
+ 13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
+ 15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
+ 16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
+ 17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
+ 19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
+ 20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
+ 22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
+ 24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
+ 26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
+ 28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
+ 30903, 31212, 31524, 31839, 32157, 32479, 32804
+};
+
+static void selfTestInit(TASK, enum SensorIndex idx)
+{
+ uint8_t val;
+
+ // Disable Interrupt
+ SPI_WRITE(REG_INT_SOURCE0, 0);
+ SPI_WRITE(REG_INT_SOURCE1, 0);
+
+ // reset offset registers
+ resetOffsetReg(_task, idx);
+
+ // stop FIFO
+ calSelftestFifoEnable(_task, idx, false, 0);
+
+ // self-test mode set
+ val = 0;
+ if (T(self_test).st_mode) {
+ if (idx == ACC) {
+ val |= (BIT_TEST_AX_EN | BIT_TEST_AY_EN | BIT_TEST_AZ_EN);
+ val |= BIT_SELF_TEST_REGULATOR_EN;
+ } else if (idx == GYR) {
+ val |= (BIT_TEST_GX_EN | BIT_TEST_GY_EN | BIT_TEST_GZ_EN);
+ }
+ }
+ SPI_WRITE(REG_SELF_TEST_CONFIG, val);
+
+ // set rate
+ SPI_WRITE(REG_GYRO_CONFIG0, (SELF_TEST_GYR_FS << SHIFT_GYRO_FS_SEL) |
+ (SELF_TEST_ODR << SHIFT_ODR_CONF));
+ SPI_WRITE(REG_ACCEL_CONFIG0, (SELF_TEST_ACC_FS << SHIFT_ACCEL_FS_SEL) |
+ (SELF_TEST_ODR << SHIFT_ODR_CONF));
+
+ // set filter
+ val = 0;
+ if (idx == ACC) {
+ val |= SELF_TEST_ACC_BW_IND;
+ } else if (idx == GYR) {
+ val |= SELF_TEST_GYR_BW_IND;
+ }
+ SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, val);
+
+ // turn on sensors
+ val = 0;
+ if (idx == ACC) {
+ val |= BIT_ACCEL_MODE_LN;
+ } else if (idx == GYR) {
+ val |= BIT_GYRO_MODE_LN;
+ }
+ SPI_WRITE(REG_PWR_MGMT_0, val, 200 * 1000);
+
+ calSelftestFifoEnable(_task, idx, true, SELF_TEST_READ_INTERVAL_US);
+}
+
+static void selfTestDeinit(TASK, enum SensorIndex idx)
+{
+ calSelftestFifoEnable(_task, idx, false, 0);
+
+ SPI_WRITE(REG_SELF_TEST_CONFIG, 0);
+ SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND);
+ SPI_WRITE(REG_PWR_MGMT_0, 0, 200); // 9136
+
+ // make register accesses happen when sensor is enabled next time
+ T(config).gyro_rate = 0;
+ T(config).accel_rate = 0;
+ T(config).fifo_rate = 0;
+ T(config).fifo_watermark = 0;
+ T(config).accel_on = false;
+ T(config).gyro_on = false;
+ T(config).wom_on = false;
+}
+
+static bool checkAccelSelftest(TASK)
+{
+ int32_t st_res;
+ uint32_t st_otp[3];
+ int i;
+ int32_t ratio;
+ bool pass = true;
+ bool otp_value_zero = false;
+
+ /* calculate ST_OTP */
+ for (i = 0; i < 3; i++) {
+ if (T(self_test.otp_st_data_accel[i] != 0))
+ st_otp[i] = SelfTestEquation[T(self_test).otp_st_data_accel[i] - 1];
+ else
+ otp_value_zero = true;
+ }
+
+ if (!otp_value_zero) {
+ /* criteria a */
+ for (i = 0; i < 3; i++) {
+ st_res = T(self_test).data_st_on[i] - T(self_test).data_st_off[i];
+ ratio = abs(st_res / st_otp[i] - SELF_TEST_PRECISION);
+ if (ratio >= SELF_TEST_ACC_SHIFT_DELTA) {
+ INFO_PRINT("error accel[%d] : st_res = %ld, st_otp = %ld\n", i, st_res, st_otp[i]);
+ pass = false;
+ }
+ }
+ } else {
+ /* criteria b */
+ for (i = 0; i < 3; i++) {
+ st_res = abs(T(self_test).data_st_on[i] - T(self_test).data_st_off[i]);
+ if (st_res < SELF_TEST_MIN_ACC || st_res > SELF_TEST_MAX_ACC) {
+ INFO_PRINT("error accel[%d] : st_res = %ld, min = %d, max = %d\n", i, st_res, SELF_TEST_MIN_ACC, SELF_TEST_MAX_ACC);
+ pass = false;
+ }
+ }
+ }
+
+ return pass;
+}
+
+static bool checkGyroSelftest(TASK)
+{
+ int32_t st_res;
+ uint32_t st_otp[3];
+ int i;
+ bool pass = true;
+ bool otp_value_zero = false;
+
+ /* calculate ST_OTP */
+ for (i = 0; i < 3; i++) {
+ if (T(self_test).otp_st_data_gyro[i] != 0)
+ st_otp[i] = SelfTestEquation[T(self_test).otp_st_data_gyro[i] - 1];
+ else
+ otp_value_zero = true;
+ }
+
+ if (!otp_value_zero) {
+ /* criteria a */
+ for (i = 0; i < 3; i++) {
+ st_res = T(self_test).data_st_on[i] - T(self_test).data_st_off[i];
+ if (st_res <= st_otp[i] * SELF_TEST_GYR_SHIFT_DELTA) {
+ INFO_PRINT("error gyro[%d] : st_res = %ld, st_otp = %ld\n", i, st_res, st_otp[i]);
+ pass = false;
+ }
+ }
+ } else {
+ /* criteria b */
+ for (i = 0; i < 3; i++) {
+ st_res = abs(T(self_test).data_st_on[i] - T(self_test).data_st_off[i]);
+ if (st_res < SELF_TEST_MIN_GYR) {
+ INFO_PRINT("error gyro[%d] : st_res = %ld, min = %d\n", i, st_res, SELF_TEST_MIN_GYR);
+ pass = false;
+ }
+ }
+ }
+
+ if (pass) {
+ /* criteria c */
+ for (i = 0; i < 3; i++) {
+ if (abs(T(self_test).data_st_off[i]) > SELF_TEST_MAX_GYR_OFFSET) {
+ INFO_PRINT("error gyro[%d] = %d, max = %d\n", i, abs(T(self_test).data_st_off[i]), SELF_TEST_MAX_GYR_OFFSET);
+ pass = false;
+ }
+ }
+ }
+
+ return pass;
+}
+
+static void selfTestHandling(TASK, enum SensorIndex idx)
+{
+ const uint8_t *buf, *data;
+ uint8_t int_status;
+ uint16_t fifo_count;
+ int i, t;
+ bool r;
+ int16_t raw_data[3] = { 0, 0, 0 };
+ bool pass;
+
+ if (idx != ACC && idx != GYR) {
+ ERROR_PRINT("Invalid sensor index\n");
+ return;
+ }
+
+ switch (T(selftest_state)) {
+ case TEST_START:
+ T(mRetryLeft) = RETRY_CNT_SELF_TEST;
+ selfTestInit(_task, idx);
+ for (i = 0; i < 3; i++) {
+ T(self_test).data[i] = 0;
+ }
+ SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
+ SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
+ T(selftest_state) = TEST_READ_STATUS;
+ T(self_test).data_count = SELF_TEST_SAMPLE_NB;
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ case TEST_READ_STATUS:
+ buf = T(dataBuffer[0]);
+ int_status = buf[1];
+ buf = T(dataBuffer[1]);
+ fifo_count = buf[2] << 8 | buf[1];
+ fifo_count = fifo_count - fifo_count % T(config).fifo_sample_size;
+ T(fifo_count) = fifo_count;
+ DEBUG_PRINT("fifo_count = %d\n", fifo_count);
+ if (int_status & BIT_INT_STATUS_FIFO_FULL) {
+ ERROR_PRINT("fifo overflow\n");
+ selfTestDeinit(_task, idx);
+ T(selftest_state) = TEST_DONE;
+ sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType);
+ } else {
+ T(selftest_state) = TEST_READ_DATA;
+ SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0]));
+ }
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ case TEST_READ_DATA:
+ buf = T(dataBuffer[0]);
+ data = &buf[1];
+ for (i = 0; i < T(fifo_count); i += T(config).fifo_sample_size) {
+ r = calSelftestGetOneData(idx, data, i, raw_data);
+ if (r == false) {
+ ERROR_PRINT("invalid data packet\n");
+ selfTestDeinit(_task, idx);
+ T(selftest_state) = TEST_DONE;
+ sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType);
+ break;
+ }
+ for (t = 0; t < 3; t++) {
+ T(self_test).data[t] += raw_data[t];
+ }
+ T(self_test).data_count--;
+ if (T(self_test).data_count == 0) {
+ break;
+ }
+ }
+ if (T(self_test).data_count > 0) {
+ if (--T(mRetryLeft) == 0) {
+ ERROR_PRINT("selftest timeout\n");
+ selfTestDeinit(_task, idx);
+ T(selftest_state) = TEST_DONE;
+ sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType);
+ } else {
+ calSelftestFifoEnable(_task, idx, false, 0); // toggle FIFO to reset
+ calSelftestFifoEnable(_task, idx, true, SELF_TEST_READ_INTERVAL_US);
+ SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
+ SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
+ T(selftest_state) = TEST_READ_STATUS;
+ }
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ }
+ T(selftest_state) = TEST_READ_OTP;
+ // fall-through
+ case TEST_READ_OTP:
+ for (i = 0; i < 3; i++) {
+ T(self_test).data[i] = T(self_test).data[i] / SELF_TEST_SAMPLE_NB * SELF_TEST_PRECISION;
+ }
+ INFO_PRINT("st_mode=%d average (scaled) : %ld, %ld, %ld\n",
+ T(self_test).st_mode,
+ T(self_test).data[0],
+ T(self_test).data[1],
+ T(self_test).data[2]);
+
+ selfTestDeinit(_task, idx);
+
+ for (i = 0; i < 3; i++) {
+ if (T(self_test).st_mode) {
+ T(self_test).data_st_on[i] = T(self_test).data[i];
+ } else {
+ T(self_test).data_st_off[i] = T(self_test).data[i];
+ }
+ }
+
+ // Run again with self-test mode on
+ if (!T(self_test).st_mode) {
+ T(self_test).st_mode = true;
+ T(selftest_state) = TEST_START;
+ } else {
+ INFO_PRINT("st mode on : %ld %ld %ld\n",
+ T(self_test).data_st_on[0],
+ T(self_test).data_st_on[1],
+ T(self_test).data_st_on[2]);
+ INFO_PRINT("st mode off : %ld %ld %ld\n",
+ T(self_test).data_st_off[0],
+ T(self_test).data_st_off[1],
+ T(self_test).data_st_off[2]);
+
+ // read OTP
+ if (idx == ACC) {
+ SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_2);
+ SPI_READ(REG_XA_ST_DATA, 3, &T(dataBuffer[0]));
+ } else if (idx == GYR) {
+ SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_1);
+ SPI_READ(REG_XG_ST_DATA, 3, &T(dataBuffer[0]));
+ }
+ SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_0);
+
+ T(selftest_state) = TEST_REPORT;
+ }
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ case TEST_REPORT:
+ buf = T(dataBuffer[0]);
+ if (idx == ACC) {
+ T(self_test).otp_st_data_accel[0] = buf[1];
+ T(self_test).otp_st_data_accel[1] = buf[2];
+ T(self_test).otp_st_data_accel[2] = buf[3];
+ INFO_PRINT("otp accel : %d %d %d\n",
+ T(self_test).otp_st_data_accel[0],
+ T(self_test).otp_st_data_accel[1],
+ T(self_test).otp_st_data_accel[2]);
+ } else if (idx == GYR) {
+ T(self_test).otp_st_data_gyro[0] = buf[1];
+ T(self_test).otp_st_data_gyro[1] = buf[2];
+ T(self_test).otp_st_data_gyro[2] = buf[3];
+ INFO_PRINT("otp gyro : %d %d %d\n",
+ T(self_test).otp_st_data_gyro[0],
+ T(self_test).otp_st_data_gyro[1],
+ T(self_test).otp_st_data_gyro[2]);
+ }
+
+ pass = false;
+ if (idx == ACC) {
+ pass = checkAccelSelftest(_task);
+ } else if (idx == GYR) {
+ pass = checkGyroSelftest(_task);
+ }
+ if (pass) {
+ sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS,
+ mSensorInfo[idx].sensorType);
+ } else {
+ sendTestResult(SENSOR_APP_EVT_STATUS_ERROR,
+ mSensorInfo[idx].sensorType);
+ }
+
+ T(selftest_state) = TEST_DONE;
+ selfTestDeinit(_task, idx);
+ spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
+ break;
+ default:
+ break;
+ }
+}
+
+static bool accSelfTest(void *cookie)
+{
+ TDECL();
+
+ INFO_PRINT("Accel Selftest\n");
+
+ if (!T(powered) && trySwitchState(SENSOR_TESTING)) {
+ T(self_test).st_mode = false;
+ T(selftest_state) = TEST_START;
+ selfTestHandling(_task, ACC);
+ return true;
+ } else {
+ ERROR_PRINT("cannot test accel because sensor is busy\n");
+ sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL);
+ return false;
+ }
+}
+
+static bool gyrSelfTest(void *cookie)
+{
+ TDECL();
+
+ INFO_PRINT("Gyro Selftest\n");
+
+ if (!T(powered) && trySwitchState(SENSOR_TESTING)) {
+ T(self_test).st_mode = false;
+ T(selftest_state) = TEST_START;
+ selfTestHandling(_task, GYR);
+ return true;
+ } else {
+ ERROR_PRINT("cannot test accel because sensor is busy\n");
+ sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO);
+ return false;
+ }
+}
+
+static bool sensorFirmwareUpload(void *cookie)
+{
+ TDECL();
+ int i = (long int)cookie;
+
+ sensorSignalInternalEvt(T(sensors[i]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
+
+ return true;
+}
+
+static bool enableInterrupt(struct Gpio *pin, IRQn_Type irq, struct ChainedIsr *isr)
+{
+ gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
+ syscfgSetExtiPort(pin);
+ extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
+ extiChainIsr(irq, isr);
+ return true;
+}
+
+static bool disableInterrupt(struct Gpio *pin, IRQn_Type irq, struct ChainedIsr *isr)
+{
+ extiUnchainIsr(irq, isr);
+ extiDisableIntGpio(pin);
+ return true;
+}
+
+static void configInt1(TASK, bool on)
+{
+ enum SensorIndex i;
+ bool powered = false;
+
+ if (on && !T(Int1_EN)) {
+ enableInterrupt(T(Int1), T(Irq1), &T(Isr1));
+ T(Int1_EN) = true;
+ } else if (!on && T(Int1_EN)) {
+ for (i = 0; i < NUM_OF_SENSOR; i++) {
+ if (T(sensors[i]).powered) {
+ powered = true;
+ break;
+ }
+ }
+ if (!powered) {
+ disableInterrupt(T(Int1), T(Irq1), &T(Isr1));
+ T(Int1_EN) = false;
+ }
+ }
+}
+
+static uint8_t computeOdrConf(uint32_t rate)
+{
+ switch (rate) {
+ case SENSOR_HZ(1000.0f):
+ return 6;
+ case SENSOR_HZ(200.0f):
+ return 7;
+ case SENSOR_HZ(100.0f):
+ return 8;
+ case SENSOR_HZ(50.0f):
+ return 9;
+ case SENSOR_HZ(25.0f):
+ default:
+ return 10;
+ }
+}
+
+static void computeConfig(TASK, struct ICM40600Config *config)
+{
+ uint64_t latency;
+ uint64_t val;
+ uint32_t latency_ms;
+ int i;
+
+ // copy current parameters
+ *config = T(config);
+
+ // compute sensors on
+ if (T(sensors[ACC]).configed || T(sensors[WOM]).configed || T(sensors[NOMO]).configed) {
+ config->accel_on = true;
+ } else {
+ config->accel_on = false;
+ }
+ config->gyro_on = T(sensors[GYR]).configed;
+ if (T(sensors[WOM]).configed || T(sensors[NOMO]).configed) {
+ config->wom_on = true;
+ } else {
+ config->wom_on = false;
+ }
+
+ // compute accel and gyro rates
+ if (config->accel_on) {
+ if (T(sensors[ACC]).configed) {
+ if (T(sensors[ACC]).rate > NO_DECIMATION_MAX_RATE) {
+ config->accel_rate = DECIMATION_HIGH_RATE;
+ } else if (T(sensors[ACC]).rate < NO_DECIMATION_MIN_RATE) {
+ config->accel_rate = DECIMATION_LOW_RATE;
+ } else {
+ config->accel_rate = T(sensors[ACC]).rate;
+ }
+ } else {
+ config->accel_rate = NO_DECIMATION_MIN_RATE;
+ }
+ }
+ if (config->gyro_on) {
+ if (T(sensors[GYR]).configed) {
+ if (T(sensors[GYR]).rate > NO_DECIMATION_MAX_RATE) {
+ config->gyro_rate = DECIMATION_HIGH_RATE;
+ } else if (T(sensors[GYR]).rate < NO_DECIMATION_MIN_RATE) {
+ config->gyro_rate = DECIMATION_LOW_RATE;
+ } else {
+ config->gyro_rate = T(sensors[GYR]).rate;
+ }
+ } else {
+ config->gyro_rate = NO_DECIMATION_MIN_RATE;
+ }
+ }
+
+ // compute wom threshold
+ if (config->wom_on) {
+ config->wom_threshold = ICM40600_WOM_THRESHOLD_MG / (config->accel_rate / NO_DECIMATION_MIN_RATE);
+ }
+
+ // compute fifo configuration
+ if (T(sensors[ACC]).configed || T(sensors[GYR]).configed) {
+ config->fifo_sample_size = FIFO_PACKET_SIZE;
+ } else {
+ config->fifo_sample_size = 0;
+ }
+
+ // compute fifo rate and latency/watermark
+ config->fifo_rate = 0;
+ latency = SENSOR_LATENCY_NODATA;
+ for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
+ if (T(sensors[i]).configed) {
+ // look for the highest rate
+ if (T(sensors[i]).rate > config->fifo_rate) {
+ config->fifo_rate = T(sensors[i]).rate;
+ }
+ // look for the shortest latency
+ if (T(sensors[i]).latency < latency) {
+ latency = T(sensors[i]).latency;
+ }
+ }
+ }
+ if (config->fifo_rate > NO_DECIMATION_MAX_RATE) {
+ config->fifo_rate = DECIMATION_HIGH_RATE;
+ } else if (config->fifo_rate < NO_DECIMATION_MIN_RATE) {
+ config->fifo_rate = DECIMATION_LOW_RATE;
+ }
+
+ // add 0.5ms for rounding
+ latency_ms = cpuMathU64DivByU16(cpuMathU64DivByU16(latency + 500000, 1000), 1000);
+ val = (uint64_t)latency_ms * (uint64_t)config->fifo_rate;
+ config->fifo_watermark = cpuMathU64DivByU16(cpuMathU64DivByU16(val, 1000), RATE_TO_HZ);
+ config->fifo_watermark *= config->fifo_sample_size;
+ if (config->fifo_watermark < config->fifo_sample_size) {
+ config->fifo_watermark = config->fifo_sample_size;
+ }
+ if (config->fifo_watermark > MAX_BATCH_SIZE) {
+ config->fifo_watermark = MAX_BATCH_SIZE - (MAX_BATCH_SIZE % config->fifo_sample_size);
+ }
+}
+
+static void updateConfig(TASK, const struct ICM40600Config *config)
+{
+ uint32_t delay_us;
+ uint8_t val, val2;
+
+ // Disable Interrupt
+ SPI_WRITE(REG_INT_SOURCE0, 0);
+ SPI_WRITE(REG_INT_SOURCE1, 0);
+
+ // set rate and WoM threshold
+ if (config->gyro_rate != T(config).gyro_rate) {
+ val = computeOdrConf(config->gyro_rate);
+ SPI_WRITE(REG_GYRO_CONFIG0, (ICM40600_GYR_FS << SHIFT_GYRO_FS_SEL) |
+ (val << SHIFT_ODR_CONF));
+ }
+ if (config->accel_rate != T(config).accel_rate) {
+ val = computeOdrConf(config->accel_rate);
+ SPI_WRITE(REG_ACCEL_CONFIG0, (ICM40600_ACC_FS << SHIFT_ACCEL_FS_SEL) |
+ (val << SHIFT_ODR_CONF));
+ }
+ if (config->wom_threshold != T(config).wom_threshold) {
+ val = ICM40600_WOM_COMPUTE(config->wom_threshold);
+ SPI_WRITE(REG_ACCEL_WOM_X_THR, val);
+ SPI_WRITE(REG_ACCEL_WOM_Y_THR, val);
+ SPI_WRITE(REG_ACCEL_WOM_Z_THR, val);
+ }
+
+ // set WM
+ if (config->fifo_watermark != T(config).fifo_watermark) {
+ SPI_WRITE(REG_FIFO_CONFIG2, config->fifo_watermark & 0xFF);
+ SPI_WRITE(REG_FIFO_CONFIG3, (config->fifo_watermark >> 8) & 0xFF);
+ }
+
+ // turn on/off accel and gyro if any change
+ if (config->gyro_on != T(config).gyro_on || config->accel_on != T(config).accel_on) {
+ val = 0;
+ if (config->gyro_on) {
+ val |= BIT_GYRO_MODE_LN;
+ }
+ if (config->accel_on) {
+ val |= BIT_ACCEL_MODE_LN;
+ }
+ // delay needed if gyro or accel turning on
+ if ((config->gyro_on && !T(config).gyro_on) || (config->accel_on && !T(config).accel_on)) {
+ delay_us = 200; // 9136
+ } else {
+ delay_us = 0;
+ }
+ SPI_WRITE(REG_PWR_MGMT_0, val, delay_us);
+ }
+
+ // turn on/off WOM
+ if (config->wom_on != T(config).wom_on) {
+ if (config->wom_on) {
+ val = BIT_WOM_INT_MODE_OR | BIT_WOM_MODE_PREV | BIT_SMD_MODE_OLD;
+ } else {
+ val = 0;
+ }
+ SPI_WRITE(REG_SMD_CONFIG, val);
+ }
+
+ // turn on/off FIFO
+ if (config->fifo_sample_size != T(config).fifo_sample_size) {
+ if (config->fifo_sample_size != 0) {
+ // enabling FIFO
+ val = BIT_FIFO_MODE_STREAM;
+ val2 = BIT_FIFO_ACCEL_EN | BIT_FIFO_GYRO_EN | BIT_FIFO_TEMP_EN |
+ BIT_FIFO_TMST_FSYNC_EN | BIT_FIFO_WM_TH;
+ // reset chip time
+ T(chip_time_us) = 0;
+ T(chip_timestamp) = 0;
+ T(fifo_start_sync) = true;
+ invalidate_sensortime_to_rtc_time(_task);
+ } else {
+ // disabling FIFO
+ val = BIT_FIFO_MODE_BYPASS;
+ val2 = 0;
+ }
+ SPI_WRITE(REG_FIFO_CONFIG, val);
+ SPI_WRITE(REG_FIFO_CONFIG1, val2);
+ if (val == BIT_FIFO_MODE_BYPASS) {
+ SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[0])); // 9052
+ }
+ }
+
+ // enable/disable FIFO data interrupt
+ if (config->fifo_sample_size != 0) {
+ val = BIT_INT_FIFO_THS_INT1_EN;
+ } else {
+ val = 0;
+ }
+ SPI_WRITE(REG_INT_SOURCE0, val);
+
+ // enable/disable WOM interrupt (only when FIFO disabled or batch mode)
+ if (config->wom_on && (config->fifo_sample_size == 0 || config->fifo_watermark > config->fifo_sample_size)) {
+ val = BIT_INT_WOM_XYZ_INT1_EN;
+ } else {
+ val = 0;
+ }
+ SPI_WRITE(REG_INT_SOURCE1, val);
+}
+
+static void applyConfig(TASK, const struct ICM40600Config *config)
+{
+ uint32_t duration_us;
+ uint32_t decimator;
+ int i;
+
+ // setup wait for odr change
+ if (config->accel_rate != T(config).accel_rate) {
+ T(sensors[ACC]).wait_for_odr = true;
+ }
+ if (config->gyro_rate != T(config).gyro_rate) {
+ T(sensors[GYR]).wait_for_odr = true;
+ }
+
+ // setup first samples skip
+ if (config->gyro_on && !T(config).gyro_on) {
+ // gyro turning on
+ duration_us = (1000000 * RATE_TO_HZ) / config->gyro_rate;
+ if (duration_us * GYR_SKIP_SAMPLE_NB > ICM40600_GYRO_START_TIME_MS * 1000) {
+ T(sensors[GYR]).skip_sample_cnt = GYR_SKIP_SAMPLE_NB;
+ } else {
+ T(sensors[GYR]).skip_sample_cnt = (ICM40600_GYRO_START_TIME_MS * 1000) / duration_us;
+ if ((ICM40600_GYRO_START_TIME_MS * 1000) % duration_us) {
+ T(sensors[GYR]).skip_sample_cnt++;
+ }
+ }
+ }
+ if (config->accel_on && !T(config).accel_on) {
+ // accel turning on
+ duration_us = (1000000 * RATE_TO_HZ) / config->accel_rate;
+ if (duration_us * ACC_SKIP_SAMPLE_NB > ICM40600_ACCEL_START_TIME_MS * 1000) {
+ T(sensors[ACC]).skip_sample_cnt = ACC_SKIP_SAMPLE_NB;
+ } else {
+ T(sensors[ACC]).skip_sample_cnt = (ICM40600_ACCEL_START_TIME_MS * 1000) / duration_us;
+ if ((ICM40600_ACCEL_START_TIME_MS * 1000) % duration_us) {
+ T(sensors[ACC]).skip_sample_cnt++;
+ }
+ }
+ }
+
+ // update all sensors decimators
+ for (i = 0; i <= GYR; i++) {
+ if (!T(sensors[i]).configed || T(sensors[i]).rate == 0) {
+ decimator = 1;
+ } else {
+ if (i == ACC) {
+ decimator = config->accel_rate / T(sensors[i]).rate;
+ } else if (i == GYR) {
+ decimator = config->gyro_rate / T(sensors[i]).rate;
+ }
+ }
+ if (decimator != T(sensors[i]).decimator) {
+ T(sensors[i]).decimator = decimator;
+ T(sensors[i]).data_cnt = 0;
+ }
+ }
+
+ // setup NOMO timer
+ if (T(sensors[NOMO]).configed && !T(noMotionTimerHandle)) {
+ T(noMotionTimerHandle) = timTimerSet(ICM40600_NOM_DURATION_NS, 0, 100, noMotionCallback, &T(sensors[NOMO]), false);
+ } else if (!T(sensors[NOMO]).configed && T(noMotionTimerHandle)) {
+ timTimerCancel(T(noMotionTimerHandle));
+ T(noMotionTimerHandle) = 0;
+ }
+
+ // update config
+ T(config) = *config;
+
+ DEBUG_PRINT("config: accel(%d, %luHz/%u), gyro(%d, %luHz/%u), wom(%d, %lu), "
+ "fifo(%u/%u, %luHz)\n",
+ T(config).accel_on, T(config).accel_rate / RATE_TO_HZ, T(sensors[ACC]).decimator,
+ T(config).gyro_on, T(config).gyro_rate / RATE_TO_HZ, T(sensors[GYR]).decimator,
+ T(config).wom_on, T(config).wom_threshold,
+ T(config).fifo_sample_size, T(config).fifo_watermark, T(config).fifo_rate / RATE_TO_HZ);
+}
+
+static void configSensor(TASK)
+{
+ struct ICM40600Config config;
+
+ computeConfig(_task, &config);
+ updateConfig(_task, &config);
+ applyConfig(_task, &config);
+}
+
+static void sensorTurnOn(TASK)
+{
+ /* enable chip timestamp */
+ SPI_WRITE(REG_TMST_CONFIG, BIT_EN_DREG_FIFO_D2A | BIT_TMST_EN);
+
+ T(powered) = true;
+#if DBG_TIMESTAMP
+ memset(&T(statistics_set), 0, sizeof(struct StatisticsSet));
+#endif
+ DEBUG_PRINT("chip on\n");
+}
+
+static void sensorTurnOff(TASK)
+{
+ /* disable chip timestamp */
+ SPI_WRITE(REG_TMST_CONFIG, BIT_EN_DREG_FIFO_D2A);
+
+ T(powered) = false;
+#if DBG_TIMESTAMP
+ DEBUG_PRINT("time sync stats: reset: %ld, sync: %ld, adjust+: %ld, adjust-: %ld, truncate: %ld\n",
+ T(statistics_set).sync_reset_count,
+ T(statistics_set).sync_count,
+ T(statistics_set).sync_adjust_plus,
+ T(statistics_set).sync_adjust_minus,
+ T(statistics_set).sync_truncate);
+#endif
+ DEBUG_PRINT("chip off\n");
+}
+
+static void sensorPower(TASK, int sensorType, bool on)
+{
+ bool chip_on;
+ int i;
+
+ if (on) {
+ // turn on chip if needed
+ if (!T(powered)) {
+ sensorTurnOn(_task);
+ }
+ } else {
+ // change chip configuration
+ configSensor(_task);
+ // turn chip off if needed
+ chip_on = false;
+ for (i = 0; i < NUM_OF_SENSOR; ++i) {
+ if (T(sensors[i]).powered) {
+ chip_on = true;
+ break;
+ }
+ }
+ if (!chip_on) {
+ sensorTurnOff(_task);
+ }
+ }
+}
+
+static bool sensorSetPower(bool on, void *cookie)
+{
+ TDECL();
+ int sensor_type = (int)cookie;
+ struct ICM40600Sensor *sensor = &T(sensors[sensor_type]);
+
+ DEBUG_PRINT("%s: on=%d, state=%" PRI_STATE "\n", mSensorInfo[sensor_type].sensorName, on, getStateName(GET_STATE()));
+
+ if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
+ sensor->powered = on;
+ if (!on) {
+ sensor->configed = false;
+ }
+ configInt1(_task, on);
+ sensorPower(_task, sensor_type, on);
+ spiBatchTxRx(&T(mode), sensorSpiCallback, sensor, __FUNCTION__);
+ } else {
+ T(pending_config[sensor_type]) = true;
+ sensor->pConfig.enable = on;
+ }
+
+ return true;
+}
+
+static bool sensorSetRate(uint32_t rate, uint64_t latency, void *cookie)
+{
+ TDECL();
+ int sensor_type = (int)cookie;
+ struct ICM40600Sensor *sensor = &T(sensors[sensor_type]);
+
+ DEBUG_PRINT("%s: rate=%lu, latency=%llu, state=%" PRI_STATE "\n",
+ mSensorInfo[sensor_type].sensorName, rate, latency, getStateName(GET_STATE()));
+
+ if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
+ sensor->rate = rate;
+ sensor->latency = latency;
+ sensor->configed = true;
+ configSensor(_task);
+ spiBatchTxRx(&T(mode), sensorSpiCallback, sensor, __FUNCTION__);
+ } else {
+ T(pending_config[sensor_type]) = true;
+ sensor->pConfig.enable = sensor->powered;
+ sensor->pConfig.rate = rate;
+ sensor->pConfig.latency = latency;
+ }
+
+ return true;
+}
+
+static void sendFlushEvt(void)
+{
+ TDECL();
+ uint32_t evtType = 0;
+ int i;
+
+ for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
+ while (T(sensors[i]).flush > 0) {
+ evtType = sensorGetMyEventType(mSensorInfo[i].sensorType);
+ osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL);
+ T(sensors[i]).flush--;
+ }
+ }
+}
+
+static void int1Evt(TASK, bool flush);
+
+static bool flushSensor(void *cookie)
+{
+ TDECL();
+ int sensor_idx = (int)cookie;
+ uint32_t evtType;
+
+ DEBUG_PRINT("%s flush\n", mSensorInfo[sensor_idx].sensorName);
+
+ if (sensor_idx >= FIRST_CONT_SENSOR && sensor_idx < NUM_CONT_SENSOR) {
+ T(sensors[sensor_idx]).flush++;
+ int1Evt(_task, true);
+ return true;
+ }
+ if (sensor_idx >= FIRST_ONESHOT_SENSOR && sensor_idx < NUM_OF_SENSOR) {
+ evtType = sensorGetMyEventType(mSensorInfo[sensor_idx].sensorType);
+ osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL);
+ return true;
+ }
+
+ return false;
+}
+
+static bool flushData(struct ICM40600Sensor *sensor, uint32_t eventId)
+{
+ bool success = false;
+
+ if (sensor->data_evt) {
+ success = osEnqueueEvtOrFree(eventId, sensor->data_evt, dataEvtFree);
+ sensor->data_evt = NULL;
+ }
+
+ return success;
+}
+
+static void flushAllData(void)
+{
+ TDECL();
+ int i;
+
+ for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
+ flushData(&T(sensors[i]),
+ EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType));
+ }
+}
+
+static bool allocateDataEvt(struct ICM40600Sensor *mSensor, uint64_t rtc_time)
+{
+ TDECL();
+
+ mSensor->data_evt = slabAllocatorAlloc(T(mDataSlab));
+ if (mSensor->data_evt == NULL) {
+ // slab allocation failed
+ ERROR_PRINT("slabAllocatorAlloc() failed\n");
+ return false;
+ }
+
+ // delta time for the first sample is sample count
+ memset(&mSensor->data_evt->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
+ mSensor->data_evt->referenceTime = rtc_time;
+ mSensor->prev_rtc_time = rtc_time;
+
+ return true;
+}
+
+static void parseOnePacket(TASK, const uint8_t *data, int idx)
+{
+ struct FifoPacketData fifo_data;
+ uint16_t diff;
+
+ getFifoData(data, idx, &fifo_data);
+
+ /* wait for odr */
+ if (T(sensors[ACC]).wait_for_odr) {
+ if (fifo_data.odr_accel) {
+ T(sensors[ACC]).wait_for_odr = false;
+ } else {
+ fifo_data.valid_accel = false;
+ }
+ }
+ if (T(sensors[GYR]).wait_for_odr) {
+ if (fifo_data.odr_gyro) {
+ T(sensors[GYR]).wait_for_odr = false;
+ } else {
+ fifo_data.valid_gyro = false;
+ }
+ }
+
+ /* drop first some samples */
+ if (fifo_data.valid_accel) {
+ if (T(sensors[ACC]).skip_sample_cnt > 0) {
+ fifo_data.valid_accel = false;
+ T(sensors[ACC]).skip_sample_cnt--;
+ }
+ }
+ if (fifo_data.valid_gyro) {
+ if (T(sensors[GYR]).skip_sample_cnt > 0) {
+ fifo_data.valid_gyro = false;
+ T(sensors[GYR]).skip_sample_cnt--;
+ }
+ }
+
+ /* update sensors data */
+ if (fifo_data.valid_accel) {
+ ICM40600_TO_ANDROID_COORDINATE(fifo_data.accel[0], fifo_data.accel[1], fifo_data.accel[2]);
+ if (T(sensors[ACC]).configed) {
+ T(sensors[ACC]).data[0] = fifo_data.accel[0];
+ T(sensors[ACC]).data[1] = fifo_data.accel[1];
+ T(sensors[ACC]).data[2] = fifo_data.accel[2];
+ T(sensors[ACC]).updated = true;
+ T(sensors[ACC]).data_cnt++;
+ }
+ }
+ if (fifo_data.valid_gyro) {
+ ICM40600_TO_ANDROID_COORDINATE(fifo_data.gyro[0], fifo_data.gyro[1], fifo_data.gyro[2]);
+ if (T(sensors[GYR]).configed) {
+ T(sensors[GYR]).data[0] = fifo_data.gyro[0];
+ T(sensors[GYR]).data[1] = fifo_data.gyro[1];
+ T(sensors[GYR]).data[2] = fifo_data.gyro[2];
+ T(sensors[GYR]).updated = true;
+ T(sensors[GYR]).data_cnt++;
+ }
+ }
+
+ // update temperature
+ T(chip_temperature) = fifo_data.temp * TEMP_SCALE + TEMP_OFFSET;
+
+ // count up chip time
+ if (T(chip_time_us) == 0) {
+ T(chip_time_us) = (uint64_t)fifo_data.timestamp * CHIP_TIME_RES_US + CHIP_TIME_OFFSET_US;
+ } else {
+ // unsigned difference handle counter roll-up
+ diff = fifo_data.timestamp - T(chip_timestamp);
+ T(chip_time_us) += diff * CHIP_TIME_RES_US;
+ }
+ T(chip_timestamp) = fifo_data.timestamp;
+}
+
+static void pushSensorData(TASK, struct ICM40600Sensor *mSensor, uint64_t rtc_time)
+{
+ float x, y, z;
+ float offset[3];
+ bool new_offset_update = false;
+ struct TripleAxisDataPoint *sample;
+ uint32_t delta_time;
+
+ switch (mSensor->idx) {
+ case ACC:
+ // scale data to android units
+ x = mSensor->data[0] * kScale_acc;
+ y = mSensor->data[1] * kScale_acc;
+ z = mSensor->data[2] * kScale_acc;
+ // run and apply calibration on sensor data
+#ifdef ACCEL_CAL_ENABLED
+ accelCalRun(&T(accel_cal), rtc_time, x, y, z, T(chip_temperature));
+ accelCalBiasRemove(&T(accel_cal), &x, &y, &z);
+# ifdef ACCEL_CAL_DBG_ENABLED
+ accelCalDebPrint(&T(accel_cal), T(chip_temperature));
+# endif
+#endif
+#ifdef GYRO_CAL_ENABLED
+ gyroCalUpdateAccel(&T(gyro_cal), rtc_time, x, y, z);
+#endif
+ break;
+ case GYR:
+ // scale data to android units
+ x = mSensor->data[0] * kScale_gyr;
+ y = mSensor->data[1] * kScale_gyr;
+ z = mSensor->data[2] * kScale_gyr;
+ // run and apply calibration on sensor data
+#ifdef GYRO_CAL_ENABLED
+ gyroCalUpdateGyro(&T(gyro_cal), rtc_time, x, y, z, T(chip_temperature));
+ gyroCalRemoveBias(&T(gyro_cal), x, y, z, &x, &y, &z);
+ new_offset_update = gyroCalNewBiasAvailable(&T(gyro_cal));
+ if (new_offset_update) {
+ float gyro_offset_temperature_celsius;
+ gyroCalGetBias(&T(gyro_cal), &offset[0], &offset[1], &offset[2],
+ &gyro_offset_temperature_celsius);
+ }
+#endif
+ break;
+ default:
+ return;
+ }
+
+ if (mSensor->data_evt == NULL) {
+ if (!allocateDataEvt(mSensor, rtc_time))
+ return;
+ }
+
+ if (mSensor->data_evt->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) {
+ ERROR_PRINT("samples bad index\n");
+ return;
+ }
+
+ // handle bias sending
+ if (new_offset_update) {
+ if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
+ // flush existing samples so the bias appears after them.
+ flushData(mSensor, EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[mSensor->idx].sensorType));
+ if (!allocateDataEvt(mSensor, rtc_time)) {
+ return;
+ }
+ }
+ mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
+ mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
+ mSensor->data_evt->samples[0].firstSample.biasSample = mSensor->data_evt->samples[0].firstSample.numSamples;
+ sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+ // Updates the offset in HAL.
+ sample->x = offset[0];
+ sample->y = offset[1];
+ sample->z = offset[2];
+ flushData(mSensor, sensorGetMyEventType(mSensorInfo[mSensor->idx].biasType));
+ DEBUG_PRINT("send new gyro bias\n");
+ if (!allocateDataEvt(mSensor, rtc_time)) {
+ return;
+ }
+ }
+
+ sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+
+ // the first deltatime is for sample size
+ if (mSensor->data_evt->samples[0].firstSample.numSamples > 1) {
+ delta_time = rtc_time - mSensor->prev_rtc_time;
+ delta_time = delta_time < 0 ? 0 : delta_time;
+ sample->deltaTime = delta_time;
+ mSensor->prev_rtc_time = rtc_time;
+ }
+
+ sample->x = x;
+ sample->y = y;
+ sample->z = z;
+
+ if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
+ flushAllData();
+ }
+}
+
+static void computeTimeSync(TASK, const uint8_t *data, uint32_t count, uint64_t data_timestamp)
+{
+ const uint64_t now = sensorGetTime();
+ uint64_t chip_time_us;
+ uint32_t sample_index;
+ uint32_t duration_us;
+ struct FifoPacketData packet;
+ uint16_t sample_nb;
+ uint16_t chip_timestamp;
+ uint16_t diff;
+ bool first_sync = false;
+ bool sync = false;
+
+ /* scan fifo data for the latest chip timestamp */
+ chip_time_us = T(chip_time_us);
+ chip_timestamp = T(chip_timestamp);
+ sample_index = 0;
+ sample_nb = 0;
+ while (count >= T(config).fifo_sample_size) {
+ sample_nb++;
+ getFifoData(data, sample_index, &packet);
+ sample_index += T(config).fifo_sample_size;
+ count -= T(config).fifo_sample_size;
+ // count up chip time
+ if (chip_time_us == 0) {
+ // first chip timestamp
+ chip_time_us = (uint64_t)packet.timestamp * CHIP_TIME_RES_US + CHIP_TIME_OFFSET_US;
+ } else {
+ // unsigned difference handle counter roll-up
+ diff = packet.timestamp - chip_timestamp;
+ chip_time_us += diff * CHIP_TIME_RES_US;
+ }
+ chip_timestamp = packet.timestamp;
+ }
+
+ /* first time sync after FIFO enable */
+ if (T(fifo_start_sync)) {
+ if (T(config).fifo_rate != 0) {
+ duration_us = (1000000 * RATE_TO_HZ) / T(config).fifo_rate;
+ } else {
+ duration_us = 0;
+ }
+ // use estimated duration
+ map_sensortime_to_rtc_time(_task,
+ chip_time_us - duration_us * sample_nb,
+ data_timestamp - (uint64_t)duration_us * 1000ULL * sample_nb);
+ T(fifo_start_sync) = false;
+ T(last_sync_time) = now;
+ first_sync = true;
+ }
+
+ /* periodical time sync */
+ if ((now - T(last_sync_time)) > MSEC_TO_NANOS(100)) {
+ // every 100ms
+ uint64_t estimated_rtc;
+ uint64_t min_rtc, max_rtc;
+ uint64_t limit_ns, adjust_ns;
+ uint64_t updated_data_timestamp = data_timestamp;
+ bool valid = sensortime_to_rtc_time(_task, chip_time_us, &estimated_rtc);
+ // check if appropriate to inject to time sync algorithm
+ // adjust rtc time if necessary not to make large jitters
+ if (valid) {
+ limit_ns = U64_DIV_BY_CONST_U16((data_timestamp - T(last_sync_data_ts)) * 10, 1000); // 1% (100ms * 1% = 1ms)
+ adjust_ns = U64_DIV_BY_CONST_U16((data_timestamp - T(last_sync_data_ts)) * 1, 10000); // 0.01% (100ms * 0.01% = 10us)
+ min_rtc = data_timestamp - limit_ns; // actual ts - x
+ max_rtc = data_timestamp + limit_ns; // actual ts + x
+ if ((estimated_rtc >= min_rtc) && (estimated_rtc <= max_rtc)) {
+ sync = true;
+ } else if (estimated_rtc < min_rtc) {
+ sync = true;
+ updated_data_timestamp = updated_data_timestamp - adjust_ns;
+#if DBG_TIMESTAMP
+ T(statistics_set).sync_adjust_minus++;
+#endif
+ } else if (estimated_rtc > max_rtc) {
+ sync = true;
+ updated_data_timestamp = updated_data_timestamp + adjust_ns;
+#if DBG_TIMESTAMP
+ T(statistics_set).sync_adjust_plus++;
+#endif
+ }
+ // limit the adjustment
+ if (sync) {
+ if (updated_data_timestamp > (data_timestamp + MSEC_TO_NANOS(1))) {
+ updated_data_timestamp = data_timestamp + MSEC_TO_NANOS(1);
+ }
+ }
+ }
+ if (sync) {
+ data_timestamp = updated_data_timestamp;
+ }
+ }
+
+ /* do time sync */
+ if (first_sync || sync) {
+ map_sensortime_to_rtc_time(_task, chip_time_us, data_timestamp);
+ T(last_sync_time) = now;
+ T(last_sync_data_ts) = data_timestamp;
+ }
+}
+
+static void dispatchData(TASK, const uint8_t *data, uint32_t count)
+{
+ uint64_t ts = 0;
+ uint32_t sample_index = 0;
+ int i;
+
+ if (count == 0) {
+ return;
+ }
+
+ /* do time sync if no flush or fist time starting FIFO */
+ if (T(fifo_start_sync) || !T(flush)) {
+ computeTimeSync(_task, data, count, T(data_timestamp));
+ }
+
+ /* process FIFO data */
+ while (count >= T(config).fifo_sample_size) {
+ for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
+ T(sensors[i]).updated = false;
+ }
+ parseOnePacket(_task, data, sample_index);
+ sample_index += T(config).fifo_sample_size;
+ count -= T(config).fifo_sample_size;
+ // compute timestamp
+ if (!sensortime_to_rtc_time(_task, T(chip_time_us), &ts)) {
+ continue;
+ }
+ // add data
+ for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
+ if (!T(sensors[i]).configed || !T(sensors[i]).updated) {
+ continue;
+ }
+ if (T(sensors[i]).data_cnt % T(sensors[i]).decimator == 0) {
+ if (ts <= T(sensors[i]).prev_rtc_time) {
+ ts = T(sensors[i]).prev_rtc_time + MIN_INCREMENT_TIME_NS;
+#if DBG_TIMESTAMP
+ T(statistics_set).sync_truncate++;
+#endif
+ }
+ pushSensorData(_task, &T(sensors[i]), ts);
+ }
+ }
+ }
+
+ flushAllData();
+}
+
+static void int1Handling(TASK)
+{
+ uint8_t int_status;
+ uint8_t int_status2;
+ uint16_t fifo_count = 0;
+ union EmbeddedDataPoint trigger_axies;
+ const uint8_t *buf;
+
+ switch (T(int_state)) {
+ case INT_READ_STATUS:
+ T(int_state) = INT_PROCESS_STATUS;
+ // read INT status1, status2 and fifo_count registers
+ SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
+ SPI_READ(REG_INT_STATUS2, 1, &T(dataBuffer[1]));
+ SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[2]));
+ T(data_timestamp) = sensorGetTime();
+ spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
+ break;
+ case INT_PROCESS_STATUS:
+ buf = T(dataBuffer[0]);
+ int_status = buf[1];
+ buf = T(dataBuffer[1]);
+ int_status2 = buf[1];
+ buf = T(dataBuffer[2]);
+ fifo_count = buf[2] << 8 | buf[1];
+ // INT status handling
+ if (int_status2 & BIT_INT_STATUS_WOM_XYZ) {
+ if (T(sensors[WOM]).configed) {
+ trigger_axies.idata = ((int_status2 & BIT_INT_STATUS_WOM_X) >> 0) | ((int_status2 & BIT_INT_STATUS_WOM_Y) >> 1) | ((int_status2 & BIT_INT_STATUS_WOM_Z) >> 2);
+ osEnqueueEvt(EVT_SENSOR_ANY_MOTION, trigger_axies.vptr, NULL);
+ }
+ if (T(sensors[NOMO]).configed && T(noMotionTimerHandle)) {
+ timTimerCancel(T(noMotionTimerHandle));
+ T(noMotionTimerHandle) = timTimerSet(ICM40600_NOM_DURATION_NS, 0, 100, noMotionCallback, (void *)&T(sensors[NOMO]), false);
+ }
+ }
+ // FIFO overflow
+ if (int_status & BIT_INT_STATUS_FIFO_FULL) {
+ ERROR_PRINT("fifo overflow\n");
+ }
+ // FIFO WM status handling
+ if ((int_status & BIT_INT_STATUS_FIFO_THS) || T(flush)) {
+ T(int_state) = INT_READ_FIFO_DATA;
+ } else {
+ T(int_state) = INT_DONE;
+ T(fifo_count) = 0;
+ sensorSpiCallback(_task, 0);
+ break;
+ }
+ // fall-through
+ case INT_READ_FIFO_DATA:
+ T(int_state) = INT_DONE;
+ // compute fifo count and delete partial sample
+ fifo_count -= fifo_count % T(config).fifo_sample_size;
+ // read FIFO data
+ T(fifo_count) = fifo_count;
+ if (fifo_count > 0) {
+ SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0]));
+ spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
+ } else {
+ sensorSpiCallback(_task, 0);
+ }
+ break;
+ default:
+ T(int_state) = INT_DONE;
+ T(fifo_count) = 0;
+ sensorSpiCallback(_task, 0);
+ break;
+ }
+}
+
+static void int1Evt(TASK, bool flush)
+{
+ if (trySwitchState(SENSOR_INT_1_HANDLING)) {
+ T(flush) = flush;
+ T(int_state) = INT_READ_STATUS;
+ int1Handling(_task);
+ } else {
+ if (flush) {
+ T(pending_flush) = true;
+ } else {
+ T(pending_int[0]) = true;
+ }
+ }
+}
+
+#define DEC_OPS(power, firmware, rate, flush) \
+ .sensorPower = power, \
+ .sensorFirmwareUpload = firmware, \
+ .sensorSetRate = rate, \
+ .sensorFlush = flush
+
+#define DEC_OPS_CAL_CFG_TEST(power, firmware, rate, flush, cal, cfg, test) \
+ DEC_OPS(power, firmware, rate, flush), \
+ .sensorCalibrate = cal, \
+ .sensorCfgData = cfg, \
+ .sensorSelfTest = test,
+
+#define DEC_OPS_CFG(power, firmware, rate, flush, cfg) \
+ DEC_OPS(power, firmware, rate, flush), \
+ .sensorCfgData = cfg
+
+static const struct SensorOps mSensorOps[NUM_OF_SENSOR] =
+{
+ { DEC_OPS_CAL_CFG_TEST(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor, accCalibration,
+ accCfgData, accSelfTest) },
+ { DEC_OPS_CAL_CFG_TEST(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor, gyrCalibration,
+ gyrCfgData, gyrSelfTest) },
+ { DEC_OPS(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor) },
+ { DEC_OPS(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor) },
+};
+
+static void configEvent(struct ICM40600Sensor *mSensor, struct ConfigStat *ConfigData)
+{
+ TDECL();
+ int i;
+
+ for (i = 0; &T(sensors[i]) != mSensor; i++) ;
+
+ if (ConfigData->enable == 0 && mSensor->powered) {
+ mSensorOps[i].sensorPower(false, (void *)i);
+ } else if (ConfigData->enable == 1 && !mSensor->powered) {
+ mSensorOps[i].sensorPower(true, (void *)i);
+ } else {
+ mSensorOps[i].sensorSetRate(ConfigData->rate, ConfigData->latency, (void *)i);
+ }
+}
+
+static void processPendingEvt(TASK)
+{
+ enum SensorIndex i;
+
+ if (T(pending_int[0])) {
+ T(pending_int[0]) = false;
+ int1Evt(_task, false);
+ return;
+ }
+
+ if (T(pending_flush)) {
+ T(pending_flush) = false;
+ int1Evt(_task, true);
+ return;
+ }
+
+ for (i = 0; i < NUM_OF_SENSOR; i++) {
+ if (T(pending_config[i])) {
+ T(pending_config[i]) = false;
+ configEvent(&T(sensors[i]), &T(sensors[i]).pConfig);
+ return;
+ }
+ }
+
+ if (T(pending_calibration_save)) {
+ T(pending_calibration_save) = !saveCalibration(_task);
+ return;
+ }
+}
+
+static void sensorInit(TASK)
+{
+ const uint8_t *buf;
+
+ switch (T(init_state)) {
+ case RESET_ICM40600:
+ // reset chip and turn on
+ SPI_WRITE(REG_DEVICE_CONFIG, BIT_SOFT_RESET, 100 * 1000);
+ SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
+ T(powered) = false;
+ sensorTurnOn(_task);
+ // set SPI speed register
+ SPI_WRITE(REG_SPI_SPEED, ICM40600_SPI_SPEED_REG_VALUE);
+ T(init_state) = INIT_ICM40600;
+ spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
+ break;
+ case INIT_ICM40600:
+ buf = T(dataBuffer[0]);
+ if (!(buf[1] & BIT_INT_STATUS_RESET_DONE)) {
+ ERROR_PRINT("chip reset failed!\n");
+ }
+ // reconfigure SPI speed
+ T(mode).speed = ICM40600_SPI_SPEED_HZ;
+ // configure interface
+ // i2c disabled
+ // fifo_count: little endian
+ // sensor data: little endian
+ SPI_WRITE(REG_INTF_CONFIG0, BIT_UI_SIFS_DISABLE_I2C);
+ // configure interrupt
+ SPI_WRITE(REG_INT_CONFIG, (INT1_POLARITY << SHIFT_INT1_POLARITY) |
+ (INT1_DRIVE_CIRCUIT << SHIFT_INT1_DRIVE_CIRCUIT) |
+ (INT1_MODE << SHIFT_INT1_MODE));
+ SPI_WRITE(REG_INT_CONFIG1, BIT_INT_ASY_RST_DISABLE); // 9139
+ // static config of sensors
+ SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND);
+ SPI_WRITE(REG_GYRO_CONFIG1, 0x1A); // default reset value
+ SPI_WRITE(REG_ACCEL_CONFIG1, 0x15); // default reset value
+ // turn off FIFO and sensors
+ SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_BYPASS);
+ SPI_WRITE(REG_FIFO_CONFIG1, 0);
+ SPI_WRITE(REG_FIFO_CONFIG2, 0);
+ SPI_WRITE(REG_FIFO_CONFIG3, 0);
+ SPI_WRITE(REG_SMD_CONFIG, BIT_SMD_MODE_OFF);
+ SPI_WRITE(REG_PWR_MGMT_0, BIT_GYRO_MODE_OFF | BIT_ACCEL_MODE_OFF, 200); // 9136
+ sensorTurnOff(_task);
+ T(init_state) = INIT_DONE;
+ spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
+ break;
+ default:
+ break;
+ }
+}
+
+static void handleSpiDoneEvt(const void* evtData)
+{
+ TDECL();
+ struct ICM40600Sensor *mSensor;
+ bool returnIdle = false;
+ uint32_t i;
+ const uint8_t *buf;
+#ifdef ACCEL_CAL_ENABLED
+ bool accelCalNewBiasAvailable;
+ struct TripleAxisDataPoint *sample;
+ float accelCalBiasX, accelCalBiasY, accelCalBiasZ;
+ bool fallThrough;
+#endif
+
+ switch (GET_STATE()) {
+ case SENSOR_BOOT:
+ SET_STATE(SENSOR_VERIFY_ID);
+ SPI_READ(REG_WHO_AM_I, 1, &T(dataBuffer[0]));
+ spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
+ break;
+ case SENSOR_VERIFY_ID:
+ buf = T(dataBuffer[0]);
+ if (buf[1] != WHO_AM_I_ICM40604 && buf[1] != WHO_AM_I_ICM40605) {
+ ERROR_PRINT("chip not found (id=0x%x)\n", buf[1]);
+ break;
+ }
+ INFO_PRINT("chip found (id=0x%x)\n", buf[1]);
+ SET_STATE(SENSOR_INITIALIZING);
+ T(init_state) = RESET_ICM40600;
+ sensorInit(_task);
+ break;
+ case SENSOR_INITIALIZING:
+ if (T(init_state) == INIT_DONE) {
+ for (i = 0; i < NUM_OF_SENSOR; i++) {
+ sensorRegisterInitComplete(T(sensors[i]).handle);
+ }
+ returnIdle = true;
+ } else {
+ sensorInit(_task);
+ }
+ break;
+ case SENSOR_POWERING_UP:
+ mSensor = (struct ICM40600Sensor *)evtData;
+ sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
+ returnIdle = true;
+ break;
+ case SENSOR_POWERING_DOWN:
+ mSensor = (struct ICM40600Sensor *)evtData;
+#ifdef ACCEL_CAL_ENABLED
+ if (mSensor->idx == ACC) {
+ // https://source.android.com/devices/sensors/sensor-types.html
+ // "The bias and scale calibration must only be updated while the sensor is deactivated,
+ // so as to avoid causing jumps in values during streaming."
+ accelCalNewBiasAvailable = accelCalUpdateBias(&T(accel_cal), &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ);
+ // notify HAL about new accel bias calibration
+ if (accelCalNewBiasAvailable) {
+ fallThrough = true;
+ if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
+ // flush existing samples so the bias appears after them
+ flushData(mSensor, EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[ACC].sensorType));
+ // try to allocate another data event and break if unsuccessful
+ if (!allocateDataEvt(mSensor, sensorGetTime())) {
+ fallThrough = false;
+ }
+ }
+ if (fallThrough) {
+ mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
+ mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
+ mSensor->data_evt->samples[0].firstSample.biasSample =
+ mSensor->data_evt->samples[0].firstSample.numSamples;
+ sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
+ sample->x = accelCalBiasX;
+ sample->y = accelCalBiasY;
+ sample->z = accelCalBiasZ;
+ flushData(mSensor, sensorGetMyEventType(mSensorInfo[ACC].biasType));
+ DEBUG_PRINT("send new accel bias\n");
+ allocateDataEvt(mSensor, sensorGetTime());
+ }
+ }
+ }
+#endif
+ sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
+ returnIdle = true;
+ break;
+ case SENSOR_INT_1_HANDLING:
+ if (T(int_state) == INT_DONE) {
+ buf = T(dataBuffer[0]);
+ dispatchData(_task, &buf[1], T(fifo_count));
+ if (T(flush)) {
+ sendFlushEvt();
+ }
+ returnIdle = true;
+ } else {
+ int1Handling(_task);
+ }
+ break;
+ case SENSOR_CONFIG_CHANGING:
+ mSensor = (struct ICM40600Sensor *)evtData;
+ sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency);
+ returnIdle = true;
+ break;
+ case SENSOR_CALIBRATING:
+ mSensor = (struct ICM40600Sensor *)evtData;
+ if (mSensor->idx == ACC) {
+ if (T(calibration_state) == CALIBRATION_DONE) {
+ INFO_PRINT("Accel Calibration done\n");
+ returnIdle = true;
+ } else {
+ calibrationHandling(_task, ACC);
+ }
+ } else if (mSensor->idx == GYR) {
+ if (T(calibration_state) == CALIBRATION_DONE) {
+ INFO_PRINT("Gyro Calibration done\n");
+ returnIdle = true;
+ } else {
+ calibrationHandling(_task, GYR);
+ }
+ }
+ break;
+ case SENSOR_TESTING:
+ mSensor = (struct ICM40600Sensor *)evtData;
+ if (mSensor->idx == ACC) {
+ if (T(selftest_state) == TEST_DONE) {
+ INFO_PRINT("Accel Selftest done\n");
+ returnIdle = true;
+ } else {
+ selfTestHandling(_task, ACC);
+ }
+ } else if (mSensor->idx == GYR) {
+ if (T(selftest_state) == TEST_DONE) {
+ INFO_PRINT("Gyro Selftest done\n");
+ returnIdle = true;
+ } else {
+ selfTestHandling(_task, GYR);
+ }
+ }
+ break;
+ case SENSOR_SAVE_CALIBRATION:
+ returnIdle = true;
+ break;
+ default:
+ break;
+ }
+
+ if (returnIdle) {
+ SET_STATE(SENSOR_IDLE);
+ processPendingEvt(_task);
+ }
+}
+
+#ifdef GYRO_CAL_ENABLED
+static void processMagEvents(TASK, const struct TripleAxisDataEvent *evtData)
+{
+ const struct SensorFirstSample * const first = &evtData->samples[0].firstSample;
+ const struct TripleAxisDataPoint *sample;
+ uint64_t ts = evtData->referenceTime;
+ unsigned int i;
+
+ for (i = 0; i < first->numSamples; ++i) {
+ sample = &evtData->samples[i];
+ if (i > 0) {
+ ts += sample->deltaTime;
+ }
+ gyroCalUpdateMag(&T(gyro_cal), ts, sample->x, sample->y, sample->z);
+ }
+}
+#endif
+
+static void handleEvent(uint32_t evtType, const void* evtData)
+{
+ TDECL();
+
+ switch (evtType) {
+ case EVT_APP_START:
+ SET_STATE(SENSOR_BOOT);
+ osEventUnsubscribe(T(tid), EVT_APP_START);
+#ifdef GYRO_CAL_ENABLED
+ osEventSubscribe(T(tid), EVT_SENSOR_MAG_DATA_RDY);
+#endif
+ // fall through
+ case EVT_SPI_DONE:
+ handleSpiDoneEvt(evtData);
+ break;
+ case EVT_SENSOR_INTERRUPT_1:
+ int1Evt(_task, false);
+ break;
+#ifdef GYRO_CAL_ENABLED
+ case EVT_SENSOR_MAG_DATA_RDY:
+ if (evtData != SENSOR_DATA_EVENT_FLUSH) {
+ processMagEvents(_task, (const struct TripleAxisDataEvent *)evtData);
+ }
+ break;
+#endif
+ default:
+ break;
+ }
+}
+
+static void initSensorStruct(struct ICM40600Sensor *sensor, enum SensorIndex idx)
+{
+ sensor->data_evt = NULL;
+ sensor->rate = 0;
+ sensor->latency = 0;
+ sensor->decimator = 1;
+ sensor->data_cnt = 0;
+ sensor->prev_rtc_time = 0;
+ sensor->skip_sample_cnt = 0;
+ sensor->data[0] = 0;
+ sensor->data[1] = 0;
+ sensor->data[2] = 0;
+ sensor->offset[0] = 0;
+ sensor->offset[1] = 0;
+ sensor->offset[2] = 0;
+ sensor->updated = false;
+ sensor->powered = false;
+ sensor->configed = false;
+ sensor->wait_for_odr = false;
+ sensor->idx = idx;
+ sensor->flush = 0;
+}
+
+static bool startTask(uint32_t task_id)
+{
+ TDECL();
+ enum SensorIndex i;
+ size_t slabSize;
+
+ time_init(_task);
+
+ T(tid) = task_id;
+
+ T(Int1) = gpioRequest(ICM40600_INT1_PIN);
+ T(Irq1) = ICM40600_INT1_IRQ;
+ T(Isr1).func = icm40600Isr1;
+ T(Int1_EN) = false;
+ T(pending_int[0]) = false;
+ T(pending_flush) = false;
+ T(pending_calibration_save) = false;
+
+ T(mode).speed = ICM40600_SPI_DEFAULT_SPEED_HZ;
+ T(mode).bitsPerWord = 8;
+ T(mode).cpol = SPI_CPOL_IDLE_HI;
+ T(mode).cpha = SPI_CPHA_TRAILING_EDGE;
+ T(mode).nssChange = true;
+ T(mode).format = SPI_FORMAT_MSB_FIRST;
+ T(cs) = GPIO_PB(12);
+ T(config).accel_rate = 0;
+ T(config).gyro_rate = 0;
+ T(config).fifo_rate = 0;
+ T(config).wom_threshold = 0;
+ T(config).fifo_watermark = 0;
+ T(config).fifo_sample_size = 0;
+ T(config).accel_on = false;
+ T(config).gyro_on = false;
+ T(config).wom_on = false;
+ T(noMotionTimerHandle) = 0;
+ T(fifo_count) = 0;
+ T(powered) = false;
+ T(flush) = false;
+ T(data_timestamp) = 0;
+ T(chip_time_us) = 0;
+ T(last_sync_time) = 0;
+ T(last_sync_data_ts) = 0;
+ T(chip_timestamp) = 0;
+ T(fifo_start_sync) = false;
+ T(chip_temperature) = TEMP_OFFSET;
+
+ spiMasterRequest(ICM40600_SPI_BUS_ID, &T(spiDev));
+
+ for (i = 0; i < NUM_OF_SENSOR; i++) {
+ initSensorStruct(&T(sensors[i]), i);
+ T(sensors[i]).handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], (void *)i, false);
+ T(pending_config[i]) = false;
+ }
+
+ osEventSubscribe(T(tid), EVT_APP_START);
+
+#ifdef ACCEL_CAL_ENABLED
+ // Init Accel Cal
+ accelCalInit(&T(accel_cal),
+ 800000000, // Stillness Time in ns (0.8s)
+ 5, // Minimum Sample Number
+ 0.00025, // Threshold
+ 15, // nx bucket count
+ 15, // nxb bucket count
+ 15, // ny bucket count
+ 15, // nyb bucket count
+ 15, // nz bucket count
+ 15, // nzb bucket count
+ 15); // nle bucket count
+#endif
+#ifdef GYRO_CAL_ENABLED
+ // Init Gyro Cal
+ gyroCalInit(&T(gyro_cal),
+ SEC_TO_NANOS(5.0f), // Min stillness period = 5.0 seconds
+ SEC_TO_NANOS(5.9f), // Max stillness period = 6.0 seconds (NOTE 1)
+ 0, 0, 0, // Initial bias offset calibration
+ 0, // Time stamp of initial bias calibration
+ SEC_TO_NANOS(1.5f), // Analysis window length = 1.5 seconds
+ 7.5e-5f, // Gyroscope variance threshold [rad/sec]^2
+ 1.5e-5f, // Gyroscope confidence delta [rad/sec]^2
+ 4.5e-3f, // Accelerometer variance threshold [m/sec^2]^2
+ 9.0e-4f, // Accelerometer confidence delta [m/sec^2]^2
+ 5.0f, // Magnetometer variance threshold [uT]^2
+ 1.0f, // Magnetometer confidence delta [uT]^2
+ 0.95f, // Stillness threshold [0,1]
+ 40.0f * MDEG_TO_RAD, // Stillness mean variation limit [rad/sec]
+ 1.5f, // Max temperature delta during stillness [C]
+ true); // Gyro calibration enable
+ // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness
+ // period of 6.0 seconds and avoid buffer boundary conditions that could push
+ // the max stillness to the next multiple of the analysis window length
+ // (i.e., 7.5 seconds).
+#endif
+
+ slabSize = sizeof(struct TripleAxisDataEvent) +
+ MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);
+ // TODO: need to investigate slab items number
+ T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20);
+ if (!T(mDataSlab)) {
+ ERROR_PRINT("slabAllocatorNew() failed\n");
+ return false;
+ }
+ T(mWbufCnt) = 0;
+ T(mRegCnt) = 0;
+ T(spiInUse) = false;
+
+ return true;
+}
+
+static void endTask(void)
+{
+ TDECL();
+
+#ifdef ACCEL_CAL_ENABLED
+ accelCalDestroy(&T(accel_cal));
+#endif
+#ifdef GYRO_CAL_ENABLED
+ gyroCalDestroy(&T(gyro_cal));
+#endif
+ slabAllocatorDestroy(T(mDataSlab));
+ spiMasterRelease(T(spiDev));
+
+ // disable and release interrupt.
+ disableInterrupt(T(Int1), T(Irq1), &T(Isr1));
+ gpioRelease(T(Int1));
+}
+
+INTERNAL_APP_INIT(ICM40600_APP_ID, ICM40600_APP_VERSION, startTask, endTask, handleEvent);
diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c
index 3d7522e..d8e7e87 100644
--- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c
+++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c
@@ -38,7 +38,7 @@
#include <plat/rtc.h>
#include <calibration/accelerometer/accel_cal.h>
#include <calibration/gyroscope/gyro_cal.h>
-#include <calibration/magnetometer/mag_cal.h>
+#include <calibration/magnetometer/mag_cal/mag_cal.h>
#include <calibration/over_temp/over_temp_cal.h>
#include <algos/time_sync.h>
@@ -47,7 +47,7 @@
#include "st_lsm6dsm_ak09916_slave.h"
#include "st_lsm6dsm_lps22hb_slave.h"
-#define LSM6DSM_APP_VERSION 1
+#define LSM6DSM_APP_VERSION 2
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) || defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
#define LSM6DSM_I2C_MASTER_ENABLED 1
@@ -3734,9 +3734,8 @@
(int)(d->inclination * 180 / M_PI + 0.5f));
// Passing local field information to mag calibration routine
-#ifdef DIVERSITY_CHECK_ENABLED
diversityCheckerLocalFieldUpdate(&T(magnCal).diversity_checker, d->strength);
-#endif
+
// TODO: pass local field information to rotation vector sensor.
} else {
ERROR_PRINT("lsm6dsm_magnCfgData: unknown type 0x%04x, size %d", p->type, p->size);
@@ -3850,7 +3849,8 @@
struct LSM6DSMAccelGyroCfgData *cfgData = data;
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- gyroCalSetBias(&T(gyroCal), cfgData->sw[0], cfgData->sw[1], cfgData->sw[2], sensorGetTime());
+ const float dummy_temperature_celsius = 25.0f;
+ gyroCalSetBias(&T(gyroCal), cfgData->sw[0], cfgData->sw[1], cfgData->sw[2], dummy_temperature_celsius, sensorGetTime());
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
DEBUG_PRINT("Gyro hw bias data [LSB]: %ld %ld %ld\n", cfgData->hw[0], cfgData->hw[1], cfgData->hw[2]);
@@ -4263,8 +4263,9 @@
if (gyroCalNewBiasAvailable(&T(gyroCal))) {
float biasTemperature, gyroOffset[3] = { 0.0f, 0.0f, 0.0f };
+ uint64_t calTime;
- gyroCalGetBias(&T(gyroCal), &gyroOffset[0], &gyroOffset[1], &gyroOffset[2], &biasTemperature);
+ gyroCalGetBias(&T(gyroCal), &gyroOffset[0], &gyroOffset[1], &gyroOffset[2], &biasTemperature, &calTime);
if (!samples->firstSample.biasCurrent) {
samples->firstSample.biasCurrent = true;
@@ -5089,73 +5090,95 @@
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
- accelCalInit(&T(accelCal),
- 800000000, /* stillness period [ns] */
- 5, /* minimum sample number */
- 0.00025, /* threshold */
- 15, /* nx bucket count */
- 15, /* nxb bucket count */
- 15, /* ny bucket count */
- 15, /* nyb bucket count */
- 15, /* nz bucket count */
- 15, /* nzb bucket count */
- 15); /* nle bucket count */
+ // Initializes the accelerometer offset calibration algorithm.
+ const struct AccelCalParameters accelCalParameters = {
+ MSEC_TO_NANOS(800), // t0
+ 5, // n_s
+ 15, // fx
+ 15, // fxb
+ 15, // fy
+ 15, // fyb
+ 15, // fz
+ 15, // fzb
+ 15, // fle
+ 0.00025f // th
+ };
+ accelCalInit(&T(accelCal), &accelCalParameters);
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- gyroCalInit(&T(gyroCal),
- 5e9, /* min stillness period [ns] */
- 6e9, /* max stillness period [ns] */
- 0, 0, 0, /* initial bias offset calibration values [rad/sec] */
- 0, /* timestamp of initial bias calibration [ns] */
- 1.5e9, /* analysis window length = 1.5 seconds [ns] */
- 5e-5f, /* gyroscope variance threshold [(rad/sec)^2] */
- 1e-5f, /* gyroscope confidence delta [(rad/sec)^2] */
- 8e-3f, /* accelerometer variance threshold [(m/sec^2)^2] */
- 1.6e-3f, /* accelerometer confidence delta [(m/sec^2)^2] */
- 1.4f, /* magnetometer variance threshold [uT^2] */
- 0.25, /* magnetometer confidence delta [uT^2] */
- 0.95f, /* stillness threshold [0, 1] */
- 40.0e-3f * M_PI / 180.0f, /* stillness mean variation limit [rad/sec] */
- 1.5f, /* maximum temperature deviation during stillness */
- true); /* gyro calibration enable */
+ const struct GyroCalParameters gyroCalParameters = {
+ SEC_TO_NANOS(5), // min_still_duration_nanos
+ SEC_TO_NANOS(5.9f), // max_still_duration_nanos [see, NOTE 1]
+ 0, // calibration_time_nanos
+ SEC_TO_NANOS(1.5f), // window_time_duration_nanos
+ 0, // bias_x
+ 0, // bias_y
+ 0, // bias_z
+ 0.95f, // stillness_threshold
+ MDEG_TO_RAD * 40.0f, // stillness_mean_delta_limit [rad/sec]
+ 5e-5f, // gyro_var_threshold [rad/sec]^2
+ 1e-5f, // gyro_confidence_delta [rad/sec]^2
+ 8e-3f, // accel_var_threshold [m/sec^2]^2
+ 1.6e-3f, // accel_confidence_delta [m/sec^2]^2
+ 1.4f, // mag_var_threshold [uTesla]^2
+ 0.25f, // mag_confidence_delta [uTesla]^2
+ 1.5f, // temperature_delta_limit_celsius
+ true // gyro_calibration_enable
+ };
+ // [NOTE 1]: 'max_still_duration_nanos' is set to 5.9 seconds to achieve a
+ // max stillness period of 6.0 seconds and avoid buffer boundary conditions
+ // that could push the max stillness to the next multiple of the analysis
+ // window length (i.e., 7.5 seconds).
+ gyroCalInit(&T(gyroCal), &gyroCalParameters);
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
- overTempCalInit(&T(overTempCal),
- 5, /* min num of points to enable model update */
- 5000000000, /* min model update interval [ns] */
- 0.75f, /* temperature span of bin method [C] */
- 50.0e-3f * M_PI / 180.0f, /* model fit tolerance [rad/sec/] */
- 172800000000000, /* model data point age limit [ns] */
- 50.0e-3f * M_PI / 180.0f, /* limit for temperature sensitivity [(rad/sec)/C] */
- 3.0f * M_PI / 180.0f, /* limit for model intercept parameter [rad/sec] */
- true); /* over temperature compensation enable */
+ // Initializes the gyroscope over-temperature offset compensation algorithm.
+ const struct OverTempCalParameters gyroOtcParameters = {
+ MSEC_TO_NANOS(500), // min_temp_update_period_nanos
+ DAYS_TO_NANOS(2), // age_limit_nanos
+ 0.75f, // delta_temp_per_bin
+ 40.0f * MDEG_TO_RAD, // jump_tolerance
+ 50.0f * MDEG_TO_RAD, // outlier_limit
+ 80.0f * MDEG_TO_RAD, // temp_sensitivity_limit
+ 3.0e3f * MDEG_TO_RAD, // sensor_intercept_limit
+ 0.1f * MDEG_TO_RAD, // significant_offset_change
+ 5, // min_num_model_pts
+ true // over_temp_enable
+ };
+ overTempCalInit(&T(overTempCal), &gyroOtcParameters);
#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
-#ifdef DIVERSITY_CHECK_ENABLED
- initMagCal(&T(magnCal),
- 0.0f, 0.0f, 0.0f, /* magn offset x - y - z */
- 1.0f, 0.0f, 0.0f, /* magn scale matrix c00 - c01 - c02 */
- 0.0f, 1.0f, 0.0f, /* magn scale matrix c10 - c11 - c12 */
- 0.0f, 0.0f, 1.0f, /* magn scale matrix c20 - c21 - c22 */
- 3000000, /* min_batch_window_in_micros */
- 8, /* min_num_diverse_vectors */
- 1, /* max_num_max_distance */
- 6.0f, /* var_threshold */
- 10.0f, /* max_min_threshold */
- 48.f, /* local_field */
- 0.5f, /* threshold_tuning_param */
- 2.552f); /* max_distance_tuning_param */
-#else
- initMagCal(&T(magnCal),
- 0.0f, 0.0f, 0.0f, /* magn offset x - y - z */
- 1.0f, 0.0f, 0.0f, /* magn scale matrix c00 - c01 - c02 */
- 0.0f, 1.0f, 0.0f, /* magn scale matrix c10 - c11 - c12 */
- 0.0f, 0.0f, 1.0f, /* magn scale matrix c20 - c21 - c22 */
- 3000000); /* min_batch_window_in_micros */
-#endif
+ const struct MagCalParameters magCalParameters = {
+ 3000000, // min_batch_window_in_micros
+ 0.0f, // x_bias
+ 0.0f, // y_bias
+ 0.0f, // z_bias
+ 1.0f, // c00
+ 0.0f, // c01
+ 0.0f, // c02
+ 0.0f, // c10
+ 1.0f, // c11
+ 0.0f, // c12
+ 0.0f, // c20
+ 0.0f, // c21
+ 1.0f // c22
+ };
+
+ // Initializes the magnetometer offset calibration algorithm with diversity
+ // checker.
+ const struct DiversityCheckerParameters magDiversityParameters = {
+ 6.0f, // var_threshold
+ 10.0f, // max_min_threshold
+ 48.0f, // local_field
+ 0.5f, // threshold_tuning_param
+ 2.552f, // max_distance_tuning_param
+ 8, // min_num_diverse_vectors
+ 1 // max_num_max_distance
+ };
+ initMagCal(&T(magnCal), &magCalParameters, &magDiversityParameters);
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
/* Initialize index used to fill/get data from buffer */
diff --git a/firmware/variant/argonkey/argonkey.mk b/firmware/variant/argonkey/argonkey.mk
index 75dc5af..d6f2613 100644
--- a/firmware/variant/argonkey/argonkey.mk
+++ b/firmware/variant/argonkey/argonkey.mk
@@ -31,22 +31,23 @@
SRCS_os += $(VARIANT_PATH)/src/os/spi.c
#keys
-#drivers
-# Fusion algorithms
-SRCS_os += os/algos/fusion.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c
+SRCS_os += \
+ os/algos/calibration/accelerometer/accel_cal.c \
+ os/algos/calibration/gyroscope/gyro_cal.c \
+ os/algos/calibration/gyroscope/gyro_stillness_detect.c \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/calibration/over_temp/over_temp_cal.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c
+#drivers
# LSM6DSL accel+gyro with mag slave
-SRCS_os += os/drivers/st_lsm6dsm/st_lsm6dsm.c \
- os/algos/calibration/accelerometer/accel_cal.c \
- os/algos/calibration/gyroscope/gyro_cal.c \
- os/algos/calibration/gyroscope/gyro_stillness_detect.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/calibration/common/diversity_checker.c \
- os/algos/calibration/over_temp/over_temp_cal.c \
- os/algos/time_sync.c
+SRCS_os += os/drivers/st_lsm6dsm/st_lsm6dsm.c
# Orientation sensor driver
SRCS_os += os/drivers/orientation/orientation.c
diff --git a/firmware/variant/argonkey/inc/variant/sensType.h b/firmware/variant/argonkey/inc/variant/sensType.h
index 23cd0ef..6fca84a 100644
--- a/firmware/variant/argonkey/inc/variant/sensType.h
+++ b/firmware/variant/argonkey/inc/variant/sensType.h
@@ -23,6 +23,7 @@
#define SENS_TYPE_GYRO_BIAS (SENS_TYPE_FIRST_USER + 0)
#define SENS_TYPE_MAG_BIAS (SENS_TYPE_FIRST_USER + 1)
+#define SENS_TYPE_ACCEL_BIAS (SENS_TYPE_FIRST_USER + 2)
#ifdef __cplusplus
}
diff --git a/firmware/variant/argonkey/inc/variant/variant.h b/firmware/variant/argonkey/inc/variant/variant.h
index 62fb4e8..abd92d2 100644
--- a/firmware/variant/argonkey/inc/variant/variant.h
+++ b/firmware/variant/argonkey/inc/variant/variant.h
@@ -84,8 +84,10 @@
#define LSM6DSM_ACCEL_GYRO_ROT_MATRIX -1, 0, 0, 0, -1, 0, 0, 0, 1
#define LSM6DSM_MAGN_ROT_MATRIX -1, 0, 0, 0, 1, 0, 0, 0, 1
+#define LSM6DSM_ACCEL_CALIB_ENABLED 1
#define LSM6DSM_GYRO_CALIB_ENABLED 1
#define LSM6DSM_MAGN_CALIB_ENABLED 1
+#define LSM6DSM_OVERTEMP_CALIB_ENABLED 1
/* ------------------------------------------------------------------------ */
/* sensor baro+temp lps22hb on i2c */
diff --git a/firmware/variant/lunchbox/lunchbox.mk b/firmware/variant/lunchbox/lunchbox.mk
index 4b400eb..90ed90f 100644
--- a/firmware/variant/lunchbox/lunchbox.mk
+++ b/firmware/variant/lunchbox/lunchbox.mk
@@ -30,22 +30,25 @@
SRCS_os += $(VARIANT_PATH)/src/os/spi.c
#keys
+SRCS_os += \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c
+
#drivers
-# ROHM ALS/prox
-SRCS_os += os/drivers/rohm_rpr0521/rohm_rpr0521.c
-
-# Fusion algorithms
-SRCS_os += os/algos/fusion.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c
-
# BMI160 accel and gyro, BMM150 mag drivers
FLAGS += -DUSE_BMM150 -DMAG_SLAVE_PRESENT
-SRCS_os += os/drivers/bosch_bmi160/bosch_bmi160.c \
- os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/time_sync.c
+SRCS_os += \
+ os/drivers/bosch_bmi160/bosch_bmi160.c \
+ os/drivers/bosch_bmi160/bosch_bmm150_slave.c
+
+# ROHM ALS/prox
+SRCS_os += os/drivers/rohm_rpr0521/rohm_rpr0521.c
# Orientation sensor driver
SRCS_os += os/drivers/orientation/orientation.c
diff --git a/firmware/variant/neonkey/neonkey.mk b/firmware/variant/neonkey/neonkey.mk
index b762f92..217f8e6 100644
--- a/firmware/variant/neonkey/neonkey.mk
+++ b/firmware/variant/neonkey/neonkey.mk
@@ -32,7 +32,22 @@
SRCS_os += $(VARIANT_PATH)/src/os/led.c
#keys
+SRCS_os += \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c
+
#drivers
+# Bosch BMI160 accel and gyro, BMM150 mag drivers
+FLAGS += -DUSE_BMM150 -DMAG_SLAVE_PRESENT
+SRCS_os += \
+ os/drivers/bosch_bmi160/bosch_bmi160.c \
+ os/drivers/bosch_bmi160/bosch_bmm150_slave.c
# ROHM ALS/prox
SRCS_os += os/drivers/rohm_rpr0521/rohm_rpr0521.c
@@ -52,23 +67,10 @@
# Bosch BMP280 Barometer/Temperature
SRCS_os += os/drivers/bosch_bmp280/bosch_bmp280.c
-# Bosch BMI160 accel and gyro, BMM150 mag drivers
-FLAGS += -DUSE_BMM150 -DMAG_SLAVE_PRESENT
-SRCS_os += os/drivers/bosch_bmi160/bosch_bmi160.c \
- os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/time_sync.c
-
# Hall effect sensor driver
SRCS_os += os/drivers/hall/hall.c
# Tilt detection
SRCS_os += os/drivers/tilt_detection/tilt_detection.c
-# Fusion algorithm
-SRCS_os += os/algos/fusion.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c
-
$(info included NEONKEY variant)
diff --git a/firmware/variant/nucleo/nucleo.mk b/firmware/variant/nucleo/nucleo.mk
index 2e09dae..eddd644 100644
--- a/firmware/variant/nucleo/nucleo.mk
+++ b/firmware/variant/nucleo/nucleo.mk
@@ -29,29 +29,31 @@
#board configuration shims
SRCS_os += $(VARIANT_PATH)/src/os/i2c.c
SRCS_os += $(VARIANT_PATH)/src/os/spi.c
-
#keys
+SRCS_os += \
+ os/algos/calibration/magnetometer/mag_cal/mag_cal.c \
+ os/algos/calibration/diversity_checker/diversity_checker.c \
+ os/algos/common/math/kasa.c \
+ os/algos/common/math/mat.c \
+ os/algos/common/math/quat.c \
+ os/algos/common/math/vec.c \
+ os/algos/fusion.c \
+ os/algos/time_sync.c
+
#drivers
+# BMI160 accel and gyro, BMM150 mag drivers
+FLAGS += -DUSE_BMM150 -DMAG_SLAVE_PRESENT
+SRCS_os += \
+ os/drivers/bosch_bmi160/bosch_bmi160.c \
+ os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
+
# AMS ALS/prox
SRCS_os += os/drivers/ams_tmd2772/ams_tmd2772.c
# ROHM ALS/prox
SRCS_os += os/drivers/rohm_rpr0521/rohm_rpr0521.c
-# Fusion algorithm
-SRCS_os += os/algos/fusion.c \
- os/algos/common/math/mat.c \
- os/algos/common/math/quat.c \
- os/algos/common/math/vec.c
-
-# BMI160 accel and gyro, BMM150 mag drivers
-FLAGS += -DUSE_BMM150 -DMAG_SLAVE_PRESENT
-SRCS_os += os/drivers/bosch_bmi160/bosch_bmi160.c \
- os/drivers/bosch_bmi160/bosch_bmm150_slave.c \
- os/algos/calibration/magnetometer/mag_cal.c \
- os/algos/time_sync.c
-
# Orientation sensor driver
SRCS_os += os/drivers/orientation/orientation.c
diff --git a/lib/Android.bp b/lib/Android.bp
index 828d29e..58b7126 100644
--- a/lib/Android.bp
+++ b/lib/Android.bp
@@ -1,5 +1,47 @@
+//
+// Copyright (C) 2016 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.
+//
+
cc_library_headers {
name: "libnanohub_common_headers",
vendor_available: true,
export_include_dirs: ["include"],
}
+
+cc_library_static {
+ name: "libnanohub_common",
+ host_supported: true,
+
+ srcs: ["nanohub/nanoapp.c"],
+
+ export_include_dirs: ["include"],
+
+ target: {
+ host: {
+ cflags: [
+ "-DHOST_BUILD",
+ "-DRSA_SUPPORT_PRIV_OP_BIGRAM",
+ ],
+
+ srcs: [
+ "nanohub/aes.c",
+ "nanohub/rsa.c",
+ "nanohub/sha2.c",
+
+ "nanohub/softcrc.c",
+ ],
+ },
+ },
+}
diff --git a/lib/Android.mk b/lib/Android.mk
index 88707e1..c47b6af 100644
--- a/lib/Android.mk
+++ b/lib/Android.mk
@@ -46,29 +46,6 @@
include $(BUILD_NANOHUB_OS_STATIC_LIBRARY)
-include $(CLEAR_VARS)
-
-LOCAL_MODULE := libnanohub_common
-LOCAL_MODULE_TAGS := optional
-LOCAL_SRC_FILES := \
- $(src_files) \
- nanohub/softcrc.c \
- nanohub/nanoapp.c \
-
-LOCAL_CFLAGS := \
- -DHOST_BUILD \
- -DRSA_SUPPORT_PRIV_OP_BIGRAM \
-
-LOCAL_C_INCLUDES := \
- $(src_includes)
-
-LOCAL_EXPORT_C_INCLUDE_DIRS := \
- $(src_includes)
-
-LOCAL_MULTILIB := both
-
-include $(BUILD_HOST_STATIC_LIBRARY)
-
include $(call first-makefiles-under, $(LOCAL_PATH))
src_files :=
diff --git a/lib/include/nanohub/nanohub.h b/lib/include/nanohub/nanohub.h
index 61d1dc8..aba500b 100644
--- a/lib/include/nanohub/nanohub.h
+++ b/lib/include/nanohub/nanohub.h
@@ -30,6 +30,7 @@
#define NANOHUB_VENDOR_GOOGLE UINT64_C(0x476F6F676C) // "Googl"
#define NANOHUB_VENDOR_STMICRO UINT64_C(0x53544d6963) // "STMic"
+#define NANOHUB_VENDOR_INVENSENSE UINT64_C(0x496E76656E) // "Inven"
#define NANOAPP_SIGNED_FLAG 0x1 // contents is signed with one or more signature block(s)
#define NANOAPP_ENCRYPTED_FLAG 0x2 // contents is encrypted with exactly one encryption key
diff --git a/sensorhal/hubconnection.cpp b/sensorhal/hubconnection.cpp
index e43305a..83392f2 100644
--- a/sensorhal/hubconnection.cpp
+++ b/sensorhal/hubconnection.cpp
@@ -815,6 +815,7 @@
&& isSampleIntervalSatisfied(COMMS_SENSOR_MAG_UNCALIBRATED, timestamp)) {
++cnt;
}
+ break;
default:
break;
}
@@ -1439,26 +1440,34 @@
if (isActivitySensor(sensor) && mActivityEventHandler != NULL) {
mActivityEventHandler->OnFlush();
} else {
- struct Flush& flush = mFlushesPending[primary].front();
- memset(&ev, 0x00, sizeof(sensors_event_t));
- ev.version = META_DATA_VERSION;
- ev.timestamp = 0;
- ev.type = SENSOR_TYPE_META_DATA;
- ev.sensor = 0;
- ev.meta_data.what = META_DATA_FLUSH_COMPLETE;
- ev.meta_data.sensor = flush.handle;
+ bool internal = false;
- if (flush.internal) {
- if (flush.handle == COMMS_SENSOR_ACCEL_WRIST_AWARE)
- mLefty.accel = !mLefty.accel;
- else if (flush.handle == COMMS_SENSOR_GYRO_WRIST_AWARE)
- mLefty.gyro = !mLefty.gyro;
- } else
+ {
+ Mutex::Autolock autoLock(mLock);
+ struct Flush& flush = mFlushesPending[primary].front();
+ memset(&ev, 0x00, sizeof(sensors_event_t));
+ ev.version = META_DATA_VERSION;
+ ev.timestamp = 0;
+ ev.type = SENSOR_TYPE_META_DATA;
+ ev.sensor = 0;
+ ev.meta_data.what = META_DATA_FLUSH_COMPLETE;
+ ev.meta_data.sensor = flush.handle;
+
+ if (flush.internal) {
+ internal = true;
+ if (flush.handle == COMMS_SENSOR_ACCEL_WRIST_AWARE)
+ mLefty.accel = !mLefty.accel;
+ else if (flush.handle == COMMS_SENSOR_GYRO_WRIST_AWARE)
+ mLefty.gyro = !mLefty.gyro;
+ }
+
+ if (--flush.count == 0)
+ mFlushesPending[primary].pop_front();
+ }
+
+ if (!internal)
write(&ev, 1);
- if (--flush.count == 0)
- mFlushesPending[primary].pop_front();
-
ALOGV("flushing %d", ev.meta_data.sensor);
}
}
@@ -2300,15 +2309,14 @@
switch (rateLevel) {
case SENSOR_DIRECT_RATE_VERY_FAST:
- // No sensor support VERY_FAST, fall through
+ [[fallthrough]]; // No sensor support VERY_FAST, fall through
case SENSOR_DIRECT_RATE_FAST:
if (handle != COMMS_SENSOR_MAG && handle != COMMS_SENSOR_MAG_UNCALIBRATED) {
return 2500*1000; // 400Hz
}
- // fall through
+ [[fallthrough]];
case SENSOR_DIRECT_RATE_NORMAL:
return 20*1000*1000; // 50 Hz
- // fall through
default:
return INT64_MAX;
}
diff --git a/util/nanoapp_cmd/Android.bp b/util/nanoapp_cmd/Android.bp
new file mode 100644
index 0000000..62aabae
--- /dev/null
+++ b/util/nanoapp_cmd/Android.bp
@@ -0,0 +1,42 @@
+//
+// Copyright (C) 2016 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.
+//
+
+cc_binary {
+ name: "nanoapp_cmd",
+
+ srcs: ["nanoapp_cmd.c"],
+
+ header_libs: [
+ "libnanohub_common_headers",
+ ],
+ include_dirs: [
+ "device/google/contexthub/firmware/os/inc",
+ ],
+
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+ ],
+
+ shared_libs: [
+ "liblog",
+ "libutils",
+ ],
+
+ owner: "google",
+ vendor: true,
+}
diff --git a/util/nanoapp_cmd/Android.mk b/util/nanoapp_cmd/Android.mk
deleted file mode 100644
index 35320eb..0000000
--- a/util/nanoapp_cmd/Android.mk
+++ /dev/null
@@ -1,39 +0,0 @@
-#
-# Copyright (C) 2016 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.
-#
-
-LOCAL_PATH:= $(call my-dir)
-include $(CLEAR_VARS)
-
-LOCAL_SRC_FILES:= nanoapp_cmd.c
-
-LOCAL_C_INCLUDES += \
- $(LOCAL_PATH)/../../firmware/os/inc \
- $(LOCAL_PATH)/../../lib/include
-
-LOCAL_CFLAGS := -Wall -Werror -Wextra
-
-LOCAL_MODULE:= nanoapp_cmd
-
-LOCAL_MODULE_TAGS:= optional
-LOCAL_MODULE_OWNER := google
-LOCAL_PROPRIETARY_MODULE := true
-
-LOCAL_LDLIBS := -llog
-
-LOCAL_SHARED_LIBRARIES := \
- libutils \
-
-include $(BUILD_EXECUTABLE)
diff --git a/util/nanoapp_encr/Android.bp b/util/nanoapp_encr/Android.bp
new file mode 100644
index 0000000..5e9ff1e
--- /dev/null
+++ b/util/nanoapp_encr/Android.bp
@@ -0,0 +1,32 @@
+//
+// Copyright (C) 2016 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.
+//
+
+cc_binary_host {
+ name: "nanoapp_encr",
+
+ srcs: ["nanoapp_encr.c"],
+
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+ "-DHOST_BUILD",
+ "-DBOOTLOADER=",
+ "-DBOOTLOADER_RO=",
+ ],
+
+ static_libs: ["libnanohub_common"],
+}
diff --git a/util/nanoapp_encr/Android.mk b/util/nanoapp_encr/Android.mk
deleted file mode 100644
index 84444e1..0000000
--- a/util/nanoapp_encr/Android.mk
+++ /dev/null
@@ -1,40 +0,0 @@
-#
-# Copyright (C) 2016 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.
-#
-
-LOCAL_PATH := $(call my-dir)
-
-include $(CLEAR_VARS)
-
-
-LOCAL_SRC_FILES := \
- nanoapp_encr.c \
-
-
-LOCAL_CFLAGS := \
- -Wall \
- -Werror \
- -Wextra \
- -DHOST_BUILD \
- -DBOOTLOADER= \
- -DBOOTLOADER_RO= \
-
-LOCAL_STATIC_LIBRARIES := libnanohub_common
-
-LOCAL_MODULE := nanoapp_encr
-
-LOCAL_MODULE_TAGS := optional
-
-include $(BUILD_HOST_EXECUTABLE)
diff --git a/util/nanoapp_postprocess/Android.bp b/util/nanoapp_postprocess/Android.bp
new file mode 100644
index 0000000..6a8400f
--- /dev/null
+++ b/util/nanoapp_postprocess/Android.bp
@@ -0,0 +1,47 @@
+//
+// Copyright (C) 2016 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.
+//
+
+cc_binary_host {
+ name: "nanoapp_postprocess",
+
+ srcs: ["postprocess_elf.c"],
+
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+ ],
+
+ static_libs: [
+ "libnanohub_common",
+
+ // libelf needed for ELF parsing support, libz required by libelf
+ "libelf",
+ "libz",
+ ],
+
+ // Statically linking libc++ so this binary can be copied out of the tree and
+ // still work (needed by dependencies)
+ stl: "libc++_static",
+
+ target: {
+ // libelf is not available in the Mac build as of June 2016, but we currently
+ // only need to use this tool on Linux, so exclude this from non-Linux builds
+ darwin: {
+ enabled: false,
+ },
+ },
+}
diff --git a/util/nanoapp_postprocess/Android.mk b/util/nanoapp_postprocess/Android.mk
deleted file mode 100644
index 9b4539b..0000000
--- a/util/nanoapp_postprocess/Android.mk
+++ /dev/null
@@ -1,45 +0,0 @@
-#
-# Copyright (C) 2016 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.
-#
-
-# libelf is not available in the Mac build as of June 2016, but we currently
-# only need to use this tool on Linux, so exclude this from non-Linux builds
-ifeq ($(HOST_OS),linux)
-
-LOCAL_PATH := $(call my-dir)
-
-include $(CLEAR_VARS)
-
-LOCAL_SRC_FILES := \
- postprocess_elf.c \
-
-LOCAL_CFLAGS := -Wall -Werror -Wextra
-
-LOCAL_STATIC_LIBRARIES := libnanohub_common
-
-LOCAL_MODULE := nanoapp_postprocess
-
-# libelf needed for ELF parsing support, libz required by libelf
-LOCAL_STATIC_LIBRARIES += libelf libz
-
-# Statically linking libc++ so this binary can be copied out of the tree and
-# still work (needed by dependencies)
-LOCAL_CXX_STL := libc++_static
-
-LOCAL_MODULE_TAGS := optional
-
-include $(BUILD_HOST_EXECUTABLE)
-
-endif # linux
diff --git a/util/nanoapp_sign/Android.bp b/util/nanoapp_sign/Android.bp
new file mode 100644
index 0000000..752ba87
--- /dev/null
+++ b/util/nanoapp_sign/Android.bp
@@ -0,0 +1,33 @@
+//
+// Copyright (C) 2016 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.
+//
+
+cc_binary_host {
+ name: "nanoapp_sign",
+
+ srcs: ["nanoapp_sign.c"],
+
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+ "-DRSA_SUPPORT_PRIV_OP_BIGRAM",
+ "-DHOST_BUILD",
+ "-DBOOTLOADER=",
+ "-DBOOTLOADER_RO=",
+ ],
+
+ static_libs: ["libnanohub_common"],
+}
diff --git a/util/nanoapp_sign/Android.mk b/util/nanoapp_sign/Android.mk
deleted file mode 100644
index 7be5197..0000000
--- a/util/nanoapp_sign/Android.mk
+++ /dev/null
@@ -1,44 +0,0 @@
-#
-# Copyright (C) 2016 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.
-#
-
-LOCAL_PATH := $(call my-dir)
-
-include $(CLEAR_VARS)
-
-LOCAL_SRC_FILES := \
- nanoapp_sign.c \
-
-LOCAL_CFLAGS := \
- -Wall \
- -Werror \
- -Wextra \
- -DRSA_SUPPORT_PRIV_OP_BIGRAM \
- -DHOST_BUILD \
- -DBOOTLOADER= \
- -DBOOTLOADER_RO= \
-
-
-ifneq ($(filter userdebug eng,$(TARGET_BUILD_VARIANT)),)
-LOCAL_CFLAGS += -DDEBUG_KEYS
-endif
-
-LOCAL_STATIC_LIBRARIES := libnanohub_common
-
-LOCAL_MODULE := nanoapp_sign
-
-LOCAL_MODULE_TAGS := optional
-
-include $(BUILD_HOST_EXECUTABLE)
diff --git a/util/nanotool/Android.bp b/util/nanotool/Android.bp
new file mode 100644
index 0000000..0dc1ec0
--- /dev/null
+++ b/util/nanotool/Android.bp
@@ -0,0 +1,54 @@
+//
+// Copyright (C) 2016 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.
+//
+
+NANOTOOL_VERSION = "1.2.0"
+
+cc_binary {
+ name: "nanotool",
+
+ srcs: [
+ "androidcontexthub.cpp",
+ "apptohostevent.cpp",
+ "calibrationfile.cpp",
+ "contexthub.cpp",
+ "log.cpp",
+ "logevent.cpp",
+ "nanomessage.cpp",
+ "nanotool.cpp",
+ "resetreasonevent.cpp",
+ "sensorevent.cpp",
+ ],
+
+ // JSON file handling from chinook
+ static_libs: ["libhubutilcommon"],
+ shared_libs: [
+ "liblog",
+ "libstagefright_foundation",
+ "libutils",
+ ],
+
+ cpp_std: "c++11",
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+
+ "-DNANOTOOL_VERSION_STR=\"version " + NANOTOOL_VERSION + "\"",
+ ],
+
+ owner: "google",
+ vendor: true,
+}
diff --git a/util/nanotool/Android.mk b/util/nanotool/Android.mk
deleted file mode 100644
index fdd0295..0000000
--- a/util/nanotool/Android.mk
+++ /dev/null
@@ -1,58 +0,0 @@
-#
-# Copyright (C) 2016 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.
-#
-
-LOCAL_PATH := $(call my-dir)
-
-NANOTOOL_VERSION := 1.2.0
-
-include $(CLEAR_VARS)
-
-LOCAL_SRC_FILES := \
- androidcontexthub.cpp \
- apptohostevent.cpp \
- calibrationfile.cpp \
- contexthub.cpp \
- log.cpp \
- logevent.cpp \
- nanomessage.cpp \
- nanotool.cpp \
- resetreasonevent.cpp \
- sensorevent.cpp
-
-# JSON file handling from chinook
-COMMON_UTILS_DIR := ../common
-LOCAL_SRC_FILES += \
- $(COMMON_UTILS_DIR)/file.cpp \
- $(COMMON_UTILS_DIR)/JSONObject.cpp
-
-LOCAL_C_INCLUDES := \
- $(LOCAL_PATH)/$(COMMON_UTILS_DIR)
-
-LOCAL_SHARED_LIBRARIES := \
- liblog \
- libstagefright_foundation \
- libutils
-
-LOCAL_CFLAGS += -Wall -Werror -Wextra
-LOCAL_CFLAGS += -std=c++11
-LOCAL_CFLAGS += -DNANOTOOL_VERSION_STR='"version $(NANOTOOL_VERSION)"'
-
-LOCAL_MODULE := nanotool
-
-LOCAL_MODULE_TAGS := optional
-LOCAL_MODULE_OWNER := google
-
-include $(BUILD_EXECUTABLE)
diff --git a/util/nanotool/apptohostevent.cpp b/util/nanotool/apptohostevent.cpp
index d7ef575..af9c4cc 100644
--- a/util/nanotool/apptohostevent.cpp
+++ b/util/nanotool/apptohostevent.cpp
@@ -53,7 +53,8 @@
switch (sensor_type) {
case SensorType::Accel:
case SensorType::Gyro:
- if (GetAppId() != kAppIdBoschBmi160Bmm150 && GetAppId() != kAppIdSTMicroLsm6dsm) {
+ if (GetAppId() != kAppIdBoschBmi160Bmm150 && GetAppId() != kAppIdSTMicroLsm6dsm &&
+ GetAppId() != kAppIdInvnIcm40600) {
return false;
}
break;
diff --git a/util/nanotool/apptohostevent.h b/util/nanotool/apptohostevent.h
index 54c61b6..89a78e3 100644
--- a/util/nanotool/apptohostevent.h
+++ b/util/nanotool/apptohostevent.h
@@ -59,6 +59,7 @@
constexpr uint64_t kAppIdVendorGoogle = 0x476f6f676cULL; // "Googl"
constexpr uint64_t kAppIdVendorSTMicro = 0x53544d6963ULL; // "STMic"
+constexpr uint64_t kAppIdVendorInvn = 0x496E76656EULL; // "Inven"
constexpr uint64_t kAppIdBoschBmi160Bmm150 = MakeAppId(kAppIdVendorGoogle, 2);
constexpr uint64_t kAppIdBoschBmp280 = MakeAppId(kAppIdVendorGoogle, 5);
@@ -68,6 +69,7 @@
constexpr uint64_t kAppIdSTMicroLsm6dsm = MakeAppId(kAppIdVendorSTMicro, 0);
constexpr uint64_t kAppIdSTMicroLps22hb = MakeAppId(kAppIdVendorSTMicro, 1);
constexpr uint64_t kAppIdSTMicroMag40 = MakeAppId(kAppIdVendorSTMicro, 3);
+constexpr uint64_t kAppIdInvnIcm40600 = MakeAppId(kAppIdVendorInvn, 2);
constexpr uint64_t kAppIdBridge = MakeAppId(kAppIdVendorGoogle, 50);
diff --git a/util/sensortest/Android.bp b/util/sensortest/Android.bp
new file mode 100644
index 0000000..e1de6ae
--- /dev/null
+++ b/util/sensortest/Android.bp
@@ -0,0 +1,35 @@
+// Copyright (C) 2015 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.
+
+cc_binary {
+ name: "sensortest",
+
+ srcs: ["sensortest.cpp"],
+
+ shared_libs: ["libandroid"],
+
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+ ],
+
+ owner: "google",
+
+ strip: {
+ keep_symbols: true,
+ },
+
+ sdk_version: "current",
+}
diff --git a/util/sensortest/Android.mk b/util/sensortest/Android.mk
deleted file mode 100644
index 0f385e0..0000000
--- a/util/sensortest/Android.mk
+++ /dev/null
@@ -1,39 +0,0 @@
-# Copyright (C) 2015 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.
-
-LOCAL_PATH:= $(call my-dir)
-
-include $(CLEAR_VARS)
-
-LOCAL_SRC_FILES:= \
- sensortest.cpp \
-
-LOCAL_SHARED_LIBRARIES := \
- libandroid \
-
-LOCAL_C_INCLUDES:= \
-
-LOCAL_CFLAGS := -Wall -Werror -Wextra
-
-LOCAL_MODULE_TAGS := optional
-LOCAL_MODULE_OWNER := google
-
-LOCAL_STRIP_MODULE := keep_symbols
-
-LOCAL_MODULE:= sensortest
-
-LOCAL_SDK_VERSION := current
-
-include $(BUILD_EXECUTABLE)
-
diff --git a/util/stm32_flash/Android.bp b/util/stm32_flash/Android.bp
new file mode 100644
index 0000000..18e6f7b
--- /dev/null
+++ b/util/stm32_flash/Android.bp
@@ -0,0 +1,36 @@
+//
+// Copyright (C) 2016 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.
+//
+
+cc_binary {
+ name: "stm32_flash",
+
+ shared_libs: ["libcutils"],
+
+ srcs: [
+ "flash.c",
+ "i2c.c",
+ "spi.c",
+ "stm32_bl.c",
+ "stm32f4_crc.c",
+ "uart.c",
+ ],
+
+ cflags: [
+ "-Wall",
+ "-Werror",
+ "-Wextra",
+ ],
+}
diff --git a/util/stm32_flash/Android.mk b/util/stm32_flash/Android.mk
deleted file mode 100644
index 3063c0c..0000000
--- a/util/stm32_flash/Android.mk
+++ /dev/null
@@ -1,37 +0,0 @@
-#
-# Copyright (C) 2016 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.
-#
-
-LOCAL_PATH := $(call my-dir)
-
-include $(CLEAR_VARS)
-
-LOCAL_SHARED_LIBRARIES := libcutils libc
-
-LOCAL_SRC_FILES := \
- flash.c \
- i2c.c \
- spi.c \
- stm32_bl.c \
- stm32f4_crc.c \
- uart.c
-
-LOCAL_CFLAGS := -Wall -Werror -Wextra
-
-LOCAL_MODULE := stm32_flash
-
-LOCAL_MODULE_TAGS := optional
-
-include $(BUILD_EXECUTABLE)
