nanohub:drivers:st_lsm6dsm: update driver in order to use internal FIFO
This patch introduce a big change of driver architecture. Internal FIFO
is now used to store accel, gyro, magn and timestamp in order to
be able to fully support android batch mode.
Driver is still able to use one single interrupt pad (INT1) to manage all the
interrupts coming from sensor.
Thanks to FIFO driver is now able to support 416Hz as maximum output data
rate and thanks to FIFO decimators it can also achieve 0.81Hz as minimum.
Change-Id: I997c66d504a4a132ed22bdba00da8feb2f79449e
Signed-off-by: Denis Ciocca <denis.ciocca@st.com>
diff --git a/firmware/os/drivers/st_lsm6dsm/README b/firmware/os/drivers/st_lsm6dsm/README
index bd1e1b9..4eaf851 100644
--- a/firmware/os/drivers/st_lsm6dsm/README
+++ b/firmware/os/drivers/st_lsm6dsm/README
@@ -1,57 +1,78 @@
-LSM6DSM device driver for Google nanohub.
-Driver is by default configured to work on STMicroelectronics nucleo board using SPI.
+/*
+ * Copyright (C) 2016-2017 STMicroelectronics
+ *
+ * Author: Denis Ciocca <denis.ciocca@st.com>
+ *
+ * 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.
+ */
+
+ <---------- Introduction ---------->
+
+In order to build the driver for a platform some macros (see Macros list) must
+be defined into variant.h file. [variant/{VARIANT_NAME}/inc/variant/variant.h]
+Those macros define specific configuration that is platform dependent (SPI bus,
+SPI frequency, etc).
+
+Optional FLAGS (FLAGS list) can be used to enable specific features using
+variant makefile. [variant/{VARIANT_NAME}/{VARIANT_NAME}.mk]
+Those flags modify @ compile time the behavior of the driver enabling for
+example i2c master support or calibration libraries.
----------- Default driver configuration ----------
-SPI bus ID : 1 (PB12: SPI_NSS, PB13: SPI_CLK, PB14: SPI_MISO, PB15: SPI_MOSI)
-SPI frequency: 10MHz
-Interrupts: INT1 (PB6), INT2 (unused)
+ <---------- Macros list ---------->
+
+#define LSM6DSM_SPI_SLAVE_BUS_ID 1 /* SPI bus ID, on STM32F4xx indicate SPI2 */
+#define LSM6DSM_SPI_SLAVE_FREQUENCY_HZ 8000000 /* SPI frequency in Hz */
+#define LSM6DSM_SPI_SLAVE_CS_GPIO GPIO_PB(12) /* SPI NSS pin, on STM32F4xx indicate NSS of SPI2 */
+
+#define LSM6DSM_INT_IRQ EXTI2_IRQn
+#define LSM6DSM_INT1_GPIO GPIO_PB(6) /* LSM6DSM INT1 is required, here connected to STM32F4xx PB6 */
+
+#define LSM6DSM_ACCEL_GYRO_ROT_MATRIX 1, 0, 0, 0, 1, 0, 0, 0, 1 /* Accelerometer and gyroscope axis orientation */
+#define LSM6DSM_MAGN_ROT_MATRIX 1, 0, 0, 0, 1, 0, 0, 0, 1 /* Magnetometer axis orientation [MUST be set if a magn sensor is enabled] */
+
+#define LSM6DSM_LIS3MDL_I2C_ADDRESS 0x1e /* STM LIS3MDL I2C address */
+#define LSM6DSM_LPS22HB_I2C_ADDRESS 0x5d /* STM LPS22HB I2C address */
----------- Compiling FLAGS ----------
-LSM6DSM driver supports FLAGS at compile time to enable/disable features.
-It is necessary to modify variant Makefile to include directive to compile the
-driver itself and enable flags.
+ <---------- FLAGS list ---------->
-Supported flags:
-* LSM6DSM_DBG_ENABLED: enable debug messages
-* LSM6DSM_ACCEL_CALIB_ENABLED: enable accelerometer bias calibration library
-* LSM6DSM_GYRO_CALIB_ENABLED: enable gyroscope bias calibration library
-* LSM6DSM_MAGN_CALIB_ENABLED: enable magnetometer calibration library (if magnetometer is enabled)
-* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP: enable internal pull-up resistors for I2C master (if at least one I2C sensor is enabled)
+- LSM6DSM_DBG_ENABLED /* Enable debug messages */
+- LSM6DSM_ACCEL_CALIB_ENABLED /* Enable accelerometer bias calibration */
+- LSM6DSM_GYRO_CALIB_ENABLED /* Enable gyroscope bias calibration */
+- LSM6DSM_OVERTEMP_CALIB_ENABLED /* Enable gyroscope over temperature bias calibration [Require LSM6DSM_GYRO_CALIB_ENABLED] */
+- LSM6DSM_MAGN_CALIB_ENABLED /* Enable magnetometer offset calibration */
-Supported I2C master flags:
+- LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP /* Use internal pull-up resistors for I2C master interface */
--> Magnetometer sensor (only one per time can be used):
-* LSM6DSM_I2C_MASTER_LIS3MDL: enable support for STMicroelectronics LIS3MDL magnetometer sensor
-* LSM6DSM_I2C_MASTER_LSM303AGR: enable support for STMicroelectronics LSM303AGR magnetometer sensor
-* LSM6DSM_I2C_MASTER_AK09916: enable support for AKM AK09916 magnetometer sensor
+/* Magnetometer sensor (only one per time can be used) */
+- LSM6DSM_I2C_MASTER_LIS3MDL /* Enable STM LIS3MDL magn sensor */
+- LSM6DSM_I2C_MASTER_LSM303AGR /* Enable STM LSM303AGR magn sensor */
+- LSM6DSM_I2C_MASTER_AK09916 /* Enable AKM AK09916 magn sensor */
--> Barometer sensor (only one per time can be used):
-* LSM6DSM_I2C_MASTER_LPS22HB: enable support for STMicroelectronics LPS22HB pressure sensor
+/* Barometer sensor (only one per time can be used) */
+- LSM6DSM_I2C_MASTER_LPS22HB /* Enable STM LPS22HB pressure sensor */
-Example: firmware/variant/nucleo/nucleo.mk
+Example: variant/nucleo/nucleo.mk
-FLAGS += -DLSM6DSM_DBG_ENABLED -DLSM6DSM_ACCEL_CALIB_ENABLED -DLSM6DSM_GYRO_CALIB_ENABLED
+FLAGS += -DLSM6DSM_DBG_ENABLED -DLSM6DSM_ACCEL_CALIB_ENABLED -DLSM6DSM_GYRO_CALIB_ENABLED -DLSM6DSM_OVERTEMP_CALIB_ENABLED
FLAGS += -DLSM6DSM_I2C_MASTER_LSM303AGR -DLSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP -DLSM6DSM_MAGN_CALIB_ENABLED
-SRCS_os += os/drivers/st_lsm6dsm/st_lsm6dsm.c
----------- Driver porting ----------
-If sensor is used in different HW setup, here few modifications to apply into driver:
+ <---------- ROT_MATRIX macro explanation ---------->
-Regarding SPI:
-#define LSM6DSM_SPI_SLAVE_BUS_ID 1 /* SPI bus ID, on STM32F4xx indicate SPI2 */
-#define LSM6DSM_SPI_SLAVE_FREQUENCY_HZ 10000000 /* SPI frequency in Hz */
-#define LSM6DSM_SPI_SLAVE_CS_GPIO GPIO_PB(12) /* SPI NSS pin, on STM32F4xx indicate NSS of SPI2 */
-
-Regarding interrupts:
-#define LSM6DSM_INT_IRQ EXTI9_5_IRQn
-#define LSM6DSM_INT1_GPIO GPIO_PB(6) /* LSM6DSM INT1 is required, here connected to STM32F4xx PB6 */
-
-Sensors Orientation:
Sensors orientation can be modified through rotation matrices. Accelerometer and gyroscope are sharing same
-configuration (LSM6DSM_ROT_MATRIX), magnetometer sensor different one (LSM6DSM_MAGN_ROT_MATRIX).
+configuration (LSM6DSM_ACCEL_GYRO_ROT_MATRIX), magnetometer sensor different one (LSM6DSM_MAGN_ROT_MATRIX).
It is following standard rule of matrices moltiplications.
Here an example:
@@ -61,12 +82,13 @@
where x,y,z are sensors output and x",y",z" are android coordinate system data.
Matrix is so defined:
-#define LSM6DSM_ROT_MATRIX r11,r12,r13,r21,r22,r23,r31,r32,r33
+#define LSM6DSM_ACCEL_GYRO_ROT_MATRIX r11,r12,r13,r21,r22,r23,r31,r32,r33
r** can only be: 0 or 1 or -1.
-- Supported features:
+ <---------- Supported features ---------->
+
> Accel & Gyro data;
> Accel bias calibration through accel_cal lib;
> Gyro bias calibration through gyro_cal & gyro_stillnes_detect libs;
diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c
index 634759d..02a1bff 100644
--- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c
+++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm.c
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 STMicroelectronics
+ * Copyright (C) 2016-2017 STMicroelectronics
*
* Author: Denis Ciocca <denis.ciocca@st.com>
*
@@ -37,6 +37,8 @@
#include <calibration/accelerometer/accel_cal.h>
#include <calibration/gyroscope/gyro_cal.h>
#include <calibration/magnetometer/mag_cal.h>
+#include <calibration/over_temp/over_temp_cal.h>
+#include <algos/time_sync.h>
#include "st_lsm6dsm_lis3mdl_slave.h"
#include "st_lsm6dsm_lsm303agr_slave.h"
@@ -44,7 +46,7 @@
#include "st_lsm6dsm_lps22hb_slave.h"
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) || defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
-#define LSM6DSM_I2C_MASTER_ENABLED 1
+#define LSM6DSM_I2C_MASTER_ENABLED 1
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
#if defined(LSM6DSM_MAGN_CALIB_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
@@ -53,345 +55,366 @@
#endif /* LSM6DSM_MAGN_CALIB_ENABLED, LSM6DSM_I2C_MASTER_ENABLED */
#if defined(LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP) && !defined(LSM6DSM_I2C_MASTER_ENABLED)
-#pragma message("LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP has no meaning if no sensors are enabled on I2C master. Discarding!")
+#pragma message("LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP has no meaning if no sensors are enabled on I2C master. Discarding it!")
#endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP, LSM6DSM_I2C_MASTER_ENABLED */
-#define LSM6DSM_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 0)
+#if defined(LSM6DSM_OVERTEMP_CALIB_ENABLED) && !defined(LSM6DSM_GYRO_CALIB_ENABLED)
+#pragma message("LSM6DSM_OVERTEMP_CALIB_ENABLED has no meaning if gyro calibration is not enabled. Discarding it!")
+#undef LSM6DSM_OVERTEMP_CALIB_ENABLED
+#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED, LSM6DSM_GYRO_CALIB_ENABLED */
-#define LSM6DSM_WAI_VALUE (0x6a)
-#define LSM6DSM_RETRY_CNT_WAI 5
-#define LSM6DSM_ACCEL_KSCALE 0.00239364f /* Accel scale @8g in (m/s^2)/LSB */
-#define LSM6DSM_GYRO_KSCALE 0.00122173f /* Gyro scale @2000dps in (rad/sec)/LSB */
-#define LSM6DSM_ONE_SAMPLE_BYTE 6 /* One sample on triaxial sensor generate 6 byte */
-#define LSM6DSM_TEMP_SAMPLE_BYTE 2 /* One sample on temperature sensor generate 2 byte */
-#define LSM6DSM_TEMP_OFFSET (25.0f)
+#define LSM6DSM_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 0)
-/* Sensors orientation */
-#define LSM6DSM_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_WAI_VALUE (0x6a)
+#define LSM6DSM_RETRY_CNT_WAI 5 /* Retry #n times if WAI value is wrong. Maybe HW is not ready after power on */
+#define LSM6DSM_ACCEL_KSCALE 0.00239364f /* Accel scale @8g in (m/s^2)/LSB */
+#define LSM6DSM_GYRO_KSCALE 0.00122173f /* Gyro scale @2000dps in (rad/sec)/LSB */
+#define LSM6DSM_ONE_SAMPLE_BYTE 6 /* One sample of triaxial sensor is expressed on 6 byte */
+#define LSM6DSM_TEMP_SAMPLE_BYTE 2 /* One sample of temperature sensor is expressed on 2 byte */
+#define LSM6DSM_TIMESTAMP_SAMPLE_BYTE 3 /* One sample of timestamp is expressed on 3 byte */
+#define LSM6DSM_TEMP_OFFSET (25.0f)
+#define LSM6DSM_SC_DELTA_TIME_PERIOD_SEC (1.6384f) /* Step counter deltatime resolution */
+#define LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE 15
+#define LSM6DSM_MAX_WATERMARK_VALUE 600 /* 4096byte = 682 samples, use 600 to avoid overflow */
+#define LSM6DSM_TIME_RESOLUTION 25000UL /* 25us [ns] */
+#define LSM6DSM_MASK_24BIT_TIMESTAMP 0x00ffffff /* mask to select 24bit data from 32bit storage data type */
+#define LSM6DSM_TIMEDIFF_OVERFLOW_LSB 8388608LL /* If deltatime is bigger than 2^23 it means timer is overflowed */
+#define LSM6DSM_SYNC_DELTA_INTERVAL 100000000ULL /* Sensor timestamp is synced with MCU every #n deltatime [ns] */
-/* SPI slave connection */
-#define LSM6DSM_SPI_SLAVE_BUS_ID 1
-#define LSM6DSM_SPI_SLAVE_FREQUENCY_HZ 10000000
-#define LSM6DSM_SPI_SLAVE_CS_GPIO GPIO_PB(12)
+/* SPI buffers */
+#define LSM6DSM_SPI_PACKET_SIZE 70
+#define LSM6DSM_SPI_FIFO_SIZE 1024
+#define LSM6DSM_BUF_MARGIN 100
+#define SPI_BUF_SIZE (LSM6DSM_SPI_FIFO_SIZE + LSM6DSM_BUF_MARGIN)
/* LSM6DSM status check registers */
-#define LSM6DSM_STATUS_REG_XLDA (0x01)
-#define LSM6DSM_STATUS_REG_GDA (0x02)
-#define LSM6DSM_STATUS_REG_TDA (0x04)
-#define LSM6DSM_FUNC_SRC_STEP_DETECTED (0x10)
-#define LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA (0x80)
-#define LSM6DSM_FUNC_SRC_SIGN_MOTION (0x40)
-#define LSM6DSM_FUNC_SRC_SENSOR_HUB_END_OP (0x01)
+#define LSM6DSM_FUNC_SRC_STEP_DETECTED (0x10)
+#define LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA (0x80)
+#define LSM6DSM_FUNC_SRC_SIGN_MOTION (0x40)
+#define LSM6DSM_FIFO_STATUS2_FIFO_EMPTY (0x10)
+#define LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART (0x20)
+#define LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN (0x40)
+#define LSM6DSM_FIFO_STATUS2_FIFO_ERROR (LSM6DSM_FIFO_STATUS2_FIFO_EMPTY | \
+ LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART | \
+ LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN)
/* LSM6DSM ODR related */
-#define LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON 80000
-#define LSM6DSM_ODR_12HZ_ACCEL_STD 1
-#define LSM6DSM_ODR_26HZ_ACCEL_STD 1
-#define LSM6DSM_ODR_52HZ_ACCEL_STD 1
-#define LSM6DSM_ODR_104HZ_ACCEL_STD 1
-#define LSM6DSM_ODR_208HZ_ACCEL_STD 1
-#define LSM6DSM_ODR_416HZ_ACCEL_STD 1
-#define LSM6DSM_ODR_12HZ_GYRO_STD 2
-#define LSM6DSM_ODR_26HZ_GYRO_STD 3
-#define LSM6DSM_ODR_52HZ_GYRO_STD 3
-#define LSM6DSM_ODR_104HZ_GYRO_STD 3
-#define LSM6DSM_ODR_208HZ_GYRO_STD 3
-#define LSM6DSM_ODR_416HZ_GYRO_STD 3
+#define LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON 80000
+#define LSM6DSM_ODR_12HZ_ACCEL_STD 1
+#define LSM6DSM_ODR_26HZ_ACCEL_STD 1
+#define LSM6DSM_ODR_52HZ_ACCEL_STD 1
+#define LSM6DSM_ODR_104HZ_ACCEL_STD 1
+#define LSM6DSM_ODR_208HZ_ACCEL_STD 1
+#define LSM6DSM_ODR_416HZ_ACCEL_STD 1
+#define LSM6DSM_ODR_12HZ_GYRO_STD 2
+#define LSM6DSM_ODR_26HZ_GYRO_STD 3
+#define LSM6DSM_ODR_52HZ_GYRO_STD 3
+#define LSM6DSM_ODR_104HZ_GYRO_STD 3
+#define LSM6DSM_ODR_208HZ_GYRO_STD 3
+#define LSM6DSM_ODR_416HZ_GYRO_STD 3
-#define LSM6DSM_ODR_12HZ_REG_VALUE (0x10)
-#define LSM6DSM_ODR_26HZ_REG_VALUE (0x20)
-#define LSM6DSM_ODR_52HZ_REG_VALUE (0x30)
-#define LSM6DSM_ODR_104HZ_REG_VALUE (0x40)
-#define LSM6DSM_ODR_208HZ_REG_VALUE (0x50)
-#define LSM6DSM_ODR_416HZ_REG_VALUE (0x60)
+#define LSM6DSM_ODR_12HZ_REG_VALUE (0x10)
+#define LSM6DSM_ODR_26HZ_REG_VALUE (0x20)
+#define LSM6DSM_ODR_52HZ_REG_VALUE (0x30)
+#define LSM6DSM_ODR_104HZ_REG_VALUE (0x40)
+#define LSM6DSM_ODR_208HZ_REG_VALUE (0x50)
+#define LSM6DSM_ODR_416HZ_REG_VALUE (0x60)
-/* Interrupts */
-#define LSM6DSM_INT_IRQ EXTI9_5_IRQn
-#define LSM6DSM_INT1_GPIO GPIO_PB(6)
-#define LSM6DSM_INT1_INDEX 0
-#define LSM6DSM_INT2_INDEX 1
-#define LSM6DSM_INT_NUM 2
-
-#define LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE (0x01)
-#define LSM6DSM_INT_GYRO_ENABLE_REG_VALUE (0x02)
-#define LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE (0x80)
-#define LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE (0x80)
-#define LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE (0x40)
+#define LSM6DSM_INT_FIFO_FTH_ENABLE_REG_VALUE (0x08)
+#define LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE (0x80)
+#define LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE (0x80)
+#define LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE (0x40)
/* LSM6DSM registers */
-#define LSM6DSM_FUNC_CFG_ACCESS_ADDR (0x01)
-#define LSM6DSM_DRDY_PULSE_CFG_ADDR (0x0b)
-#define LSM6DSM_INT1_CTRL_ADDR (0x0d)
-#define LSM6DSM_INT2_CTRL_ADDR (0x0e)
-#define LSM6DSM_WAI_ADDR (0x0f)
-#define LSM6DSM_CTRL1_XL_ADDR (0x10)
-#define LSM6DSM_CTRL2_G_ADDR (0x11)
-#define LSM6DSM_CTRL3_C_ADDR (0x12)
-#define LSM6DSM_CTRL4_C_ADDR (0x13)
-#define LSM6DSM_EBD_STEP_COUNT_DELTA_ADDR (0x15)
-#define LSM6DSM_CTRL10_C_ADDR (0x19)
-#define LSM6DSM_MASTER_CONFIG_ADDR (0x1a)
-#define LSM6DSM_STATUS_REG_ADDR (0x1e)
-#define LSM6DSM_OUTX_L_G_ADDR (0x22)
-#define LSM6DSM_OUTX_L_XL_ADDR (0x28)
-#define LSM6DSM_OUT_TEMP_L_ADDR (0x20)
-#define LSM6DSM_SENSORHUB1_REG_ADDR (0x2e)
-#define LSM6DSM_STEP_COUNTER_L_ADDR (0x4b)
-#define LSM6DSM_FUNC_SRC_ADDR (0x53)
+#define LSM6DSM_FUNC_CFG_ACCESS_ADDR (0x01)
+#define LSM6DSM_FIFO_CTRL1_ADDR (0x06)
+#define LSM6DSM_FIFO_CTRL5_ADDR (0x0a)
+#define LSM6DSM_DRDY_PULSE_CFG_ADDR (0x0b)
+#define LSM6DSM_INT1_CTRL_ADDR (0x0d)
+#define LSM6DSM_INT2_CTRL_ADDR (0x0e)
+#define LSM6DSM_WAI_ADDR (0x0f)
+#define LSM6DSM_CTRL1_XL_ADDR (0x10)
+#define LSM6DSM_CTRL2_G_ADDR (0x11)
+#define LSM6DSM_CTRL3_C_ADDR (0x12)
+#define LSM6DSM_CTRL4_C_ADDR (0x13)
+#define LSM6DSM_EBD_STEP_COUNT_DELTA_ADDR (0x15)
+#define LSM6DSM_CTRL10_C_ADDR (0x19)
+#define LSM6DSM_MASTER_CONFIG_ADDR (0x1a)
+#define LSM6DSM_STATUS_REG_ADDR (0x1e)
+#define LSM6DSM_OUT_TEMP_L_ADDR (0x20)
+#define LSM6DSM_OUTX_L_G_ADDR (0x22)
+#define LSM6DSM_OUTX_L_XL_ADDR (0x28)
+#define LSM6DSM_OUT_SENSORHUB1_ADDR (0x2e)
+#define LSM6DSM_FIFO_STATUS1_ADDR (0x3a)
+#define LSM6DSM_FIFO_DATA_OUT_L_ADDR (0x3e)
+#define LSM6DSM_TIMESTAMP0_REG_ADDR (0x40)
+#define LSM6DSM_TIMESTAMP2_REG_ADDR (0x42)
+#define LSM6DSM_STEP_COUNTER_L_ADDR (0x4b)
+#define LSM6DSM_FUNC_SRC_ADDR (0x53)
+#define LSM6DSM_WAKE_UP_DUR_ADDR (0x5c)
-#define LSM6DSM_SW_RESET (0x01)
-#define LSM6DSM_RESET_PEDOMETER (0x02)
-#define LSM6DSM_ENABLE_FUNC_CFG_ACCESS (0x80)
-#define LSM6DSM_ENABLE_DIGITAL_FUNC (0x04)
-#define LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC (0x10)
-#define LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC (0x01)
-#define LSM6DSM_ENABLE_TIMER_DIGITAL_FUNC (0x20)
-#define LSM6DSM_MASTER_CONFIG_PULL_UP_EN (0x08)
-#define LSM6DSM_MASTER_CONFIG_MASTER_ON (0x01)
-#define LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1 (0x80)
+#define LSM6DSM_SW_RESET (0x01)
+#define LSM6DSM_RESET_PEDOMETER (0x02)
+#define LSM6DSM_ENABLE_FUNC_CFG_ACCESS (0x80)
+#define LSM6DSM_ENABLE_DIGITAL_FUNC (0x04)
+#define LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC (0x10)
+#define LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC (0x01)
+#define LSM6DSM_MASTER_CONFIG_PULL_UP_EN (0x08)
+#define LSM6DSM_MASTER_CONFIG_MASTER_ON (0x01)
+#define LSM6DSM_ENABLE_FIFO_TIMESTAMP (0x80)
+#define LSM6DSM_TIMESTAMP2_REG_RESET_TIMESTAMP (0xaa)
+
+/* LSM6DSM fifo modes */
+#define LSM6DSM_FIFO_BYPASS_MODE (0x00)
+#define LSM6DSM_FIFO_CONTINUOS_MODE (0x36)
+#define LSM6DSM_FIFO_CTRL2_FTH_MASK (0x07)
+
+/* LSM6DSM fifo decimators */
+#define LSM6DSM_FIFO_SAMPLE_NOT_IN_FIFO (0x00)
+#define LSM6DSM_FIFO_NO_DECIMATION (0x01)
+#define LSM6DSM_FIFO_DECIMATION_FACTOR_2 (0x02)
+#define LSM6DSM_FIFO_DECIMATION_FACTOR_3 (0x03)
+#define LSM6DSM_FIFO_DECIMATION_FACTOR_4 (0x04)
+#define LSM6DSM_FIFO_DECIMATION_FACTOR_8 (0x05)
+#define LSM6DSM_FIFO_DECIMATION_FACTOR_16 (0x06)
+#define LSM6DSM_FIFO_DECIMATION_FACTOR_32 (0x07)
/* LSM6DSM embedded registers */
-#define LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR (0x02)
-#define LSM6DSM_EMBEDDED_SLV0_SUBADDR_ADDR (0x03)
-#define LSM6DSM_EMBEDDED_SLV0_CONFIG_ADDR (0x04)
-#define LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR (0x05)
-#define LSM6DSM_EMBEDDED_SLV1_SUBADDR_ADDR (0x06)
-#define LSM6DSM_EMBEDDED_SLV1_CONFIG_ADDR (0x07)
-#define LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR (0x08)
-#define LSM6DSM_EMBEDDED_SLV2_SUBADDR_ADDR (0x09)
-#define LSM6DSM_EMBEDDED_SLV2_CONFIG_ADDR (0x0a)
-#define LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR (0x0b)
-#define LSM6DSM_EMBEDDED_SLV3_SUBADDR_ADDR (0x0c)
-#define LSM6DSM_EMBEDDED_SLV3_CONFIG_ADDR (0x0d)
-#define LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR (0x0e)
-#define LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR (0x15)
+#define LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR (0x02)
+#define LSM6DSM_EMBEDDED_SLV0_SUBADDR_ADDR (0x03)
+#define LSM6DSM_EMBEDDED_SLV0_CONFIG_ADDR (0x04)
+#define LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR (0x05)
+#define LSM6DSM_EMBEDDED_SLV1_SUBADDR_ADDR (0x06)
+#define LSM6DSM_EMBEDDED_SLV1_CONFIG_ADDR (0x07)
+#define LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR (0x08)
+#define LSM6DSM_EMBEDDED_SLV2_SUBADDR_ADDR (0x09)
+#define LSM6DSM_EMBEDDED_SLV2_CONFIG_ADDR (0x0a)
+#define LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR (0x0b)
+#define LSM6DSM_EMBEDDED_SLV3_SUBADDR_ADDR (0x0c)
+#define LSM6DSM_EMBEDDED_SLV3_CONFIG_ADDR (0x0d)
+#define LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR (0x0e)
+#define LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR (0x15)
-#define LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB (0x01)
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONLY_WRITE (0x00)
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR (0x10)
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR (0x20)
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR (0x30)
-#define LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE (0x20)
-#define LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP (0x07)
+#define LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB (0x01)
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONLY_WRITE (0x00)
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR (0x10)
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR (0x20)
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR (0x30)
+#define LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE (0x20)
+#define LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP (0x07)
/* LSM6DSM I2C master - slave devices */
#ifdef LSM6DSM_I2C_MASTER_LIS3MDL
-#define LSM6DSM_MAGN_KSCALE LIS3MDL_KSCALE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LIS3MDL_I2C_ADDRESS
-#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LIS3MDL_WAI_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LIS3MDL_CTRL2_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LIS3MDL_SW_RESET
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LIS3MDL_CTRL3_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LIS3MDL_CTRL3_BASE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LIS3MDL_POWER_ON_VALUE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LIS3MDL_POWER_OFF_VALUE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LIS3MDL_CTRL1_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LIS3MDL_CTRL1_BASE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LIS3MDL_OUTDATA_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LIS3MDL_OUTDATA_LEN
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LIS3MDLMagnRatesRegValue[i]
+#define LSM6DSM_MAGN_KSCALE LIS3MDL_KSCALE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LIS3MDL_I2C_ADDRESS
+#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LIS3MDL_WAI_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LIS3MDL_CTRL2_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LIS3MDL_SW_RESET
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LIS3MDL_CTRL3_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LIS3MDL_CTRL3_BASE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LIS3MDL_POWER_ON_VALUE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LIS3MDL_POWER_OFF_VALUE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LIS3MDL_CTRL1_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LIS3MDL_CTRL1_BASE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LIS3MDL_OUTDATA_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LIS3MDL_OUTDATA_LEN
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LIS3MDLMagnRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_LIS3MDL */
#ifdef LSM6DSM_I2C_MASTER_AK09916
-#define LSM6DSM_MAGN_KSCALE AK09916_KSCALE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT AK09916_I2C_ADDRESS
-#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR AK09916_WAI_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR AK09916_CNTL3_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE AK09916_SW_RESET
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR AK09916_CNTL2_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE AK09916_CNTL2_BASE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE AK09916_POWER_ON_VALUE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE AK09916_POWER_OFF_VALUE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR AK09916_CNTL2_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE AK09916_CNTL2_BASE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR AK09916_OUTDATA_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN AK09916_OUTDATA_LEN
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) AK09916MagnRatesRegValue[i]
+#define LSM6DSM_MAGN_KSCALE AK09916_KSCALE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT AK09916_I2C_ADDRESS
+#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR AK09916_WAI_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR AK09916_CNTL3_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE AK09916_SW_RESET
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR AK09916_CNTL2_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE AK09916_CNTL2_BASE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE AK09916_POWER_ON_VALUE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE AK09916_POWER_OFF_VALUE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR AK09916_CNTL2_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE AK09916_CNTL2_BASE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR AK09916_OUTDATA_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN AK09916_OUTDATA_LEN
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) AK09916MagnRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
-#define LSM6DSM_MAGN_KSCALE LSM303AGR_KSCALE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LSM303AGR_I2C_ADDRESS
-#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LSM303AGR_WAI_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LSM303AGR_CFG_REG_A_M_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LSM303AGR_SW_RESET
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LSM303AGR_CFG_REG_A_M_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LSM303AGR_CFG_REG_A_M_BASE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LSM303AGR_POWER_ON_VALUE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LSM303AGR_POWER_OFF_VALUE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LSM303AGR_CFG_REG_A_M_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LSM303AGR_CFG_REG_A_M_BASE
-#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LSM303AGR_OUTDATA_ADDR
-#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LSM303AGR_OUTDATA_LEN
-#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LSM303AGRMagnRatesRegValue[i]
+#define LSM6DSM_MAGN_KSCALE LSM303AGR_KSCALE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LSM303AGR_I2C_ADDRESS
+#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LSM303AGR_WAI_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LSM303AGR_CFG_REG_A_M_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LSM303AGR_SW_RESET
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LSM303AGR_CFG_REG_A_M_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LSM303AGR_CFG_REG_A_M_BASE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LSM303AGR_POWER_ON_VALUE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LSM303AGR_POWER_OFF_VALUE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LSM303AGR_CFG_REG_A_M_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LSM303AGR_CFG_REG_A_M_BASE
+#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LSM303AGR_OUTDATA_ADDR
+#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LSM303AGR_OUTDATA_LEN
+#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LSM303AGRMagnRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */
#ifdef LSM6DSM_I2C_MASTER_LPS22HB
-#define LSM6DSM_PRESS_KSCALE LPS22HB_PRESS_KSCALE
-#define LSM6DSM_TEMP_KSCALE LPS22HB_TEMP_KSCALE
-#define LSM6DSM_PRESS_OUTDATA_LEN LPS22HB_OUTDATA_PRESS_BYTE
-#define LSM6DSM_TEMP_OUTDATA_LEN LPS22HB_OUTDATA_TEMP_BYTE
-#define LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT LPS22HB_I2C_ADDRESS
-#define LSM6DSM_SENSOR_SLAVE_BARO_DUMMY_REG_ADDR LPS22HB_WAI_ADDR
-#define LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR LPS22HB_CTRL2_ADDR
-#define LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE LPS22HB_SW_RESET
-#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR LPS22HB_CTRL1_ADDR
-#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE LPS22HB_CTRL1_BASE
-#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ON_VALUE LPS22HB_POWER_ON_VALUE
-#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE LPS22HB_POWER_OFF_VALUE
-#define LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR LPS22HB_CTRL1_ADDR
-#define LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE LPS22HB_CTRL1_BASE
-#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR LPS22HB_OUTDATA_ADDR
-#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN LPS22HB_OUTDATA_LEN
-#define LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i) LPS22HBBaroRatesRegValue[i]
+#define LSM6DSM_PRESS_KSCALE LPS22HB_PRESS_KSCALE
+#define LSM6DSM_TEMP_KSCALE LPS22HB_TEMP_KSCALE
+#define LSM6DSM_PRESS_OUTDATA_LEN LPS22HB_OUTDATA_PRESS_BYTE
+#define LSM6DSM_TEMP_OUTDATA_LEN LPS22HB_OUTDATA_TEMP_BYTE
+#define LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT LPS22HB_I2C_ADDRESS
+#define LSM6DSM_SENSOR_SLAVE_BARO_DUMMY_REG_ADDR LPS22HB_WAI_ADDR
+#define LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR LPS22HB_CTRL2_ADDR
+#define LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE LPS22HB_SW_RESET
+#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR LPS22HB_CTRL1_ADDR
+#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE LPS22HB_CTRL1_BASE
+#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ON_VALUE LPS22HB_POWER_ON_VALUE
+#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE LPS22HB_POWER_OFF_VALUE
+#define LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR LPS22HB_CTRL1_ADDR
+#define LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE LPS22HB_CTRL1_BASE
+#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR LPS22HB_OUTDATA_ADDR
+#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN LPS22HB_OUTDATA_LEN
+#define LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i) LPS22HBBaroRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_LPS22HB */
#ifndef LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN
-#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN 0
+#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN 0
#endif /* LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN */
#ifndef LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN
-#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN 0
+#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN 0
#endif /* LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN */
-#define LSM6DSM_SH_READ_BYTE_NUM (LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN + \
- LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN)
-
-/* SPI buffers */
-#define LSM6DSM_SPI_PACKET_SIZE 70
-#define LSM6DSM_OUTPUT_DATA_READ_SIZE ((2 * LSM6DSM_ONE_SAMPLE_BYTE) + LSM6DSM_SH_READ_BYTE_NUM + 2)
-#define LSM6DSM_BUF_MARGIN 120
-#define SPI_BUF_SIZE (LSM6DSM_OUTPUT_DATA_READ_SIZE + LSM6DSM_BUF_MARGIN)
-
-/* Magn & Baro both enabled */
-#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
-#ifdef LSM6DSM_I2C_MASTER_AK09916
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR
-#else /* LSM6DSM_I2C_MASTER_AK09916 */
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
-#endif /* LSM6DSM_I2C_MASTER_AK09916 */
-#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
-
/* Magn only enabled */
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
#ifdef LSM6DSM_I2C_MASTER_AK09916
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
#else /* LSM6DSM_I2C_MASTER_AK09916 */
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
/* Baro only enabled */
#if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
-#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
+/* Magn & Baro both enabled */
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+#ifdef LSM6DSM_I2C_MASTER_AK09916
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR
+#else /* LSM6DSM_I2C_MASTER_AK09916 */
+#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
+#endif /* LSM6DSM_I2C_MASTER_AK09916 */
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
+
+
/* LSM6DSM default base registers status */
/* LSM6DSM_FUNC_CFG_ACCESS_BASE: enable embedded functions register */
-#define LSM6DSM_FUNC_CFG_ACCESS_BASE (0x00)
+#define LSM6DSM_FUNC_CFG_ACCESS_BASE (0x00)
/* LSM6DSM_DRDY_PULSE_CFG_BASE: enable pulsed interrupt register */
-#define LSM6DSM_DRDY_PULSE_CFG_BASE (0x80)
+#define LSM6DSM_DRDY_PULSE_CFG_BASE (0x00)
/* LSM6DSM_INT1_CTRL_BASE: interrupt 1 control register default settings */
-#define LSM6DSM_INT1_CTRL_BASE ((0 << 7) | /* INT1_STEP_DETECTOR */ \
- (0 << 6) | /* INT1_SIGN_MOT */ \
- (0 << 5) | /* INT1_FULL_FLAG */ \
- (0 << 4) | /* INT1_FIFO_OVR */ \
- (0 << 3) | /* INT1_FTH */ \
- (0 << 2) | /* INT1_BOOT */ \
- (0 << 1) | /* INT1_DRDY_G */ \
- (0 << 0)) /* INT1_DRDY_XL */
+#define LSM6DSM_INT1_CTRL_BASE ((0 << 7) | /* INT1_STEP_DETECTOR */ \
+ (0 << 6) | /* INT1_SIGN_MOT */ \
+ (1 << 5) | /* INT1_FULL_FLAG */ \
+ (1 << 4) | /* INT1_FIFO_OVR */ \
+ (1 << 3) | /* INT1_FTH */ \
+ (0 << 2) | /* INT1_BOOT */ \
+ (0 << 1) | /* INT1_DRDY_G */ \
+ (0 << 0)) /* INT1_DRDY_XL */
/* LSM6DSM_INT2_CTRL_BASE: interrupt 2 control register default settings */
-#define LSM6DSM_INT2_CTRL_BASE ((0 << 7) | /* INT2_STEP_DELTA */ \
- (0 << 6) | /* INT2_STEP_OV */ \
- (0 << 5) | /* INT2_FULL_FLAG */ \
- (0 << 4) | /* INT2_FIFO_OVR */ \
- (0 << 3) | /* INT2_FTH */ \
- (0 << 2) | /* INT2_DRDY_TEMP */ \
- (0 << 1) | /* INT2_DRDY_G */ \
- (0 << 0)) /* INT2_DRDY_XL */
+#define LSM6DSM_INT2_CTRL_BASE ((0 << 7) | /* INT2_STEP_DELTA */ \
+ (0 << 6) | /* INT2_STEP_OV */ \
+ (0 << 5) | /* INT2_FULL_FLAG */ \
+ (0 << 4) | /* INT2_FIFO_OVR */ \
+ (0 << 3) | /* INT2_FTH */ \
+ (0 << 2) | /* INT2_DRDY_TEMP */ \
+ (0 << 1) | /* INT2_DRDY_G */ \
+ (0 << 0)) /* INT2_DRDY_XL */
/* LSM6DSM_CTRL1_XL_BASE: accelerometer sensor register default settings */
-#define LSM6DSM_CTRL1_XL_BASE ((0 << 7) | /* ODR_XL3 */ \
- (0 << 6) | /* ODR_XL2 */ \
- (0 << 5) | /* ODR_XL1 */ \
- (0 << 4) | /* ODR_XL0 */ \
- (1 << 3) | /* FS_XL1 */ \
- (1 << 2) | /* FS_XL0 */ \
- (0 << 1) | /* LPF1_BW_SEL */ \
- (0 << 0)) /* (0) */
+#define LSM6DSM_CTRL1_XL_BASE ((0 << 7) | /* ODR_XL3 */ \
+ (0 << 6) | /* ODR_XL2 */ \
+ (0 << 5) | /* ODR_XL1 */ \
+ (0 << 4) | /* ODR_XL0 */ \
+ (1 << 3) | /* FS_XL1 */ \
+ (1 << 2) | /* FS_XL0 */ \
+ (0 << 1) | /* LPF1_BW_SEL */ \
+ (0 << 0)) /* (0) */
/* LSM6DSM_CTRL2_G_BASE: gyroscope sensor register default settings */
-#define LSM6DSM_CTRL2_G_BASE ((0 << 7) | /* ODR_G3 */ \
- (0 << 6) | /* ODR_G2 */ \
- (0 << 5) | /* ODR_G1 */ \
- (0 << 4) | /* ODR_G0 */ \
- (1 << 3) | /* FS_G1 */ \
- (1 << 2) | /* FS_G0 */ \
- (0 << 1) | /* FS_125 */ \
- (0 << 0)) /* (0) */
+#define LSM6DSM_CTRL2_G_BASE ((0 << 7) | /* ODR_G3 */ \
+ (0 << 6) | /* ODR_G2 */ \
+ (0 << 5) | /* ODR_G1 */ \
+ (0 << 4) | /* ODR_G0 */ \
+ (1 << 3) | /* FS_G1 */ \
+ (1 << 2) | /* FS_G0 */ \
+ (0 << 1) | /* FS_125 */ \
+ (0 << 0)) /* (0) */
/* LSM6DSM_CTRL3_C_BASE: control register 3 default settings */
-#define LSM6DSM_CTRL3_C_BASE ((0 << 7) | /* BOOT */ \
- (1 << 6) | /* BDU */ \
- (0 << 5) | /* H_LACTIVE */ \
- (0 << 4) | /* PP_OD */ \
- (0 << 3) | /* SIM */ \
- (1 << 2) | /* IF_INC */ \
- (0 << 1) | /* BLE */ \
- (0 << 0)) /* SW_RESET */
+#define LSM6DSM_CTRL3_C_BASE ((0 << 7) | /* BOOT */ \
+ (1 << 6) | /* BDU */ \
+ (0 << 5) | /* H_LACTIVE */ \
+ (0 << 4) | /* PP_OD */ \
+ (0 << 3) | /* SIM */ \
+ (1 << 2) | /* IF_INC */ \
+ (0 << 1) | /* BLE */ \
+ (0 << 0)) /* SW_RESET */
/* LSM6DSM_CTRL4_C_BASE: control register 4 default settings */
-#define LSM6DSM_CTRL4_C_BASE ((0 << 7) | /* DEN_XL_EN */ \
- (0 << 6) | /* SLEEP */ \
- (1 << 5) | /* INT2_on_INT1 */ \
- (0 << 4) | /* DEN_DRDY_MASK */ \
- (0 << 3) | /* DRDY_MASK */ \
- (1 << 2) | /* I2C_disable */ \
- (0 << 1) | /* LPF1_SEL_G */ \
- (0 << 0)) /* (0) */
+#define LSM6DSM_CTRL4_C_BASE ((0 << 7) | /* DEN_XL_EN */ \
+ (0 << 6) | /* SLEEP */ \
+ (1 << 5) | /* INT2_on_INT1 */ \
+ (0 << 4) | /* DEN_DRDY_MASK */ \
+ (0 << 3) | /* DRDY_MASK */ \
+ (1 << 2) | /* I2C_disable */ \
+ (0 << 1) | /* LPF1_SEL_G */ \
+ (0 << 0)) /* (0) */
/* LSM6DSM_CTRL10_C_BASE: control register 10 default settings */
-#define LSM6DSM_CTRL10_C_BASE ((0 << 7) | /* (0) */ \
- (0 << 6) | /* (0) */ \
- (0 << 5) | /* TIMER_EN */ \
- (0 << 4) | /* PEDO_EN */ \
- (0 << 3) | /* TILT_EN */ \
- (0 << 2) | /* FUNC_EN */ \
- (0 << 1) | /* PEDO_RST_STEP */ \
- (0 << 0)) /* SIGN_MOTION_EN */
+#define LSM6DSM_CTRL10_C_BASE ((0 << 7) | /* (WRIST_TILT_EN) */ \
+ (0 << 6) | /* (0) */ \
+ (1 << 5) | /* TIMER_EN */ \
+ (0 << 4) | /* PEDO_EN */ \
+ (0 << 3) | /* TILT_EN */ \
+ (1 << 2) | /* FUNC_EN */ \
+ (0 << 1) | /* PEDO_RST_STEP */ \
+ (0 << 0)) /* SIGN_MOTION_EN */
/* LSM6DSM_MASTER_CONFIG_BASE: I2C master configuration register default value */
#ifdef LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP
-#define LSM6DSM_MASTER_CONFIG_BASE (LSM6DSM_MASTER_CONFIG_PULL_UP_EN)
+#define LSM6DSM_MASTER_CONFIG_BASE (LSM6DSM_MASTER_CONFIG_PULL_UP_EN)
#else /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */
-#define LSM6DSM_MASTER_CONFIG_BASE (0x00)
+#define LSM6DSM_MASTER_CONFIG_BASE (0x00)
#endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */
+/* LSM6DSM_WAKE_UP_DUR_BASE: control register WK default settings */
+#define LSM6DSM_WAKE_UP_DUR_BASE (0x10) /* TIMER_HR */
+
#define LSM6DSM_X_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
- ((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \
- (r21 == 1 ? y : (r21 == -1 ? -y : 0)) + \
- (r31 == 1 ? z : (r31 == -1 ? -z : 0)))
+ ((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \
+ (r21 == 1 ? y : (r21 == -1 ? -y : 0)) + \
+ (r31 == 1 ? z : (r31 == -1 ? -z : 0)))
#define LSM6DSM_Y_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
- ((r12 == 1 ? x : (r12 == -1 ? -x : 0)) + \
- (r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \
- (r32 == 1 ? z : (r32 == -1 ? -z : 0)))
+ ((r12 == 1 ? x : (r12 == -1 ? -x : 0)) + \
+ (r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \
+ (r32 == 1 ? z : (r32 == -1 ? -z : 0)))
#define LSM6DSM_Z_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
- ((r13 == 1 ? x : (r13 == -1 ? -x : 0)) + \
- (r23 == 1 ? y : (r23 == -1 ? -y : 0)) + \
- (r33 == 1 ? z : (r33 == -1 ? -z : 0)))
+ ((r13 == 1 ? x : (r13 == -1 ? -x : 0)) + \
+ (r23 == 1 ? y : (r23 == -1 ? -y : 0)) + \
+ (r33 == 1 ? z : (r33 == -1 ? -z : 0)))
-#define LSM6DSM_REMAP_X_DATA(...) LSM6DSM_X_MAP(__VA_ARGS__)
-#define LSM6DSM_REMAP_Y_DATA(...) LSM6DSM_Y_MAP(__VA_ARGS__)
-#define LSM6DSM_REMAP_Z_DATA(...) LSM6DSM_Z_MAP(__VA_ARGS__)
+#define LSM6DSM_REMAP_X_DATA(...) LSM6DSM_X_MAP(__VA_ARGS__)
+#define LSM6DSM_REMAP_Y_DATA(...) LSM6DSM_Y_MAP(__VA_ARGS__)
+#define LSM6DSM_REMAP_Z_DATA(...) LSM6DSM_Z_MAP(__VA_ARGS__)
enum SensorIndex {
- ACCEL = 0,
- GYRO,
+ GYRO = 0,
+ ACCEL,
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
MAGN,
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
@@ -402,7 +425,16 @@
STEP_DETECTOR,
STEP_COUNTER,
SIGN_MOTION,
- NUM_SENSORS
+ NUM_SENSORS,
+ EMBEDDED_TIMESTAMP
+};
+
+enum SensorFifoIndex {
+ FIFO_GYRO,
+ FIFO_ACCEL,
+ FIFO_DS3,
+ FIFO_DS4,
+ FIFO_NUM
};
enum InitState {
@@ -421,7 +453,12 @@
enum SensorEvents {
NO_EVT = -1,
EVT_SPI_DONE = EVT_APP_START + 1,
- EVT_SENSOR_INTERRUPT_1
+ EVT_START_ACCEL_TIME_CALIB,
+ EVT_SENSOR_INTERRUPT_1,
+ EVT_SENSOR_POWERING_UP,
+ EVT_SENSOR_POWERING_DOWN,
+ EVT_SENSOR_CONFIG_CHANGING,
+ EVT_TIME_SYNC
};
enum SensorState {
@@ -432,37 +469,41 @@
SENSOR_POWERING_UP,
SENSOR_POWERING_DOWN,
SENSOR_CONFIG_CHANGING,
+ SENSOR_CONFIG_WATERMARK_CHANGING,
SENSOR_INT1_STATUS_REG_HANDLING,
- SENSOR_INT1_OUTPUT_DATA_HANDLING
+ SENSOR_INT1_OUTPUT_DATA_HANDLING,
+ SENSOR_TIME_SYNC,
+ SENSOR_BARO_READ_DATA,
+ SENSOR_INVALID_STATE
};
static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay);
static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay);
static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay);
-#define SPI_MULTIWRITE_0(addr, data, size) lsm6dsm_spiQueueMultiwrite(addr, data, size, 2)
-#define SPI_MULTIWRITE_1(addr, data, size, delay) lsm6dsm_spiQueueMultiwrite(addr, data, size, delay)
-#define GET_SPI_MULTIWRITE_MACRO(_1, _2, _3, _4, NAME, ...) NAME
-#define SPI_MULTIWRITE(...) GET_SPI_MULTIWRITE_MACRO(__VA_ARGS__, SPI_MULTIWRITE_1, SPI_MULTIWRITE_0)(__VA_ARGS__)
+#define SPI_MULTIWRITE_0(addr, data, size) lsm6dsm_spiQueueMultiwrite(addr, data, size, 2)
+#define SPI_MULTIWRITE_1(addr, data, size, delay) lsm6dsm_spiQueueMultiwrite(addr, data, size, delay)
+#define GET_SPI_MULTIWRITE_MACRO(_1, _2, _3, _4, NAME, ...) NAME
+#define SPI_MULTIWRITE(...) GET_SPI_MULTIWRITE_MACRO(__VA_ARGS__, SPI_MULTIWRITE_1, SPI_MULTIWRITE_0)(__VA_ARGS__)
-#define SPI_WRITE_0(addr, data) lsm6dsm_spiQueueWrite(addr, data, 2)
-#define SPI_WRITE_1(addr, data, delay) lsm6dsm_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_WRITE_0(addr, data) lsm6dsm_spiQueueWrite(addr, data, 2)
+#define SPI_WRITE_1(addr, data, delay) lsm6dsm_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) lsm6dsm_spiQueueRead(addr, size, buf, 0)
-#define SPI_READ_1(addr, size, buf, delay) lsm6dsm_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 SPI_READ_0(addr, size, buf) lsm6dsm_spiQueueRead(addr, size, buf, 0)
+#define SPI_READ_1(addr, size, buf, delay) lsm6dsm_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__)
#ifdef LSM6DSM_I2C_MASTER_ENABLED
static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si);
-#define SPI_WRITE_SS_REGISTER_0(addr, value, accelRate, si) lsm6dsm_writeSlaveRegister(addr, value, accelRate, 0, si)
-#define SPI_WRITE_SS_REGISTER_1(addr, value, accelRate, si, delay) lsm6dsm_writeSlaveRegister(addr, value, accelRate, delay, si)
-#define GET_SPI_WRITE_SS_MACRO(_1, _2, _3, _4, _5, NAME, ...) NAME
-#define SPI_WRITE_SLAVE_SENSOR_REGISTER(...) GET_SPI_WRITE_SS_MACRO(__VA_ARGS__, SPI_WRITE_SS_REGISTER_1, \
- SPI_WRITE_SS_REGISTER_0)(__VA_ARGS__)
+#define SPI_WRITE_SS_REGISTER_0(addr, value, accelRate, si) lsm6dsm_writeSlaveRegister(addr, value, accelRate, 0, si)
+#define SPI_WRITE_SS_REGISTER_1(addr, value, accelRate, si, delay) lsm6dsm_writeSlaveRegister(addr, value, accelRate, delay, si)
+#define GET_SPI_WRITE_SS_MACRO(_1, _2, _3, _4, _5, NAME, ...) NAME
+#define SPI_WRITE_SLAVE_SENSOR_REGISTER(...) GET_SPI_WRITE_SS_MACRO(__VA_ARGS__, SPI_WRITE_SS_REGISTER_1, \
+ SPI_WRITE_SS_REGISTER_0)(__VA_ARGS__)
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
#define INFO_PRINT(fmt, ...) \
@@ -484,25 +525,27 @@
/* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */
#ifndef LSM6DSM_DBG_ENABLED
-#define LSM6DSM_DBG_ENABLED 0
+#define LSM6DSM_DBG_ENABLED 0
#endif /* LSM6DSM_DBG_ENABLED */
-/* struct LSM6DSMSPISlaveInterface: SPI slave data interface
- * @packets: spi packets to perform read/write operations.
+/*
+ * struct LSM6DSMSPISlaveInterface: SPI slave data interface
+ * @packets: spi packets needed to perform read/write operations.
* @txrxBuffer: spi data buffer.
- * @spiDev: spi device pointer.
- * @mode: spi mode (frequency, polarity, etc).
- * @cs: chip select used by SPI slave.
+ * @spiDev: spi device info.
+ * @mode: spi mode info (frequency, polarity, etc).
* @mWbufCnt: counter of total data in spi buffer.
- * @statusRegBuffer: pointer to txrxBuffer to access status register data.
- * @funcSrcBuffer: pointer to txrxBuffer to access func source register data.
- * @tmpDataBuffer: pointer to txrxBuffer to access sporadic read.
- * @accelDataBuffer: pointer to txrxBuffer to access accelerometer data.
- * @gyroDataBuffer: pointer to txrxBuffer to access gyroscope data.
- * @SHDataBuffer: pointer to txrxBuffer to access magnetometer data.
- * @stepCounterDataBuffer: pointer to txrxBuffer to access step counter data.
- * @tempCounterDataBuffer: pointer to txrxBuffer to access temperature data.
+ * @cs: chip select used by SPI slave.
+ * @funcSrcBuffer: pointer of txrxBuffer to access func source register data.
+ * @tmpDataBuffer: pointer of txrxBuffer to access sporadic temp read.
+ * @fifoDataBuffer: pointer of txrxBuffer to access fifo data.
+ * @fifoStatusRegBuffer: pointer of txrxBuffer to access fifo status registers.
+ * @stepCounterDataBuffer: pointer of txrxBuffer to access step counter data.
+ * @tempDataBuffer: pointer of txrxBuffer to access sensor temperature data needed by calibration algos.
+ * @timestampDataBuffer: pointer of txrxBuffer to access sensor timestamp data in order to syncronize time.
+ * @timestampDataBufferBaro: pointer of txrxBuffer to access sensor timestamp data for barometer when not in FIFO.
+ * @baroDataBuffer: pointer of txrx to access barometer data from DSM when not in FIFO.
* @mRegCnt: spi packet num counter.
* @spiInUse: flag used to check if SPI is currently busy.
*/
@@ -511,21 +554,26 @@
uint8_t txrxBuffer[SPI_BUF_SIZE];
struct SpiDevice *spiDev;
struct SpiMode mode;
- spi_cs_t cs;
+
uint16_t mWbufCnt;
- uint8_t *statusRegBuffer;
+
+ spi_cs_t cs;
+
uint8_t *funcSrcBuffer;
uint8_t *tmpDataBuffer;
- uint8_t *accelDataBuffer;
- uint8_t *gyroDataBuffer;
-#ifdef LSM6DSM_I2C_MASTER_ENABLED
- uint8_t *SHDataBuffer;
-#endif /* LSM6DSM_I2C_MASTER_ENABLED */
+ uint8_t *fifoDataBuffer;
+ uint8_t *fifoStatusRegBuffer;
uint8_t *stepCounterDataBuffer;
#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
uint8_t *tempDataBuffer;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
+ uint8_t *timestampDataBuffer;
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ uint8_t *timestampDataBufferBaro;
+ uint8_t *baroDataBuffer;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
uint8_t mRegCnt;
+
bool spiInUse;
};
@@ -541,67 +589,157 @@
bool enable;
};
-/* struct LSM6DSMSensor: sensor status data
+/*
+ * struct LSM6DSMSensor: sensor status data
* @pConfig: temporary data of pending events.
- * @tADataEvt: store three axis sensor data to send to nanohub.
- * @sADataEvt: store one axis sensor data to send to nanohub.
+ * @tADataEvt: three axis sensor data to send to nanohub.
+ * @sADataEvt: one axis sensor data to send to nanohub.
* @latency: current value of latency [n].
+ * @pushedTimestamp: latest sample timestamp pusshed to nanohub.
* @handle: sensor handle obtained by sensorRegister.
- * @rate: current value of rate [Hz * 1024].
+ * @rate: current value of rates based on dependecies [Hz * 1024].
* @hwRate: current value of physical rate [Hz * 1024].
* @idx: enum SensorIndex.
* @samplesToDiscard: samples to discard after enable or odr switch.
- * @samplesDecimator: decimator factor to achieve lower odr not available in hw.
- * @samplesCounter: samples counter by decimation operation.
+ * @samplesDecimator: sw decimator factor to achieve lower odr that cannot be achieved only by FIFO decimator. For example accel is used by dependecies.
+ * @samplesDecimatorCounter: samples counter working together with samplesDecimator.
+ * @samplesFifoDecimator: sw decimator factor to achieve lower odr that cannot be achived by FIFO decimator.
+ * @samplesFifoDecimatorCounter: samples counter working together with sampleFifoDecimator.
+ * @dependenciesRequireData: mask used to verify if dependencies needs data or not. For example accel is used for internal algos.
* enabled: current status of sensor.
*/
struct LSM6DSMSensor {
struct LSM6DSMConfigStatus pConfig;
- struct TripleAxisDataEvent *tADataEvt;
- struct SingleAxisDataEvent *sADataEvt;
+
+ union {
+ struct TripleAxisDataEvent *tADataEvt;
+#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
+ struct SingleAxisDataEvent *sADataEvt;
+#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+ };
+
uint64_t latency;
+ uint64_t pushedTimestamp;
uint32_t handle;
- uint32_t rate;
+ uint32_t rate[NUM_SENSORS];
uint32_t hwRate;
enum SensorIndex idx;
uint8_t samplesToDiscard;
uint8_t samplesDecimator;
- uint8_t samplesCounter;
+ uint8_t samplesDecimatorCounter;
+ uint8_t samplesFifoDecimator;
+ uint8_t samplesFifoDecimatorCounter;
+ bool dependenciesRequireData[NUM_SENSORS];
bool enabled;
};
-/* struct LSM6DSMTask: task data
+/*
+ * struct LSM6DSMFifoCntl: fifo control data
+ * @decimatorsIdx: give who is the sensor that store data in that FIFO slot.
+ * @triggerRate: frequency of FIFO [Hz * 1024].
+ * @watermark: watermark value in #num of samples.
+ * @decimators: fifo decimators value.
+ * @minDecimator: min value of decimators.
+ * @maxDecimator: max value of decimators.
+ * @maxMinDecimator: maxDecimator devided by minDecimator.
+ * @totalSip: total number of samples in one pattern.
+ * @timestampPosition: since timestamp in FIFO is the latest sensor, we need to know where is located during FIFO parsing.
+ */
+struct LSM6DSMFifoCntl {
+ enum SensorIndex decimatorsIdx[FIFO_NUM];
+ uint32_t triggerRate;
+ uint16_t watermark;
+ uint8_t decimators[FIFO_NUM];
+ uint8_t minDecimator;
+ uint8_t maxDecimator;
+ uint8_t maxMinDecimator;
+ uint8_t totalSip;
+ uint8_t timestampPosition[32];
+};
+
+/*
+ * struct LSM6DSMTimeCalibrationWithoutTimer: data used when time calibration is performed during FIFO read.
+ * If latency is smaller than LSM6DSM_SYNC_DELTA_INTERVAL no need to use a timer but we can read timestamp before read FIFO data.
+ * @lastTimestampDataAvlRtcTime: last time we perform a timestamp read from LSM6DSM based on RTC time.
+ * @newTimestampDataAvl: when deltatime is enough we can read again timestamp from LSM6DSM.
+ */
+struct LSM6DSMTimeCalibrationWithoutTimer {
+ uint64_t lastTimestampDataAvlRtcTime;
+ bool newTimestampDataAvl;
+};
+
+enum LSM6DSMTimeCalibrationStatus {
+ TIME_SYNC_DISABLED,
+ TIME_SYNC_TIMER,
+ TIME_SYNC_DURING_FIFO_READ
+};
+
+/*
+ * struct LSM6DSMTimeCalibration: time calibration task data
+ * @sensorTimeToRtcData: timeSync algo data.
+ * @noTimer: if timer is not used to perform time sync, those data will be used.
+ * @lastSampleTimestamp: last sample timestamp from FIFO. Already coverted to RTC time.
+ * @timeSyncRtcTime: Rtc time while performing timestamp read from LSM6DSM.
+ * @sampleTimestampFromFifoLSB: current timestamp from FIFO in LSB. Needs to be stored becasue of overflow.
+ * @timestampSyncTaskLSB: when timer is used to sync time, this is the last timestamp read from LSM6DSM in LSB. Needs to be stored becasue of overflow.
+ * @deltaTimeMarginLSB: is it used to verify if timestamp from FIFO is valid, this is max jitter that timestamp can have from FIFO.
+ * @timestampBaroLSB: if magn and baro are both enabled, barometer data are read with a timer because no slots are available in FIFO. This is the timestamp of baro data.
+ * @theoreticalDeltaTimeLSB: theoretical value of timestamp based on sensor frequency.
+ * @timestampIsValid: flag that indicate if current timestamp parsing FIFO is valid.
+ */
+struct LSM6DSMTimeCalibration {
+ time_sync_t sensorTimeToRtcData;
+ struct LSM6DSMTimeCalibrationWithoutTimer noTimer;
+ uint64_t lastSampleTimestamp;
+ uint64_t timeSyncRtcTime;
+ enum LSM6DSMTimeCalibrationStatus status;
+ uint32_t sampleTimestampFromFifoLSB;
+ uint32_t timestampSyncTaskLSB;
+ uint32_t deltaTimeMarginLSB;
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ uint32_t timestampBaroLSB;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+ uint32_t theoreticalDeltaTimeLSB;
+ bool timestampIsValid;
+};
+
+/* struct LSM6DSMTask: driver task data
* @sensors: sensor status data list.
* @slaveConn: slave interface / communication data.
* @accelCal: accelerometer calibration algo data.
* @gyroCal: gyroscope calibration algo data.
- * @gmagnCal: magnetometer calibration algo data.
+ * @overTempCal: gyroscope over temperature calibration algo data.
+ * @magnCal: magnetometer calibration algo data.
* @int1: int1 gpio data.
* @isr1: isr1 data.
- * @mDataSlabThreeAxis: sensors data memory three axis sensors.
- * @mDataSlabOneAxis: sensors data memory one axis sensors.
- * @currentTemperature: sensors temperature data value used by gyroscope/accelerometer bias calibration libs.
- * @timestampInt: timestamp value from isr.
+ * @mDataSlabThreeAxis: memory used to store three axis sensors data.
+ * @mDataSlabOneAxis: memory used to store one axis sensors data.
+ * @fifoCntl: fifo control data.
+ * @time: time calibration data.
+ * @currentTemperature: sensor temperature data value used by gyroscope/accelerometer bias calibration libs.
+ * @lastFifoReadTimestamp: store when last time FIFO was read.
+ * @initState: initialization is done in several steps (enum InitState).
* @tid: task id.
* @totalNumSteps: total number of steps of step counter sensor.
- * @triggerRate: max ODR value between accel or gyro.
- * @initState: initialization id done in several steps (enum InitState).
+ * @fifoDataToRead: number of byte to read in current FIFO read.
+ * @fifoDataToReadPending: in order to reduce txrxBuffer, FIFO read is performed in several read. This value tell how many data still need to read from FIFO.
+ * @baroTimerId: barometer task timer id.
* @state: task state, driver manage operations using a state machine (enum SensorState).
* @mRetryLeft: counter used to retry operations #n times before return a failure.
- * @statusRegisterDA: acceleromter/gyroscope status register (data available).
- * @statusRegisterTDA: temperature status register (data available).
- * @statusRegisterSH: sensor-hub status register (slave data available).
- * @accelSensorDependencies: dependencies mask of sensors that are using accelerometer.
- * @embeddedFunctionsDependencies: dependencies mask of sensors that are using embedded functions.
- * @int1Register: interrupt 1 register status content (addr: 0x0d).
- * @int2Register: interrupt 2 register status content (addr: 0x0e).
- * @embeddedFunctionsRegister: embedded register status content (addr: 0x19).
- * @masterConfigRegister: i2c master register status content (addr: 0x1a).
- * @newMagnCalibData: this flag indicate if new magnetometer calibration data are available.
+ * @pedometerDependencies: dependencies mask of sensors that are using embedded functions.
+ * @masterConfigDependencies: dependencies mask of sensors that are using I2C master.
+ * @int1Register: interrupt 1 register content (addr: 0x0d).
+ * @int2Register: interrupt 2 register content (addr: 0x0e).
+ * @embeddedFunctionsRegister: embedded register content (addr: 0x19).
+ * @pendingFlush: number of flush requested for each sensor.
+ * @masterConfigRegister: i2c master register content (addr: 0x1a).
* @readSteps: flag used to indicate if interrupt task need to read number of steps.
+ * @sendFlushEvt: if flush is requested, send it out after FIFO read is completed.
* @pendingEnableConfig: pending setEnable operations to be executed.
* @pendingRateConfig: pending setRate operations to be executed.
* @pendingInt: pending interrupt task to be executed.
+ * @pendingTimeSyncTask: pending time sync task to be executed.
+ * @pendingBaroTimerTask: pending baro read data task to be executed.
*/
typedef struct LSM6DSMTask {
struct LSM6DSMSensor sensors[NUM_SENSORS];
@@ -609,17 +747,15 @@
#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
struct AccelCal accelCal;
- struct TripleAxisDataEvent *accelBiasDataEvt;
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
-
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
struct GyroCal gyroCal;
- struct TripleAxisDataEvent *gyroBiasDataEvt;
+#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
+ struct OverTempCal overTempCal;
+#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
-
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
struct MagCal magnCal;
- struct TripleAxisDataEvent *magnCalDataEvt;
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
struct Gpio *int1;
@@ -628,61 +764,62 @@
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
struct SlabAllocator *mDataSlabOneAxis;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+ struct LSM6DSMFifoCntl fifoCntl;
+ struct LSM6DSMTimeCalibration time;
#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
float currentTemperature;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
- uint64_t timestampInt[LSM6DSM_INT_NUM];
+ uint64_t lastFifoReadTimestamp;
+
+ enum InitState initState;
uint32_t tid;
uint32_t totalNumSteps;
- uint32_t triggerRate;
+ uint32_t fifoDataToRead;
+ uint32_t fifoDataToReadPending;
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ uint32_t baroTimerId;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
- enum InitState initState;
volatile uint8_t state;
uint8_t mRetryLeft;
- uint8_t statusRegisterDA;
-#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
- uint8_t statusRegisterTDA;
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
-#ifdef LSM6DSM_I2C_MASTER_ENABLED
- uint8_t statusRegisterSH;
-#endif /* LSM6DSM_I2C_MASTER_ENABLED */
- uint8_t accelSensorDependencies;
- uint8_t embeddedFunctionsDependencies;
+ uint8_t pedometerDependencies;
+ uint8_t masterConfigDependencies;
uint8_t int1Register;
uint8_t int2Register;
uint8_t embeddedFunctionsRegister;
-
+ uint8_t pendingFlush[NUM_SENSORS];
#ifdef LSM6DSM_I2C_MASTER_ENABLED
uint8_t masterConfigRegister;
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
-#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- bool newMagnCalibData;
-#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
-
bool readSteps;
+ bool sendFlushEvt[NUM_SENSORS];
bool pendingEnableConfig[NUM_SENSORS];
bool pendingRateConfig[NUM_SENSORS];
- bool pendingInt[LSM6DSM_INT_NUM];
+ bool pendingInt;
+ bool pendingTimeSyncTask;
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ bool pendingBaroTimerTask;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
} LSM6DSMTask;
static LSM6DSMTask mTask;
-#define TASK LSM6DSMTask* const _task
-#define TDECL() TASK = &mTask; (void)_task
-#define T(v) (_task->v)
-#define T_SLAVE_INTERFACE(v) (_task->slaveConn.v)
+#define TASK LSM6DSMTask* const _task
+#define TDECL() TASK = &mTask; (void)_task
+#define T(v) (_task->v)
+#define T_SLAVE_INTERFACE(v) (_task->slaveConn.v)
-#define BIT(x) (0x01 << x)
-#define SENSOR_HZ_RATE_TO_US(x) (1024000000UL / x)
-#define NS_TO_US(ns) cpuMathU64DivByU16(ns, 1000)
+#define BIT(x) (0x01 << x)
+#define SENSOR_HZ_RATE_TO_US(x) (1024000000UL / x)
+#define NS_TO_US(ns) cpuMathU64DivByU16(ns, 1000)
/* Atomic get state */
-#define GET_STATE() (atomicReadByte(&(_task->state)))
+#define GET_STATE() (atomicReadByte(&(_task->state)))
/* Atomic set state, this set the state to arbitrary value, use with caution */
#define SET_STATE(s) \
@@ -697,7 +834,7 @@
#define trySwitchState(s) trySwitchState_(_task, (s))
static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext);
-#define lsm6dsm_readStatusReg(a) lsm6dsm_readStatusReg_(_task, (a))
+#define lsm6dsm_readStatusReg(a) lsm6dsm_readStatusReg_(_task, (a))
#define DEC_INFO(name, type, axis, inter, samples) \
.sensorName = name, \
@@ -733,30 +870,50 @@
* LSM6DSMGyroRatesSamplesToDiscard must have same length.
*/
static uint32_t LSM6DSMImuRates[] = {
- SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */
- SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */
- SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */
- SENSOR_HZ(26.0f), /* 26Hz */
- SENSOR_HZ(52.0f), /* 52Hz */
- SENSOR_HZ(104.0f), /* 104Hz */
- SENSOR_HZ(208.0f), /* 208Hz */
- SENSOR_HZ(416.0f), /* 416Hz */
+ SENSOR_HZ(26.0f / 32.0f), /* 0.8125Hz */
+ SENSOR_HZ(26.0f / 16.0f), /* 1.625Hz */
+ SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */
+ SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */
+ SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */
+ SENSOR_HZ(26.0f), /* 26Hz */
+ SENSOR_HZ(52.0f), /* 52Hz */
+ SENSOR_HZ(104.0f), /* 104Hz */
+ SENSOR_HZ(208.0f), /* 208Hz */
+ SENSOR_HZ(416.0f), /* 416Hz */
+ 0,
+};
+
+static uint32_t LSM6DSMImuRatesInNs[] = {
+ 1230769230, /* 0.8125Hz */
+ 615384615, /* 1.625Hz */
+ 307692308, /* 3.25Hz */
+ 153846154, /* 6.5Hz */
+ 80000000, /* 12.5Hz */
+ 38461538, /* 26Hz */
+ 19230769, /* 52Hz */
+ 9615385, /* 104Hz */
+ 4807692, /* 208Hz */
+ 2403846, /* 416Hz */
0,
};
static uint8_t LSM6DSMImuRatesRegValue[] = {
- LSM6DSM_ODR_12HZ_REG_VALUE, /* 3.25Hz - do not exist, use 12.5Hz */
- LSM6DSM_ODR_12HZ_REG_VALUE, /* 6.5Hz - do not exist, use 12.5Hz */
- LSM6DSM_ODR_12HZ_REG_VALUE, /* 12.5Hz */
- LSM6DSM_ODR_26HZ_REG_VALUE, /* 26Hz */
- LSM6DSM_ODR_52HZ_REG_VALUE, /* 52Hz */
- LSM6DSM_ODR_104HZ_REG_VALUE, /* 104Hz */
- LSM6DSM_ODR_208HZ_REG_VALUE, /* 208Hz */
- LSM6DSM_ODR_416HZ_REG_VALUE, /* 416Hz */
+ LSM6DSM_ODR_12HZ_REG_VALUE, /* 0.8125Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_REG_VALUE, /* 1.625Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_REG_VALUE, /* 3.25Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_REG_VALUE, /* 6.5Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_REG_VALUE, /* 12.5Hz */
+ LSM6DSM_ODR_26HZ_REG_VALUE, /* 26Hz */
+ LSM6DSM_ODR_52HZ_REG_VALUE, /* 52Hz */
+ LSM6DSM_ODR_104HZ_REG_VALUE, /* 104Hz */
+ LSM6DSM_ODR_208HZ_REG_VALUE, /* 208Hz */
+ LSM6DSM_ODR_416HZ_REG_VALUE, /* 416Hz */
};
/* When sensors switch status from power-down, constant boottime must be considered, some samples should be discarded */
static uint8_t LSM6DSMRatesSamplesToDiscardGyroPowerOn[] = {
+ LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 0.8125Hz - do not exist, use 12.5Hz = 80000us */
+ LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 1.625Hz - do not exist, use 12.5Hz = 80000us */
LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 3.25Hz - do not exist, use 12.5Hz = 80000us */
LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 6.5Hz - do not exist, use 12.5Hz = 80000us */
LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 12.5Hz = 80000us */
@@ -769,42 +926,46 @@
/* When accelerometer change odr but sensor is already on, few samples should be discarded */
static uint8_t LSM6DSMAccelRatesSamplesToDiscard[] = {
- LSM6DSM_ODR_12HZ_ACCEL_STD, /* 3.25Hz - do not exist, use 12.5Hz */
- LSM6DSM_ODR_12HZ_ACCEL_STD, /* 6.5Hz - do not exist, use 12.5Hz */
- LSM6DSM_ODR_12HZ_ACCEL_STD, /* 12.5Hz */
- LSM6DSM_ODR_26HZ_ACCEL_STD, /* 26Hz */
- LSM6DSM_ODR_52HZ_ACCEL_STD, /* 52Hz */
- LSM6DSM_ODR_104HZ_ACCEL_STD, /* 104Hz */
- LSM6DSM_ODR_208HZ_ACCEL_STD, /* 208Hz */
- LSM6DSM_ODR_416HZ_ACCEL_STD, /* 416Hz */
+ LSM6DSM_ODR_12HZ_ACCEL_STD, /* 0.8125Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_ACCEL_STD, /* 1.625Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_ACCEL_STD, /* 3.25Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_ACCEL_STD, /* 6.5Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_ACCEL_STD, /* 12.5Hz */
+ LSM6DSM_ODR_26HZ_ACCEL_STD, /* 26Hz */
+ LSM6DSM_ODR_52HZ_ACCEL_STD, /* 52Hz */
+ LSM6DSM_ODR_104HZ_ACCEL_STD, /* 104Hz */
+ LSM6DSM_ODR_208HZ_ACCEL_STD, /* 208Hz */
+ LSM6DSM_ODR_416HZ_ACCEL_STD, /* 416Hz */
};
/* When gyroscope change odr but sensor is already on, few samples should be discarded */
static uint8_t LSM6DSMGyroRatesSamplesToDiscard[] = {
- LSM6DSM_ODR_12HZ_GYRO_STD, /* 3.25Hz - do not exist, use 12.5Hz */
- LSM6DSM_ODR_12HZ_GYRO_STD, /* 6.5Hz - do not exist, use 12.5Hz */
- LSM6DSM_ODR_12HZ_GYRO_STD, /* 12.5Hz */
- LSM6DSM_ODR_26HZ_GYRO_STD, /* 26Hz */
- LSM6DSM_ODR_52HZ_GYRO_STD, /* 52Hz */
- LSM6DSM_ODR_104HZ_GYRO_STD, /* 104Hz */
- LSM6DSM_ODR_208HZ_GYRO_STD, /* 208Hz */
- LSM6DSM_ODR_416HZ_GYRO_STD, /* 416Hz */
+ LSM6DSM_ODR_12HZ_GYRO_STD, /* 0.8125Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_GYRO_STD, /* 1.625Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_GYRO_STD, /* 3.25Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_GYRO_STD, /* 6.5Hz - do not exist, use 12.5Hz */
+ LSM6DSM_ODR_12HZ_GYRO_STD, /* 12.5Hz */
+ LSM6DSM_ODR_26HZ_GYRO_STD, /* 26Hz */
+ LSM6DSM_ODR_52HZ_GYRO_STD, /* 52Hz */
+ LSM6DSM_ODR_104HZ_GYRO_STD, /* 104Hz */
+ LSM6DSM_ODR_208HZ_GYRO_STD, /* 208Hz */
+ LSM6DSM_ODR_416HZ_GYRO_STD, /* 416Hz */
};
#ifdef LSM6DSM_I2C_MASTER_ENABLED
static uint32_t LSM6DSMSHRates[] = {
- SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */
- SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */
- SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */
- SENSOR_HZ(26.0f), /* 26Hz */
- SENSOR_HZ(52.0f), /* 52Hz */
- SENSOR_HZ(104.0f), /* 104Hz */
+ SENSOR_HZ(26.0f / 32.0f), /* 0.8125Hz */
+ SENSOR_HZ(26.0f / 16.0f), /* 1.625Hz */
+ SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */
+ SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */
+ SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */
+ SENSOR_HZ(26.0f), /* 26Hz */
+ SENSOR_HZ(52.0f), /* 52Hz */
+ SENSOR_HZ(104.0f), /* 104Hz */
0,
};
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
-#define LSM6DSM_SC_DELTA_TIME_PERIOD_SEC (1.6384f)
-
static uint32_t LSM6DSMStepCounterRates[] = {
SENSOR_HZ(1.0f / (128 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 209.715 sec */
SENSOR_HZ(1.0f / (64 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 104.857 sec */
@@ -820,43 +981,43 @@
static const struct SensorInfo LSM6DSMSensorInfo[NUM_SENSORS] = {
{
-#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
- DEC_INFO_RATE_RAW_BIAS("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
- NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE, SENS_TYPE_ACCEL_BIAS)
-#else /* LSM6DSM_ACCEL_CALIB_ENABLED */
- DEC_INFO_RATE_RAW("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
- NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE)
-#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
+#ifdef LSM6DSM_GYRO_CALIB_ENABLED
+ DEC_INFO_RATE_BIAS("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS)
+#else /* LSM6DSM_GYRO_CALIB_ENABLED */
+ DEC_INFO_RATE("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20)
+#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
},
{
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- DEC_INFO_RATE_BIAS("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_GYRO_BIAS)
-#else /* LSM6DSM_GYRO_CALIB_ENABLED */
- DEC_INFO_RATE("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1)
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
+#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
+ DEC_INFO_RATE_RAW_BIAS("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000,
+ SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE, SENS_TYPE_ACCEL_BIAS)
+#else /* LSM6DSM_ACCEL_CALIB_ENABLED */
+ DEC_INFO_RATE_RAW("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000,
+ SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE)
+#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
},
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
{
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- DEC_INFO_RATE_BIAS("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_MAG_BIAS)
+ DEC_INFO_RATE_BIAS("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS)
#else /* LSM6DSM_MAGN_CALIB_ENABLED */
- DEC_INFO_RATE("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1)
+ DEC_INFO_RATE("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 600)
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
},
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
{
- DEC_INFO_RATE("Pressure", LSM6DSMSHRates, SENS_TYPE_BARO, NUM_AXIS_ONE, NANOHUB_INT_NONWAKEUP, 1)
+ DEC_INFO_RATE("Pressure", LSM6DSMSHRates, SENS_TYPE_BARO, NUM_AXIS_ONE, NANOHUB_INT_NONWAKEUP, 300)
},
{
- DEC_INFO_RATE("Temperature", LSM6DSMSHRates, SENS_TYPE_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 1)
+ DEC_INFO_RATE("Temperature", LSM6DSMSHRates, SENS_TYPE_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20)
},
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
{
- DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 1)
+ DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 100)
},
{
- DEC_INFO_RATE("Step Counter", LSM6DSMStepCounterRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 1)
+ DEC_INFO_RATE("Step Counter", LSM6DSMStepCounterRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20)
},
{
DEC_INFO("Significant Motion", SENS_TYPE_SIG_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_WAKEUP, 1)
@@ -917,8 +1078,8 @@
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
static const struct SensorOps LSM6DSMSensorOps[NUM_SENSORS] = {
- { DEC_OPS(lsm6dsm_setAccelPower, lsm6dsm_accelFirmwareUpload, lsm6dsm_setAccelRate, lsm6dsm_accelFlush) },
{ DEC_OPS(lsm6dsm_setGyroPower, lsm6dsm_gyroFirmwareUpload, lsm6dsm_setGyroRate, lsm6dsm_gyroFlush) },
+ { DEC_OPS(lsm6dsm_setAccelPower, lsm6dsm_accelFirmwareUpload, lsm6dsm_setAccelRate, lsm6dsm_accelFlush) },
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
{ DEC_OPS(lsm6dsm_setMagnPower, lsm6dsm_magnFirmwareUpload, lsm6dsm_setMagnRate, lsm6dsm_magnFlush) },
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
@@ -931,12 +1092,21 @@
{ DEC_OPS(lsm6dsm_setSignMotionPower, lsm6dsm_signMotionFirmwareUpload, lsm6dsm_setSignMotionRate, lsm6dsm_signMotionFlush) },
};
+static void lsm6dsm_processPendingEvt(void);
+
+/*
+ * lsm6dsm_spiQueueRead: enqueue a new SPI read that will be performed after lsm6dsm_spiBatchTxRx function is called
+ * @addr: start reading from this register address.
+ * @size: number of byte to read.
+ * @buf: address of pointer where store data.
+ * @delay: wait `delay time' after read is completed. [us]
+ */
static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
{
TDECL();
if (T_SLAVE_INTERFACE(spiInUse)) {
- ERROR_PRINT("SPI in use, cannot queue read (addr=%d len=%d)\n", (int)addr, (int)size);
+ ERROR_PRINT("spiQueueRead: SPI in use, cannot queue read (addr=%x len=%d)\n", addr, (int)size);
return;
}
@@ -952,12 +1122,18 @@
T_SLAVE_INTERFACE(mRegCnt)++;
}
+/*
+ * lsm6dsm_spiQueueWrite: enqueue a new SPI 1-byte write that will be performed after lsm6dsm_spiBatchTxRx function is called
+ * @addr: write byte to this register address.
+ * @data: value to write.
+ * @delay: wait `delay time' after write is completed. [us]
+ */
static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
{
TDECL();
if (T_SLAVE_INTERFACE(spiInUse)) {
- ERROR_PRINT("SPI in use, cannot queue write (addr=%d data=%d)\n", (int)addr, (int)data);
+ ERROR_PRINT("spiQueueWrite: SPI in use, cannot queue 1-byte write (addr=%x data=%x)\n", addr, data);
return;
}
@@ -971,13 +1147,20 @@
T_SLAVE_INTERFACE(mRegCnt)++;
}
+/*
+ * lsm6dsm_spiQueueMultiwrite: enqueue a new SPI n-byte write that will be performed after lsm6dsm_spiBatchTxRx function is called
+ * @addr: start writing from this register address.
+ * @data: array data to write.
+ * @size: number of byte to write.
+ * @delay: wait `delay time' after write is completed. [us]
+ */
static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay)
{
TDECL();
uint8_t i;
if (T_SLAVE_INTERFACE(spiInUse)) {
- ERROR_PRINT("SPI in use, cannot queue multiwrite (addr=%d size=%d)\n", (int)addr, (int)size);
+ ERROR_PRINT("spiQueueMultiwrite: SPI in use, cannot queue multiwrite (addr=%x size=%d)\n", addr, (int)size);
return;
}
@@ -994,19 +1177,25 @@
T_SLAVE_INTERFACE(mRegCnt)++;
}
-static void lsm6dsm_spiBatchTxRx(struct SpiMode *mode,
- SpiCbkF callback, void *cookie, const char *src)
+/*
+ * lsm6dsm_spiBatchTxRx: perform SPI read and/or write enqueued before
+ * @mode: SPI configuration data.
+ * @callback: callback function triggered when all transactions are terminated.
+ * @cookie: private data delivered to callback function.
+ * @src: function name and/or custom string used during print to trace the callstack.
+ */
+static void lsm6dsm_spiBatchTxRx(struct SpiMode *mode, SpiCbkF callback, void *cookie, const char *src)
{
TDECL();
uint8_t regCount;
if (T_SLAVE_INTERFACE(mWbufCnt) > SPI_BUF_SIZE) {
- ERROR_PRINT("No enough SPI buffer space, dropping transaction\n");
+ ERROR_PRINT("spiBatchTxRx: not enough SPI buffer space, dropping transaction. Ref. %s\n", src);
return;
}
if (T_SLAVE_INTERFACE(mRegCnt) > LSM6DSM_SPI_PACKET_SIZE) {
- ERROR_PRINT("spiBatchTxRx too many packets!\n");
+ ERROR_PRINT("spiBatchTxRx: too many packets! Ref. %s\n", src);
return;
}
@@ -1018,15 +1207,35 @@
T_SLAVE_INTERFACE(mWbufCnt) = 0;
if (spiMasterRxTx(T_SLAVE_INTERFACE(spiDev), T_SLAVE_INTERFACE(cs), T_SLAVE_INTERFACE(packets), regCount, mode, callback, cookie)) {
- ERROR_PRINT("spiBatchTxRx failed!\n");
+ ERROR_PRINT("spiBatchTxRx: transaction failed!\n");
}
}
+/*
+ * lsm6dsm_timerCallback: timer callback routine used to retry WAI read
+ * @timerId: timer identificator.
+ * @data: private data delivered to private event handler.
+ */
static void lsm6dsm_timerCallback(uint32_t timerId, void *data)
{
osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid);
}
+/*
+ * lsm6dsm_timerSyncCallback: time syncronization timer callback routine
+ * @timerId: timer identificator.
+ * @data: private data delivered to private event handler.
+ */
+static void lsm6dsm_timerSyncCallback(uint32_t timerId, void *data)
+{
+ osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid);
+}
+
+/*
+ * lsm6dsm_spiCallback: SPI callback function
+ * @cookie: private data from lsm6dsm_spiBatchTxRx function.
+ * @err: error code from SPI transfer.
+ */
static void lsm6dsm_spiCallback(void *cookie, int err)
{
TDECL();
@@ -1035,22 +1244,85 @@
osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid);
}
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+/*
+ * lsm6dsm_baroTimerTask: baro read data task
+ */
+static void lsm6dsm_baroTimerTask(void)
+{
+ TDECL();
+
+ if (trySwitchState(SENSOR_BARO_READ_DATA)) {
+ SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBufferBaro));
+ SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR + LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN,
+ LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN, &T_SLAVE_INTERFACE(baroDataBuffer));
+
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ T(pendingBaroTimerTask) = true;
+
+ return;
+}
+
+/*
+ * lsm6dsm_baroTimerCallback: baro timer callback routine
+ * @timerId: timer identificator.
+ * @data: private data delivered to private event handler.
+ */
+static void lsm6dsm_baroTimerCallback(uint32_t timerId, void *data)
+{
+ lsm6dsm_baroTimerTask();
+}
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+
+/*
+ * lsm6dsm_timeSyncTask: time syncronization task by timer
+ */
+static void lsm6dsm_timeSyncTask(void)
+{
+ TDECL();
+
+ if (T(time).status != TIME_SYNC_TIMER)
+ return;
+
+ if (trySwitchState(SENSOR_TIME_SYNC)) {
+ SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBuffer));
+#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
+ SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer));
+#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
+
+ T(time).timeSyncRtcTime = sensorGetTime();
+
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ T(pendingTimeSyncTask) = true;
+}
+
+/*
+ * lsm6dsm_readStatusReg_: read status registers (interrupt arrived)
+ * @TASK: task id.
+ * @isInterruptContext: function is called directly by ISR.
+ */
static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext)
{
if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
- SPI_READ(LSM6DSM_STATUS_REG_ADDR, 1, &T_SLAVE_INTERFACE(statusRegBuffer));
- SPI_READ(LSM6DSM_FUNC_SRC_ADDR, 1, &T_SLAVE_INTERFACE(funcSrcBuffer));
+ if (T(sensors[STEP_DETECTOR]).enabled || T(sensors[STEP_COUNTER]).enabled || T(sensors[SIGN_MOTION]).enabled)
+ SPI_READ(LSM6DSM_FUNC_SRC_ADDR, 1, &T_SLAVE_INTERFACE(funcSrcBuffer));
+
+ SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
+
lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
} else {
if (isInterruptContext)
osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid));
else
- T(pendingInt[LSM6DSM_INT1_INDEX]) = true;
+ T(pendingInt) = true;
}
}
/*
* lsm6dsm_isr1: INT-1 line service routine
+ * @isr: isr data.
*/
static bool lsm6dsm_isr1(struct ChainedIsr *isr)
{
@@ -1059,7 +1331,6 @@
if (!extiIsPendingGpio(T(int1)))
return false;
- T(timestampInt[LSM6DSM_INT1_INDEX]) = rtcGetTime();
lsm6dsm_readStatusReg(true);
extiClearPendingGpio(T(int1));
@@ -1069,6 +1340,8 @@
/*
* lsm6dsm_enableInterrupt: enable driver interrupt capability
+ * @pin: gpio data.
+ * @isr: isr data.
*/
static void lsm6dsm_enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
@@ -1080,6 +1353,8 @@
/*
* lsm6dsm_disableInterrupt: disable driver interrupt capability
+ * @pin: gpio data.
+ * @isr: isr data.
*/
static void lsm6dsm_disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
@@ -1088,30 +1363,43 @@
}
/*
- * lsm6dsm_writeEmbeddedRegister: write embedded register of sensor
+ * lsm6dsm_writeEmbeddedRegister: write embedded register
+ * @addr: address of register to be written.
+ * @value: value to write.
*/
static void lsm6dsm_writeEmbeddedRegister(uint8_t addr, uint8_t value)
{
TDECL();
+#ifdef LSM6DSM_I2C_MASTER_ENABLED
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
+#endif /* LSM6DSM_I2C_MASTER_ENABLED */
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);
SPI_WRITE(addr, value);
SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
+#ifdef LSM6DSM_I2C_MASTER_ENABLED
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+#endif /* LSM6DSM_I2C_MASTER_ENABLED */
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
}
#ifdef LSM6DSM_I2C_MASTER_ENABLED
/*
* lsm6dsm_writeSlaveRegister: write I2C slave register using sensor-hub feature
+ * @addr: address of register to be written.
+ * @value: value to write.
+ * @accelRate: sensor-hub is using accel odr as trigger. This is current accel odr value.
+ * @delay: perform a delay after write is completed.
+ * @si: which slave sensor needs to be written.
*/
static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si)
{
TDECL();
uint8_t slave_addr, buffer[3];
- uint32_t sh_op_complete_time;
+ uint32_t SHOpCompleteTime;
switch (si) {
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
@@ -1130,11 +1418,12 @@
}
if (accelRate > SENSOR_HZ(104.0f))
- sh_op_complete_time = SENSOR_HZ_RATE_TO_US(SENSOR_HZ(104.0f));
+ SHOpCompleteTime = SENSOR_HZ_RATE_TO_US(SENSOR_HZ(104.0f));
else
- sh_op_complete_time = SENSOR_HZ_RATE_TO_US(accelRate);
+ SHOpCompleteTime = SENSOR_HZ_RATE_TO_US(accelRate);
/* Perform write to slave sensor and wait write is done (1 accel ODR) */
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);
@@ -1145,9 +1434,11 @@
SPI_WRITE(LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR, value);
SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
- SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister), (3 * sh_op_complete_time) / 2);
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+ SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister), (3 * SHOpCompleteTime) / 2);
/* After write is completed slave 0 must be set to sleep mode */
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);
@@ -1157,12 +1448,14 @@
SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 3);
SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
}
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
/*
* lsm6dsm_computeOdr: get index of LSM6DSMImuRates array based on selected rate
+ * @rate: ODR value expressed in SENSOR_HZ(x).
*/
static uint8_t lsm6dsm_computeOdr(uint32_t rate)
{
@@ -1172,8 +1465,8 @@
if (LSM6DSMImuRates[i] == rate)
break;
}
- if (i == (ARRAY_SIZE(LSM6DSMImuRates) -1 )) {
- ERROR_PRINT("ODR not valid! Selected smallest ODR available\n");
+ if (i == (ARRAY_SIZE(LSM6DSMImuRates) - 1)) {
+ ERROR_PRINT("computeOdr: ODR not valid! Selected smallest ODR available\n");
i = 0;
}
@@ -1181,144 +1474,398 @@
}
/*
- * lsm6dsm_getAccelHwMinOdr: verify minimum odr needed by accel in order to satisfy dependencies
+ * lsm6dsm_sensorHzToNs: return delta time of specifi sensor rate
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
*/
-static uint8_t lsm6dsm_getAccelHwMinOdr()
+static uint32_t lsm6dsm_sensorHzToNs(uint32_t rate)
{
- TDECL();
- uint32_t minRate = SENSOR_HZ(26.0f / 2.0f);
+ int i;
- if (T(accelSensorDependencies) & BIT(ACCEL)) {
- if (minRate < T(sensors[ACCEL]).rate)
- minRate = T(sensors[ACCEL]).rate;
+ for (i = 0; i < (ARRAY_SIZE(LSM6DSMImuRates) - 1); i++) {
+ if (LSM6DSMImuRates[i] == rate)
+ break;
+ }
+ if (i == (ARRAY_SIZE(LSM6DSMImuRates) - 1)) {
+ ERROR_PRINT("sensorHzToNs: rate not available. Selected smaller rate\n");
+ i = 0;
}
- /* Embedded functions are enabled, min odr required is 26Hz */
- if (T(embeddedFunctionsDependencies)) {
- if (minRate < SENSOR_HZ(26.0f))
- minRate = SENSOR_HZ(26.0f);
- }
-
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- if (T(accelSensorDependencies) & BIT(GYRO)) {
- if (minRate < T(sensors[GYRO].rate))
- minRate = T(sensors[GYRO].rate);
- }
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
-
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- if (T(accelSensorDependencies) & BIT(MAGN)) {
- if (minRate < T(sensors[MAGN].rate))
- minRate = T(sensors[MAGN].rate);
- }
-#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
-
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- if (T(accelSensorDependencies) & BIT(PRESS)) {
- if (minRate < T(sensors[PRESS].rate))
- minRate = T(sensors[PRESS].rate);
- }
-
- if (T(accelSensorDependencies) & BIT(TEMP)) {
- if (minRate < T(sensors[TEMP].rate))
- minRate = T(sensors[TEMP].rate);
- }
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
-
- /* This call return index of LSM6DSMImuRates struct element */
- return lsm6dsm_computeOdr(minRate);
+ return LSM6DSMImuRatesInNs[i];
}
/*
- * lsm6dsm_setTriggerRate: detect between accel & gyro fastest odr
+ * lsm6dsm_decimatorToFifoDecimatorReg: get decimator reg value based on decimation factor
+ * @dec: FIFO sample decimation factor.
*/
-static void lsm6dsm_setTriggerRate()
+static uint8_t lsm6dsm_decimatorToFifoDecimatorReg(uint8_t dec)
{
- TDECL();
- uint8_t i;
+ uint8_t regValue;
- i = lsm6dsm_getAccelHwMinOdr();
+ switch (dec) {
+ case 1:
+ regValue = LSM6DSM_FIFO_NO_DECIMATION;
+ break;
+ case 2:
+ regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_2;
+ break;
+ case 3:
+ regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_3;
+ break;
+ case 4:
+ regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_4;
+ break;
+ case 8:
+ regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_8;
+ break;
+ case 16:
+ regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_16;
+ break;
+ case 32:
+ regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_32;
+ break;
+ default:
+ regValue = LSM6DSM_FIFO_SAMPLE_NOT_IN_FIFO;
+ break;
+ }
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- T(triggerRate) = SENSOR_HZ_RATE_TO_US(LSM6DSMImuRates[i]);
-#else /* LSM6DSM_GYRO_CALIB_ENABLED */
- uint32_t maxRate = LSM6DSMImuRates[i];
-
- if (maxRate < T(sensors[GYRO]).hwRate)
- maxRate = T(sensors[GYRO]).hwRate;
-
- T(triggerRate) = SENSOR_HZ_RATE_TO_US(maxRate);
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
+ return regValue;
}
/*
- * lsm6dsm_updateAccelOdr: update accel odr based on enabled dependencies
+ * lsm6dsm_calculateFifoDecimators: calculate fifo decimators
+ * @RequestedRate: list of ODRs requested by driver for each sensor in FIFO.
+ * @minLatency: the function will set the min latency based on all sensors enabled in FIFO.
*/
-static bool lsm6dsm_updateAccelOdr()
+static bool lsm6dsm_calculateFifoDecimators(uint32_t RequestedRate[FIFO_NUM], uint64_t *minLatency)
{
TDECL();
- uint8_t i;
+ uint8_t i, n, tempDec, decimators[FIFO_NUM] = { 0 }, minDec = UINT8_MAX, maxDec = 0;
+ enum SensorIndex sidx;
+ bool changed = false;
- /* If no one is using accel disable it. If dependencies are using accel try to reduce ODR */
- if (T(accelSensorDependencies) == 0) {
- DEBUG_PRINT("updateAccelOdr: no one is using accel, disabling it\n");
- T(sensors[ACCEL]).hwRate = 0;
- SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
- lsm6dsm_setTriggerRate();
+ T(fifoCntl).triggerRate = T(sensors[ACCEL]).hwRate;
+ if (T(sensors[GYRO]).hwRate > T(fifoCntl).triggerRate)
+ T(fifoCntl).triggerRate = T(sensors[GYRO]).hwRate;
+
+ for (i = 0; i < FIFO_NUM; i++) {
+ sidx = T(fifoCntl).decimatorsIdx[i];
+ if (sidx >= NUM_SENSORS)
+ continue;
+
+ if (T(sensors[i]).latency < *minLatency)
+ *minLatency = T(sensors[i]).latency;
+ }
+
+ for (i = 0; i < FIFO_NUM; i++) {
+ sidx = T(fifoCntl).decimatorsIdx[i];
+ if (sidx >= NUM_SENSORS)
+ continue;
+
+ if (RequestedRate[i]) {
+ decimators[i] = (T(fifoCntl).triggerRate / RequestedRate[i]) <= 32 ? (T(fifoCntl).triggerRate / RequestedRate[i]) : 32;
+
+ tempDec = decimators[i];
+ while (decimators[i] > 1) {
+ if (((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * decimators[i]) > *minLatency)
+ decimators[i] /= 2;
+ else
+ break;
+ }
+ T(sensors[sidx]).samplesFifoDecimator = tempDec / decimators[i];
+ T(sensors[sidx]).samplesFifoDecimatorCounter = T(sensors[sidx]).samplesFifoDecimator - 1;
+
+ if (decimators[i] < minDec)
+ minDec = decimators[i];
+
+ if (decimators[i] > maxDec)
+ maxDec = decimators[i];
+ }
+
+ DEBUG_PRINT("calculateFifoDecimators: sensorIndex=%d, fifo decimator=%d, software decimation=%d\n", sidx, decimators[i], T(sensors[sidx]).samplesFifoDecimator);
+
+ if (T(fifoCntl).decimators[i] != decimators[i]) {
+ T(fifoCntl).decimators[i] = decimators[i];
+ changed = true;
+ }
+ }
+
+ /* Embedded timestamp slot */
+ T(fifoCntl).decimators[FIFO_DS4] = minDec;
+
+ T(fifoCntl).minDecimator = minDec;
+ T(fifoCntl).maxDecimator = maxDec;
+ T(fifoCntl).maxMinDecimator = maxDec / minDec;
+ T(fifoCntl).totalSip = 0;
+
+ if (maxDec > 0) {
+ T(time).theoreticalDeltaTimeLSB = cpuMathU64DivByU16((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).minDecimator, LSM6DSM_TIME_RESOLUTION);
+ T(time).deltaTimeMarginLSB = ((T(time).theoreticalDeltaTimeLSB) * 10) / 100;
+
+ for (i = 0; i < FIFO_NUM; i++) {
+ if (T(fifoCntl).decimators[i] > 0)
+ T(fifoCntl).totalSip += (maxDec / T(fifoCntl).decimators[i]);
+ }
+ }
+
+ DEBUG_PRINT("calculateFifoDecimators: samples in pattern=%d\n", T(fifoCntl).totalSip);
+
+ for (i = 0; i < T(fifoCntl).maxMinDecimator; i++) {
+ T(fifoCntl).timestampPosition[i] = 0;
+ for (n = 0; n < FIFO_NUM - 1; n++) {
+ if ((T(fifoCntl).decimators[n] > 0) && ((i % (T(fifoCntl).decimators[n] / T(fifoCntl).minDecimator)) == 0))
+ T(fifoCntl).timestampPosition[i] += LSM6DSM_ONE_SAMPLE_BYTE;
+ }
+ }
+
+ return changed;
+}
+
+/*
+ * lsm6dsm_calculateWatermark: calculate fifo watermark level
+ * @minLatency: min latency requested by system based on all sensors in FIFO.
+ */
+static bool lsm6dsm_calculateWatermark(uint64_t *minLatency)
+{
+ TDECL();
+ uint64_t patternRate, tempLatency;
+ uint16_t watermark;
+ uint16_t i = 1;
+
+ if (T(fifoCntl).totalSip > 0) {
+ patternRate = (uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator;
+
+ do {
+ tempLatency = patternRate * (++i);
+ } while ((tempLatency < *minLatency) && (i <= LSM6DSM_MAX_WATERMARK_VALUE));
+
+ watermark = (i - 1) * T(fifoCntl).totalSip;
+
+ while (watermark > LSM6DSM_MAX_WATERMARK_VALUE) {
+ watermark /= 2;
+ watermark = watermark - (watermark % T(fifoCntl).totalSip);
+ }
+
+ DEBUG_PRINT("calculateWatermark: level=#%d, min latency=%lldns\n", watermark, *minLatency);
+
+ if (T(fifoCntl).watermark != watermark) {
+ T(fifoCntl).watermark = watermark;
+ return true;
+ }
+ }
+
+ return false;
+}
+
+/*
+ * lsm6dsm_resetTimestampSync: reset all variables used by sync timestamp task
+ */
+static inline void lsm6dsm_resetTimestampSync(void)
+{
+ TDECL();
+
+ T(lastFifoReadTimestamp) = 0;
+
+ T(time).sampleTimestampFromFifoLSB = 0;
+ T(time).timestampIsValid = false;
+ T(time).lastSampleTimestamp = 0;
+ T(time).noTimer.lastTimestampDataAvlRtcTime = 0;
+ T(time).noTimer.newTimestampDataAvl = false;
+ T(time).timestampSyncTaskLSB = 0;
+
+ time_sync_reset(&T(time).sensorTimeToRtcData);
+}
+
+/*
+ * lsm6dsm_updateSyncTaskMode: set the best way to sync timestamp
+ * @minLatency: min latency of sensors using FIFO.
+ */
+static inline void lsm6dsm_updateSyncTaskMode(uint64_t *minLatency)
+{
+ TDECL();
+
+ /* If minLatency is `small` do not use timer to read timestamp and
+ temperature but read it during FIFO read. */
+ if (*minLatency < LSM6DSM_SYNC_DELTA_INTERVAL) {
+ T(time).status = TIME_SYNC_DURING_FIFO_READ;
} else {
- i = lsm6dsm_getAccelHwMinOdr();
+ T(time).status = TIME_SYNC_TIMER;
- T(sensors[ACCEL]).samplesDecimator = LSM6DSMImuRates[i] / T(sensors[ACCEL]).rate;
- T(sensors[ACCEL]).samplesCounter = T(sensors[ACCEL]).samplesDecimator - 1;
- T(sensors[ACCEL]).samplesToDiscard = LSM6DSMAccelRatesSamplesToDiscard[i];
- T(sensors[ACCEL]).hwRate = LSM6DSMImuRates[i];
-
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- if (T(accelSensorDependencies) & BIT(MAGN)) {
- if (T(sensors[ACCEL]).hwRate > SENSOR_HZ(104.0f))
- T(sensors[MAGN]).samplesDecimator = SENSOR_HZ(104.0f) / T(sensors[MAGN]).rate;
- else
- T(sensors[MAGN]).samplesDecimator = T(sensors[ACCEL]).hwRate / T(sensors[MAGN]).rate;
-
- T(sensors[MAGN]).samplesCounter = T(sensors[MAGN]).samplesDecimator - 1;
- T(sensors[MAGN]).samplesToDiscard = 1;
+ if (!osEnqueuePrivateEvt(EVT_TIME_SYNC, 0, NULL, mTask.tid)) {
+ T(pendingTimeSyncTask) = true;
+ ERROR_PRINT("updateSyncTaskMode: failed to enqueue time sync event\n");
}
-#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+ }
+}
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- if (T(accelSensorDependencies) & BIT(PRESS)) {
- if (T(sensors[ACCEL]).hwRate > SENSOR_HZ(104.0f))
- T(sensors[PRESS]).samplesDecimator = SENSOR_HZ(104.0f) / T(sensors[PRESS]).rate;
- else
- T(sensors[PRESS]).samplesDecimator = T(sensors[ACCEL]).hwRate / T(sensors[PRESS]).rate;
+/*
+ * lsm6dsm_updateOdrs: update ODRs based on rates
+ */
+static bool lsm6dsm_updateOdrs()
+{
+ TDECL();
+ bool accelOdrChanged = false, gyroOdrChanged = false, decChanged, watermarkChanged, gyroFirstEnable = false;
+ uint32_t maxRate, maxPushDataRate[FIFO_NUM] = { 0 };
+ uint64_t minLatency = UINT64_MAX;
+ uint8_t i, regValue, buffer[5];
+ uint16_t watermarkReg;
- T(sensors[PRESS]).samplesCounter = T(sensors[PRESS]).samplesDecimator - 1;
- T(sensors[PRESS]).samplesToDiscard = 1;
- }
+ maxRate = 0;
- if (T(accelSensorDependencies) & BIT(TEMP)) {
- if (T(sensors[ACCEL]).hwRate > SENSOR_HZ(104.0f))
- T(sensors[TEMP]).samplesDecimator = SENSOR_HZ(104.0f) / T(sensors[TEMP]).rate;
- else
- T(sensors[TEMP]).samplesDecimator = T(sensors[ACCEL]).hwRate / T(sensors[TEMP]).rate;
+ /* Verify accel odr */
+ for (i = 0; i < NUM_SENSORS; i++) {
+ if (T(sensors[ACCEL]).rate[i] > maxRate)
+ maxRate = T(sensors[ACCEL]).rate[i] < SENSOR_HZ(26.0f / 2.0f) ? SENSOR_HZ(26.0f / 2.0f) : T(sensors[ACCEL]).rate[i];
- T(sensors[TEMP]).samplesCounter = T(sensors[TEMP]).samplesDecimator - 1;
- T(sensors[TEMP]).samplesToDiscard = 1;
- }
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
-
- lsm6dsm_setTriggerRate();
-
- DEBUG_PRINT("updateAccelOdr: accel in use, updating odr to %dHz\n", (int)(T(sensors[ACCEL]).hwRate / 1024));
-
- SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSMImuRatesRegValue[i]);
+ if ((T(sensors[ACCEL]).rate[i] > maxPushDataRate[FIFO_ACCEL]) && T(sensors[ACCEL]).dependenciesRequireData[i])
+ maxPushDataRate[FIFO_ACCEL] = T(sensors[ACCEL]).rate[i];
+ }
+ if (T(sensors[ACCEL]).hwRate != maxRate) {
+ T(sensors[ACCEL]).hwRate = maxRate;
+ accelOdrChanged = true;
}
- return true;
+ maxRate = 0;
+
+ /* Verify gyro odr */
+ for (i = 0; i < NUM_SENSORS; i++) {
+ if (T(sensors[GYRO]).rate[i] > maxRate)
+ maxRate = T(sensors[GYRO]).rate[i] < SENSOR_HZ(26.0f / 2.0f) ? SENSOR_HZ(26.0f / 2.0f) : T(sensors[GYRO]).rate[i];
+
+ if (T(sensors[GYRO]).rate[i] > maxPushDataRate[FIFO_GYRO])
+ maxPushDataRate[FIFO_GYRO] = T(sensors[GYRO]).rate[i];
+ }
+ if (T(sensors[GYRO]).hwRate != maxRate) {
+ /* If gyro is enabled from PowerDown more samples needs to be discarded */
+ if (T(sensors[GYRO]).hwRate == 0)
+ gyroFirstEnable = true;
+
+ T(sensors[GYRO]).hwRate = maxRate;
+ gyroOdrChanged = true;
+ }
+
+#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
+ /* If magnetometer is enabled, FIFO is used for it */
+ maxPushDataRate[FIFO_DS3] = T(sensors[MAGN]).rate[MAGN];
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
+#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
+ /* If magnetometer is not available FIFO can be used to store barometer sensor data */
+ maxPushDataRate[FIFO_DS3] = T(sensors[PRESS]).rate[PRESS] > T(sensors[TEMP]).rate[TEMP] ? T(sensors[PRESS]).rate[PRESS] : T(sensors[TEMP]).rate[TEMP];
+#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED, LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
+ decChanged = lsm6dsm_calculateFifoDecimators(maxPushDataRate, &minLatency);
+ watermarkChanged = lsm6dsm_calculateWatermark(&minLatency);
+ watermarkReg = T(fifoCntl).watermark * 3;
+
+ if (accelOdrChanged || gyroOdrChanged || decChanged) {
+ /* read all FIFO content and disable it */
+ DEBUG_PRINT("updateOdrs: disabling FIFO\n");
+ T(time).status = TIME_SYNC_DISABLED;
+
+ SPI_WRITE(LSM6DSM_TIMESTAMP2_REG_ADDR, LSM6DSM_TIMESTAMP2_REG_RESET_TIMESTAMP);
+ SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_BYPASS_MODE, 25);
+ }
+
+ if (accelOdrChanged) {
+ if (T(sensors[ACCEL]).hwRate == 0) {
+ DEBUG_PRINT("updateOdrs: no one is using accel, disabling it\n");
+ regValue = 0;
+ } else {
+ DEBUG_PRINT("updateOdrs: accel in use, updating odr to %dHz\n", (int)(T(sensors[ACCEL]).hwRate / 1024));
+ i = lsm6dsm_computeOdr(T(sensors[ACCEL]).hwRate);
+ regValue = LSM6DSMImuRatesRegValue[i];
+ T(sensors[ACCEL]).samplesToDiscard = LSM6DSMAccelRatesSamplesToDiscard[i] /
+ (T(sensors[ACCEL]).hwRate / (T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_ACCEL]));
+
+ if (T(sensors[ACCEL]).samplesToDiscard == 0)
+ T(sensors[ACCEL]).samplesToDiscard = 1;
+
+ T(sensors[ACCEL]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_ACCEL]) /
+ T(sensors[ACCEL]).samplesFifoDecimator) / T(sensors[ACCEL]).rate[ACCEL];
+ T(sensors[ACCEL]).samplesDecimatorCounter = T(sensors[ACCEL]).samplesDecimator - 1;
+ }
+ SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | regValue, 30);
+ }
+
+ if (gyroOdrChanged) {
+ if (T(sensors[GYRO]).hwRate == 0) {
+ DEBUG_PRINT("updateOdrs: no one is using gyro, disabling it\n");
+ regValue = 0;
+ } else {
+ DEBUG_PRINT("updateOdrs: gyro in use, updating odr to %dHz\n", (int)(T(sensors[GYRO]).hwRate / 1024));
+ i = lsm6dsm_computeOdr(T(sensors[GYRO]).hwRate);
+ regValue = LSM6DSMImuRatesRegValue[i];
+ T(sensors[GYRO]).samplesToDiscard = LSM6DSMGyroRatesSamplesToDiscard[i];
+
+ if (gyroFirstEnable)
+ T(sensors[GYRO]).samplesToDiscard += LSM6DSMRatesSamplesToDiscardGyroPowerOn[i];
+
+ T(sensors[GYRO]).samplesToDiscard /= (T(sensors[GYRO]).hwRate / (T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_GYRO]));
+
+ if (T(sensors[GYRO]).samplesToDiscard == 0)
+ T(sensors[GYRO]).samplesToDiscard = 1;
+
+ T(sensors[GYRO]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_GYRO]) /
+ T(sensors[GYRO]).samplesFifoDecimator) / T(sensors[GYRO]).rate[GYRO];
+ T(sensors[GYRO]).samplesDecimatorCounter = T(sensors[GYRO]).samplesDecimator - 1;
+ }
+ SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | regValue, 30);
+ }
+
+ /* Program Fifo and enable or disable it */
+ if (accelOdrChanged || gyroOdrChanged || decChanged) {
+ buffer[0] = *((uint8_t *)&watermarkReg);
+ buffer[1] = (*((uint8_t *)&watermarkReg + 1) & LSM6DSM_FIFO_CTRL2_FTH_MASK) | LSM6DSM_ENABLE_FIFO_TIMESTAMP;
+ buffer[2] = (lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_GYRO]) << 3) |
+ lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_ACCEL]);
+ buffer[3] = (lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_DS4]) << 3) |
+ lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_DS3]);
+
+ for (i = 0; i < FIFO_NUM - 1; i++) {
+ if (T(fifoCntl).decimators[i] > 0)
+ break;
+ }
+ if (i < (FIFO_NUM - 1)) {
+ /* Someone want to use FIFO */
+ DEBUG_PRINT("updateOdrs: enabling FIFO in continuos mode\n");
+ buffer[4] = LSM6DSM_FIFO_CONTINUOS_MODE;
+
+ lsm6dsm_resetTimestampSync();
+ lsm6dsm_updateSyncTaskMode(&minLatency);
+ } else {
+ /* No one is using FIFO */
+ buffer[4] = LSM6DSM_FIFO_BYPASS_MODE;
+
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ if ((T(sensors[PRESS]).rate[PRESS] > 0) || (T(sensors[TEMP]).rate[TEMP] > 0)) {
+ uint64_t latencyOnlyBaro = LSM6DSM_SYNC_DELTA_INTERVAL;
+ lsm6dsm_resetTimestampSync();
+ lsm6dsm_updateSyncTaskMode(&latencyOnlyBaro);
+ }
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+ }
+
+ SPI_MULTIWRITE(LSM6DSM_FIFO_CTRL1_ADDR, buffer, 5);
+ } else {
+ if (watermarkChanged) {
+ lsm6dsm_updateSyncTaskMode(&minLatency);
+
+ buffer[0] = *((uint8_t *)&watermarkReg);
+ buffer[1] = (*((uint8_t *)&watermarkReg + 1) & LSM6DSM_FIFO_CTRL2_FTH_MASK) | LSM6DSM_ENABLE_FIFO_TIMESTAMP;
+ SPI_MULTIWRITE(LSM6DSM_FIFO_CTRL1_ADDR, buffer, 2);
+ }
+ }
+
+ if (accelOdrChanged || gyroOdrChanged || decChanged || watermarkChanged)
+ return true;
+
+ return false;
}
/*
* lsm6dsm_setAccelPower: enable/disable accelerometer sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setAccelPower(bool on, void *cookie)
{
@@ -1329,25 +1876,17 @@
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
INFO_PRINT("setAccelPower: %s\n", on ? "enable" : "disable");
- if (on) {
- T(accelSensorDependencies) |= BIT(ACCEL);
- T(int1Register) |= LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
+ if (on)
+ osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[ACCEL]), NULL, mTask.tid);
+ else {
+ T(sensors[ACCEL]).rate[ACCEL] = 0;
+ T(sensors[ACCEL]).latency = UINT64_MAX;
- /* Discard same samples if interrupts are generated during INT enable */
- T(sensors[ACCEL]).samplesToDiscard = 255;
-
- SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
- } else {
- T(accelSensorDependencies) &= ~BIT(ACCEL);
- T(int1Register) &= ~LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
-
- lsm6dsm_updateAccelOdr();
-
- SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
+ if (lsm6dsm_updateOdrs())
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
+ else
+ osEnqueuePrivateEvt(EVT_SENSOR_POWERING_DOWN, &T(sensors[ACCEL]), NULL, mTask.tid);
}
-
- /* If enable, set only INT bit enable (sensor will be switched on by setRate function). If disable, it depends on accelSensorDependencies */
- lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
} else {
T(pendingEnableConfig[ACCEL]) = true;
T(sensors[ACCEL]).pConfig.enable = on;
@@ -1358,6 +1897,8 @@
/*
* lsm6dsm_setGyroPower: enable/disable gyroscope sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setGyroPower(bool on, void *cookie)
{
@@ -1368,33 +1909,20 @@
if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
INFO_PRINT("setGyroPower: %s\n", on ? "enable" : "disable");
- if (on) {
- T(int1Register) |= LSM6DSM_INT_GYRO_ENABLE_REG_VALUE;
-
- /* Discard same samples if interrupts are generated during INT enable */
- T(sensors[GYRO]).samplesToDiscard = 255;
-
- SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
- } else {
- T(int1Register) &= ~LSM6DSM_INT_GYRO_ENABLE_REG_VALUE;
-
+ if (on)
+ osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[GYRO]), NULL, mTask.tid);
+ else {
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- T(accelSensorDependencies) &= ~BIT(GYRO);
-
- if (!T(sensors[ACCEL].enabled))
- T(int1Register) &= ~LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
+ T(sensors[ACCEL]).rate[GYRO] = 0;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
+ T(sensors[GYRO]).rate[GYRO] = 0;
+ T(sensors[GYRO]).latency = UINT64_MAX;
- T(sensors[GYRO]).hwRate = 0;
-
- SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
- SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE);
-
- lsm6dsm_updateAccelOdr();
+ if (lsm6dsm_updateOdrs()) {
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
+ } else
+ osEnqueuePrivateEvt(EVT_SENSOR_POWERING_DOWN, &T(sensors[GYRO]), NULL, mTask.tid);
}
-
- /* If enable, set only INT bit enable (sensor will be switched on by setRate function). If disable, disable INT bit and disable ODR (power-off sensor) */
- lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
} else {
T(pendingEnableConfig[GYRO]) = true;
T(sensors[GYRO]).pConfig.enable = on;
@@ -1406,6 +1934,8 @@
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
* lsm6dsm_setMagnPower: enable/disable magnetometer sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setMagnPower(bool on, void *cookie)
{
@@ -1417,43 +1947,40 @@
INFO_PRINT("setMagnPower: %s\n", on ? "enable" : "disable");
if (on) {
- T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
+ if (T(masterConfigDependencies) != 0) {
+ T(masterConfigDependencies) |= BIT(MAGN);
- /* Discard same samples if interrupts are generated during INT enable before switch on sensor */
- T(sensors[MAGN]).samplesToDiscard = 255;
+ osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[MAGN]), NULL, mTask.tid);
+ } else {
+ T(masterConfigDependencies) |= BIT(MAGN);
+ T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
- SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
+ }
} else {
- T(accelSensorDependencies) &= ~BIT(MAGN);
- T(embeddedFunctionsDependencies) &= ~BIT(MAGN);
+ T(masterConfigDependencies) &= ~BIT(MAGN);
SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR,
LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE,
T(sensors[ACCEL]).hwRate, MAGN);
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- if (!(T(sensors[PRESS].enabled) || T(sensors[TEMP].enabled))) {
+ if (T(masterConfigDependencies) == 0) {
+ DEBUG_PRINT("setMagnPower: no sensors enabled on i2c master, disabling it\n");
T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
- T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
-
SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
}
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
- if (T(embeddedFunctionsDependencies) == 0) {
- DEBUG_PRINT("setMagnPower: no embedded sensors on, disabling digital functions\n");
- T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
- SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
- }
-
+ T(sensors[ACCEL]).rate[MAGN] = 0;
+ T(sensors[MAGN]).rate[MAGN] = 0;
+ T(sensors[MAGN]).latency = UINT64_MAX;
T(sensors[MAGN]).hwRate = 0;
- lsm6dsm_updateAccelOdr();
- }
+ lsm6dsm_updateOdrs();
- /* If enable, set only INT bit enable (sensor will be switched on by setRate function).
- If disable, disable INT bit, disable sensor-hub and disable ODR (power-off sensor) */
- lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
+ }
} else {
T(pendingEnableConfig[MAGN]) = true;
T(sensors[MAGN]).pConfig.enable = on;
@@ -1466,11 +1993,12 @@
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
* lsm6dsm_setPressPower: enable/disable pressure sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setPressPower(bool on, void *cookie)
{
TDECL();
- uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;
/* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
@@ -1478,55 +2006,60 @@
INFO_PRINT("setPressPower: %s\n", on ? "enable" : "disable");
if (on) {
- T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
+ if (T(masterConfigDependencies) != 0) {
+ T(masterConfigDependencies) |= BIT(PRESS);
- /* Discard same samples if interrupts are generated during INT enable before switch on sensor */
- T(sensors[PRESS]).samplesToDiscard = 255;
+ osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[PRESS]), NULL, mTask.tid);
+ } else {
+ T(masterConfigDependencies) |= BIT(PRESS);
+ T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
- SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
+ }
} else {
- T(accelSensorDependencies) &= ~BIT(PRESS);
- T(embeddedFunctionsDependencies) &= ~BIT(PRESS);
+ uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;
+
+ T(masterConfigDependencies) &= ~BIT(PRESS);
if (T(sensors[TEMP]).enabled) {
- i = lsm6dsm_computeOdr(T(sensors[TEMP]).rate);
+ i = lsm6dsm_computeOdr(T(sensors[TEMP]).rate[TEMP]);
reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i);
} else
reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE;
SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value,
- T(sensors[PRESS]).hwRate, PRESS);
+ T(sensors[ACCEL]).hwRate, PRESS);
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- if (!(T(sensors[MAGN].enabled) || T(sensors[TEMP].enabled))) {
+ if (T(masterConfigDependencies) == 0) {
+ DEBUG_PRINT("setPressPower: no sensors enabled on i2c master, disabling it\n");
T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
- T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
-
SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
}
-#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
- if (!T(sensors[TEMP].enabled)) {
- T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
- T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
- SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
+ if (T(baroTimerId)) {
+ timTimerCancel(T(baroTimerId));
+ T(baroTimerId) = 0;
+
+ T(pendingBaroTimerTask) = false;
+ T(time).timestampBaroLSB = 0;
+
+ if (T(sensors[TEMP]).enabled)
+ T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(T(sensors[TEMP]).rate[TEMP]), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
- if (T(embeddedFunctionsDependencies) == 0) {
- DEBUG_PRINT("setPressPower: no embedded sensors on, disabling digital functions\n");
- T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
- SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
- }
-
+ T(sensors[ACCEL]).rate[PRESS] = 0;
+ T(sensors[PRESS]).rate[PRESS] = 0;
+ T(sensors[PRESS]).latency = UINT64_MAX;
T(sensors[PRESS]).hwRate = 0;
- lsm6dsm_updateAccelOdr();
- }
+ lsm6dsm_updateOdrs();
- /* If enable, set only INT bit enable (sensor will be switched on by setRate function).
- If disable, disable INT bit, disable sensor-hub and disable ODR (power-off sensor) */
- lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
+ }
} else {
T(pendingEnableConfig[PRESS]) = true;
T(sensors[PRESS]).pConfig.enable = on;
@@ -1537,11 +2070,12 @@
/*
* lsm6dsm_setTempPower: enable/disable temperature sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setTempPower(bool on, void *cookie)
{
TDECL();
- uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;
/* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
@@ -1549,55 +2083,60 @@
INFO_PRINT("setTempPower: %s\n", on ? "enable" : "disable");
if (on) {
- T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
+ if (T(masterConfigDependencies) != 0) {
+ T(masterConfigDependencies) |= BIT(TEMP);
- /* Discard same samples if interrupts are generated during INT enable before switch on sensor */
- T(sensors[TEMP]).samplesToDiscard = 255;
+ osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[TEMP]), NULL, mTask.tid);
+ } else {
+ T(masterConfigDependencies) |= BIT(TEMP);
+ T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
- SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+ SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
+ }
} else {
- T(accelSensorDependencies) &= ~BIT(TEMP);
- T(embeddedFunctionsDependencies) &= ~BIT(TEMP);
+ uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;
+
+ T(masterConfigDependencies) &= ~BIT(TEMP);
if (T(sensors[PRESS]).enabled) {
- i = lsm6dsm_computeOdr(T(sensors[PRESS]).rate);
+ i = lsm6dsm_computeOdr(T(sensors[PRESS]).rate[PRESS]);
reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i);
} else
reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE;
SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value,
- T(sensors[TEMP]).hwRate, PRESS);
+ T(sensors[ACCEL]).hwRate, TEMP);
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- if (!(T(sensors[MAGN].enabled) || T(sensors[PRESS].enabled))) {
+ if (T(masterConfigDependencies) == 0) {
+ DEBUG_PRINT("setTempPower: no sensors enabled on i2c master, disabling it\n");
T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
- T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
-
SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
}
-#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
- if (!T(sensors[PRESS].enabled)) {
- T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
- T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
- SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
+#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
+ if (T(baroTimerId)) {
+ timTimerCancel(T(baroTimerId));
+ T(baroTimerId) = 0;
+
+ T(pendingBaroTimerTask) = false;
+ T(time).timestampBaroLSB = 0;
+
+ if (T(sensors[PRESS]).enabled)
+ T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(T(sensors[PRESS]).rate[PRESS]), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
- if (T(embeddedFunctionsDependencies) == 0) {
- DEBUG_PRINT("setTempPower: no embedded sensors on, disabling digital functions\n");
- T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
- SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
- }
-
+ T(sensors[ACCEL]).rate[TEMP] = 0;
+ T(sensors[TEMP]).rate[TEMP] = 0;
+ T(sensors[TEMP]).latency = UINT64_MAX;
T(sensors[TEMP]).hwRate = 0;
- lsm6dsm_updateAccelOdr();
- }
+ lsm6dsm_updateOdrs();
- /* If enable, set only INT bit enable (sensor will be switched on by setRate function).
- If disable, disable INT bit, disable sensor-hub and disable ODR (power-off sensor) */
- lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
+ }
} else {
T(pendingEnableConfig[TEMP]) = true;
T(sensors[TEMP]).pConfig.enable = on;
@@ -1609,6 +2148,8 @@
/*
* lsm6dsm_setStepDetectorPower: enable/disable step detector sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setStepDetectorPower(bool on, void *cookie)
{
@@ -1620,31 +2161,26 @@
INFO_PRINT("setStepDetectorPower: %s\n", on ? "enable" : "disable");
if (on) {
- T(accelSensorDependencies) |= BIT(STEP_DETECTOR);
- T(embeddedFunctionsDependencies) |= BIT(STEP_DETECTOR);
- T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC | LSM6DSM_ENABLE_DIGITAL_FUNC);
+ T(pedometerDependencies) |= BIT(STEP_DETECTOR);
+ T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
T(int1Register) |= LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE;
- lsm6dsm_updateAccelOdr();
+ T(sensors[ACCEL]).rate[STEP_DETECTOR] = SENSOR_HZ(26.0f);
+ lsm6dsm_updateOdrs();
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
} else {
- T(accelSensorDependencies) &= ~BIT(STEP_DETECTOR);
- T(embeddedFunctionsDependencies) &= ~BIT(STEP_DETECTOR);
+ T(pedometerDependencies) &= ~BIT(STEP_DETECTOR);
T(int1Register) &= ~LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE;
- if ((T(embeddedFunctionsDependencies) & (BIT(STEP_COUNTER) | BIT(SIGN_MOTION))) == 0) {
+ if ((T(pedometerDependencies) & (BIT(STEP_COUNTER) | BIT(SIGN_MOTION))) == 0) {
DEBUG_PRINT("setStepDetectorPower: no more need pedometer algo, disabling it\n");
T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
}
- if (T(embeddedFunctionsDependencies) == 0) {
- DEBUG_PRINT("setStepDetectorPower: no embedded sensors on, disabling digital functions\n");
- T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
- }
-
- lsm6dsm_updateAccelOdr();
+ T(sensors[ACCEL]).rate[STEP_DETECTOR] = 0;
+ lsm6dsm_updateOdrs();
SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
@@ -1662,6 +2198,8 @@
/*
* lsm6dsm_setStepCounterPower: enable/disable step counter sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setStepCounterPower(bool on, void *cookie)
{
@@ -1674,32 +2212,26 @@
if (on) {
T(readSteps) = false;
- T(accelSensorDependencies) |= BIT(STEP_COUNTER);
- T(embeddedFunctionsDependencies) |= BIT(STEP_COUNTER);
- T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC | LSM6DSM_ENABLE_DIGITAL_FUNC);
+ T(pedometerDependencies) |= BIT(STEP_COUNTER);
+ T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
T(int2Register) |= LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE;
- lsm6dsm_updateAccelOdr();
+ T(sensors[ACCEL]).rate[STEP_COUNTER] = SENSOR_HZ(26.0f);
+ lsm6dsm_updateOdrs();
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register));
} else {
- T(accelSensorDependencies) &= ~BIT(STEP_COUNTER);
- T(embeddedFunctionsDependencies) &= ~BIT(STEP_COUNTER);
- T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_TIMER_DIGITAL_FUNC;
+ T(pedometerDependencies) &= ~BIT(STEP_COUNTER);
T(int2Register) &= ~LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE;
- if ((T(embeddedFunctionsDependencies) & (BIT(STEP_DETECTOR) | BIT(SIGN_MOTION))) == 0) {
+ if ((T(pedometerDependencies) & (BIT(STEP_DETECTOR) | BIT(SIGN_MOTION))) == 0) {
DEBUG_PRINT("setStepCounterPower: no more need pedometer algo, disabling it\n");
T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
}
- if (T(embeddedFunctionsDependencies) == 0) {
- DEBUG_PRINT("setStepCounterPower: no embedded sensors on, disabling digital functions\n");
- T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
- }
-
- lsm6dsm_updateAccelOdr();
+ T(sensors[ACCEL]).rate[STEP_COUNTER] = 0;
+ lsm6dsm_updateOdrs();
SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register));
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
@@ -1717,6 +2249,8 @@
/*
* lsm6dsm_setSignMotionPower: enable/disable significant motion sensor
+ * @on: enable or disable sensor.
+ * @cookie: private data.
*/
static bool lsm6dsm_setSignMotionPower(bool on, void *cookie)
{
@@ -1728,31 +2262,26 @@
INFO_PRINT("setSignMotionPower: %s\n", on ? "enable" : "disable");
if (on) {
- T(accelSensorDependencies) |= BIT(SIGN_MOTION);
- T(embeddedFunctionsDependencies) |= BIT(SIGN_MOTION);
- T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC | LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC | LSM6DSM_ENABLE_DIGITAL_FUNC);
+ T(pedometerDependencies) |= BIT(SIGN_MOTION);
+ T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC | LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC);
T(int1Register) |= LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE;
- lsm6dsm_updateAccelOdr();
+ T(sensors[ACCEL]).rate[SIGN_MOTION] = SENSOR_HZ(26.0f);
+ lsm6dsm_updateOdrs();
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
} else {
- T(accelSensorDependencies) &= ~BIT(SIGN_MOTION);
- T(embeddedFunctionsDependencies) &= ~BIT(SIGN_MOTION);
+ T(pedometerDependencies) &= ~BIT(SIGN_MOTION);
T(int1Register) &= ~LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE;
- if ((T(embeddedFunctionsDependencies) & (BIT(STEP_DETECTOR) | BIT(STEP_COUNTER))) == 0) {
+ if ((T(pedometerDependencies) & (BIT(STEP_DETECTOR) | BIT(STEP_COUNTER))) == 0) {
DEBUG_PRINT("setSignMotionPower: no more need pedometer algo, disabling it\n");
T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC;
}
- if (T(embeddedFunctionsDependencies) == 0) {
- DEBUG_PRINT("setSignMotionPower: no embedded sensors on, disabling digital functions\n");
- T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
- }
-
- lsm6dsm_updateAccelOdr();
+ T(sensors[ACCEL]).rate[SIGN_MOTION] = 0;
+ lsm6dsm_updateOdrs();
SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register), 50000);
SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
@@ -1770,6 +2299,7 @@
/*
* lsm6dsm_accelFirmwareUpload: upload accelerometer firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_accelFirmwareUpload(void *cookie)
{
@@ -1782,6 +2312,7 @@
/*
* lsm6dsm_gyroFirmwareUpload: upload gyroscope firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_gyroFirmwareUpload(void *cookie)
{
@@ -1795,6 +2326,7 @@
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
* lsm6dsm_magnFirmwareUpload: upload magnetometer firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_magnFirmwareUpload(void *cookie)
{
@@ -1809,6 +2341,7 @@
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
* lsm6dsm_pressFirmwareUpload: upload pressure firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_pressFirmwareUpload(void *cookie)
{
@@ -1821,6 +2354,7 @@
/*
* lsm6dsm_tempFirmwareUpload: upload pressure firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_tempFirmwareUpload(void *cookie)
{
@@ -1834,6 +2368,7 @@
/*
* lsm6dsm_stepDetectorFirmwareUpload: upload step detector firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_stepDetectorFirmwareUpload(void *cookie)
{
@@ -1846,6 +2381,7 @@
/*
* lsm6dsm_stepCounterFirmwareUpload: upload step counter firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_stepCounterFirmwareUpload(void *cookie)
{
@@ -1858,6 +2394,7 @@
/*
* lsm6dsm_signMotionFirmwareUpload: upload significant motion firmware
+ * @cookie: private data.
*/
static bool lsm6dsm_signMotionFirmwareUpload(void *cookie)
{
@@ -1870,6 +2407,9 @@
/*
* lsm6dsm_setAccelRate: set accelerometer ODR and report latency (FIFO watermark related)
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setAccelRate(uint32_t rate, uint64_t latency, void *cookie)
{
@@ -1878,12 +2418,13 @@
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
INFO_PRINT("setAccelRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
- T(sensors[ACCEL]).rate = rate;
+ T(sensors[ACCEL]).rate[ACCEL] = rate;
T(sensors[ACCEL]).latency = latency;
- lsm6dsm_updateAccelOdr();
-
- lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
+ if (lsm6dsm_updateOdrs())
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
+ else
+ osEnqueuePrivateEvt(EVT_SENSOR_CONFIG_CHANGING, &T(sensors[ACCEL]), NULL, mTask.tid);
} else {
T(pendingRateConfig[ACCEL]) = true;
T(sensors[ACCEL].pConfig.rate) = rate;
@@ -1895,44 +2436,28 @@
/*
* lsm6dsm_setGyroRate: set gyroscope ODR and report latency (FIFO watermark related)
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setGyroRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
- uint8_t i;
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
INFO_PRINT("setGyroRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
- /* This call return index of LSM6DSMImuRates struct element */
- i = lsm6dsm_computeOdr(rate);
-
- T(sensors[GYRO]).rate = rate;
- T(sensors[GYRO]).latency = latency;
- T(sensors[GYRO]).samplesToDiscard = LSM6DSMGyroRatesSamplesToDiscard[i];
-
- if (T(sensors[GYRO]).hwRate == 0)
- T(sensors[GYRO]).samplesToDiscard += LSM6DSMRatesSamplesToDiscardGyroPowerOn[i];
-
- T(sensors[GYRO]).hwRate = rate < (SENSOR_HZ(26.0f) / 2) ? (SENSOR_HZ(26.0f) / 2) : rate;
- T(sensors[GYRO]).samplesDecimator = T(sensors[GYRO]).hwRate / rate;
- T(sensors[GYRO]).samplesCounter = T(sensors[GYRO]).samplesDecimator - 1;
-
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- /* Gyroscope bias calibration library requires accelerometer data, enabling it @26Hz if not enabled */
- T(accelSensorDependencies) |= BIT(GYRO);
- T(int1Register) |= LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
-
- lsm6dsm_updateAccelOdr();
-
- SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
-#else /* LSM6DSM_GYRO_CALIB_ENABLED */
- lsm6dsm_setTriggerRate();
+ T(sensors[ACCEL]).rate[GYRO] = rate;
+ T(sensors[ACCEL]).dependenciesRequireData[GYRO] = true;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
+ T(sensors[GYRO]).rate[GYRO] = rate;
+ T(sensors[GYRO]).latency = latency;
- SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | LSM6DSMImuRatesRegValue[i]);
-
- lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
+ if (lsm6dsm_updateOdrs())
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
+ else
+ osEnqueuePrivateEvt(EVT_SENSOR_CONFIG_CHANGING, &T(sensors[GYRO]), NULL, mTask.tid);
} else {
T(pendingRateConfig[GYRO]) = true;
T(sensors[GYRO]).pConfig.rate = rate;
@@ -1945,33 +2470,34 @@
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
* lsm6dsm_setMagnRate: set magnetometer ODR and report latency (FIFO watermark related)
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setMagnRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
- uint8_t i, buffer[2];
+ uint8_t i;
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
INFO_PRINT("setMagnRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
- T(embeddedFunctionsDependencies) |= BIT(MAGN);
- T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_DIGITAL_FUNC;
- T(accelSensorDependencies) |= BIT(MAGN);
-
- T(sensors[MAGN]).rate = rate;
+ T(sensors[ACCEL]).rate[MAGN] = rate;
+#ifdef LSM6DSM_MAGN_CALIB_ENABLED
+ T(sensors[ACCEL]).dependenciesRequireData[MAGN] = true;
+#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
+ T(sensors[MAGN]).rate[MAGN] = rate;
T(sensors[MAGN]).latency = latency;
- lsm6dsm_updateAccelOdr();
-
- T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
-
- buffer[0] = T(embeddedFunctionsRegister); /* LSM6DSM_CTRL10_C */
- buffer[1] = T(masterConfigRegister); /* LSM6DSM_MASTER_CONFIG */
- SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
+ lsm6dsm_updateOdrs();
/* This call return index of LSM6DSMImuRates struct element */
i = lsm6dsm_computeOdr(rate);
T(sensors[MAGN]).hwRate = LSM6DSMSHRates[i];
+ T(sensors[MAGN]).samplesToDiscard = 3;
+
+ T(sensors[MAGN]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[MAGN]).samplesFifoDecimator) / T(sensors[MAGN]).rate[MAGN];
+ T(sensors[MAGN]).samplesDecimatorCounter = T(sensors[MAGN]).samplesDecimator - 1;
#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
@@ -2000,38 +2526,53 @@
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
* lsm6dsm_setPressRate: set pressure ODR and report latency (FIFO watermark related)
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setPressRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
- uint8_t i, buffer[2];
+ uint8_t i;
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
INFO_PRINT("setPressRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
- T(embeddedFunctionsDependencies) |= BIT(PRESS);
- T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_DIGITAL_FUNC;
- T(accelSensorDependencies) |= BIT(PRESS);
-
- T(sensors[PRESS]).rate = rate;
+ T(sensors[ACCEL]).rate[PRESS] = rate;
+ T(sensors[PRESS]).rate[PRESS] = rate;
T(sensors[PRESS]).latency = latency;
- lsm6dsm_updateAccelOdr();
-
- T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
-
- buffer[0] = T(embeddedFunctionsRegister); /* LSM6DSM_CTRL10_C */
- buffer[1] = T(masterConfigRegister); /* LSM6DSM_MASTER_CONFIG */
- SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
+ lsm6dsm_updateOdrs();
if (T(sensors[TEMP]).enabled) {
- if (rate < T(sensors[TEMP]).rate)
- rate = T(sensors[TEMP]).rate;
+ if (rate < T(sensors[TEMP]).rate[TEMP])
+ rate = T(sensors[TEMP]).rate[TEMP];
}
/* This call return index of LSM6DSMImuRates struct element */
i = lsm6dsm_computeOdr(rate);
T(sensors[PRESS]).hwRate = LSM6DSMSHRates[i];
+ T(sensors[PRESS]).samplesToDiscard = 3;
+
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
+ if (T(baroTimerId)) {
+ timTimerCancel(T(baroTimerId));
+ T(baroTimerId) = 0;
+ T(pendingBaroTimerTask) = false;
+ }
+
+ T(sensors[PRESS]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS];
+ T(sensors[TEMP]).samplesDecimator = rate / T(sensors[TEMP]).rate[TEMP];
+ T(time).timestampBaroLSB = 0;
+
+ T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(rate), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
+#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+ T(sensors[PRESS]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[PRESS]).rate[PRESS];
+ T(sensors[TEMP]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[TEMP]).rate[TEMP];
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
+ T(sensors[PRESS]).samplesDecimatorCounter = T(sensors[PRESS]).samplesDecimator - 1;
+ T(sensors[TEMP]).samplesDecimatorCounter = T(sensors[TEMP]).samplesDecimator - 1;
SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR,
LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i),
@@ -2049,39 +2590,53 @@
/*
* lsm6dsm_setTempRate: set temperature ODR and report latency (FIFO watermark related)
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setTempRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
- uint8_t i, buffer[2];
+ uint8_t i;
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
INFO_PRINT("setTempRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
- T(embeddedFunctionsDependencies) |= BIT(TEMP);
- T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_DIGITAL_FUNC;
- T(accelSensorDependencies) |= BIT(TEMP);
-
- T(sensors[TEMP]).rate = rate;
- T(sensors[TEMP]).hwRate = rate;
+ T(sensors[ACCEL]).rate[TEMP] = rate;
+ T(sensors[TEMP]).rate[TEMP] = rate;
T(sensors[TEMP]).latency = latency;
- lsm6dsm_updateAccelOdr();
-
- T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
-
- buffer[0] = T(embeddedFunctionsRegister); /* LSM6DSM_CTRL10_C */
- buffer[1] = T(masterConfigRegister); /* LSM6DSM_MASTER_CONFIG */
- SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
+ lsm6dsm_updateOdrs();
if (T(sensors[PRESS]).enabled) {
- if (rate < T(sensors[PRESS]).rate)
- rate = T(sensors[PRESS]).rate;
+ if (rate < T(sensors[PRESS]).rate[PRESS])
+ rate = T(sensors[PRESS]).rate[PRESS];
}
/* This call return index of LSM6DSMImuRates struct element */
i = lsm6dsm_computeOdr(rate);
T(sensors[TEMP]).hwRate = LSM6DSMSHRates[i];
+ T(sensors[TEMP]).samplesToDiscard = 3;
+
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
+ if (T(baroTimerId)) {
+ timTimerCancel(T(baroTimerId));
+ T(baroTimerId) = 0;
+ T(pendingBaroTimerTask) = false;
+ }
+
+ T(sensors[TEMP]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS];
+ T(sensors[PRESS]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS];
+ T(time).timestampBaroLSB = 0;
+
+ T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(rate), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
+#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+ T(sensors[TEMP]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[TEMP]).rate[TEMP];
+ T(sensors[PRESS]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[PRESS]).rate[TEMP];
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
+ T(sensors[TEMP]).samplesDecimatorCounter = T(sensors[TEMP]).samplesDecimator - 1;
+ T(sensors[PRESS]).samplesDecimatorCounter = T(sensors[PRESS]).samplesDecimator - 1;
SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR,
LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i),
@@ -2100,6 +2655,9 @@
/*
* lsm6dsm_setStepDetectorRate: set step detector report latency
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setStepDetectorRate(uint32_t rate, uint64_t latency, void *cookie)
{
@@ -2107,7 +2665,7 @@
INFO_PRINT("setStepDetectorRate: latency=%lldns\n", latency);
- T(sensors[STEP_DETECTOR]).rate = rate;
+ T(sensors[STEP_DETECTOR]).hwRate = rate;
T(sensors[STEP_DETECTOR]).latency = latency;
sensorSignalInternalEvt(T(sensors[STEP_DETECTOR]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
@@ -2117,34 +2675,37 @@
/*
* lsm6dsm_setStepCounterRate: set step counter report latency
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setStepCounterRate(uint32_t rate, uint64_t latency, void *cookie)
{
TDECL();
- uint8_t i, step_delta_reg;
-
- if (rate == SENSOR_RATE_ONCHANGE) {
- INFO_PRINT("setStepCounterRate: delivery-rate=on_change, latency=%lldns\n", latency);
- } else
- INFO_PRINT("setStepCounterRate: delivery_rate=%dms, latency=%lldns\n", (int)((1024.0f / rate) * 1000.0f), latency);
+ uint8_t i, regValue;
if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
- T(sensors[STEP_COUNTER]).rate = rate;
+ if (rate == SENSOR_RATE_ONCHANGE) {
+ INFO_PRINT("setStepCounterRate: delivery-rate=on_change, latency=%lldns\n", latency);
+ } else
+ INFO_PRINT("setStepCounterRate: delivery_rate=%dms, latency=%lldns\n", (int)((1024.0f / rate) * 1000.0f), latency);
+
+ T(sensors[STEP_COUNTER]).hwRate = rate;
T(sensors[STEP_COUNTER]).latency = latency;
+ if (rate != SENSOR_RATE_ONCHANGE) {
for (i = 0; i < ARRAY_SIZE(LSM6DSMStepCounterRates); i++) {
if (rate == LSM6DSMStepCounterRates[i])
break;
}
if (i >= (ARRAY_SIZE(LSM6DSMStepCounterRates) - 2))
- step_delta_reg = 0;
+ regValue = 0;
else
- step_delta_reg = (128 >> i);
+ regValue = (128 >> i);
+ } else
+ regValue = 0;
- T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_TIMER_DIGITAL_FUNC;
- SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
-
- lsm6dsm_writeEmbeddedRegister(LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR, step_delta_reg);
+ lsm6dsm_writeEmbeddedRegister(LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR, regValue);
lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
} else {
@@ -2158,6 +2719,9 @@
/*
* lsm6dsm_setSignMotionRate: set significant motion report latency
+ * @rate: sensor rate expressed in SENSOR_HZ(x).
+ * @latency: max latency valud in ns.
+ * @cookie: private data.
*/
static bool lsm6dsm_setSignMotionRate(uint32_t rate, uint64_t latency, void *cookie)
{
@@ -2165,7 +2729,7 @@
DEBUG_PRINT("setSignMotionRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
- T(sensors[SIGN_MOTION]).rate = rate;
+ T(sensors[SIGN_MOTION]).rate[SIGN_MOTION] = rate;
T(sensors[SIGN_MOTION]).latency = latency;
sensorSignalInternalEvt(T(sensors[SIGN_MOTION]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
@@ -2175,24 +2739,54 @@
/*
* lsm6dsm_accelFlush: send accelerometer flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_accelFlush(void *cookie)
{
- INFO_PRINT("accelFlush\n");
+ TDECL();
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL);
+ if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
+ INFO_PRINT("accelFlush: flush accelerometer data\n");
+
+ if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL);
+ SET_STATE(SENSOR_IDLE);
+ return true;
+ }
+
+ T(sendFlushEvt[ACCEL]) = true;
+
+ SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ T(pendingFlush[ACCEL])++;
return true;
}
/*
* lsm6dsm_gyroFlush: send gyroscope flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_gyroFlush(void *cookie)
{
- INFO_PRINT("gyroFlush\n");
+ TDECL();
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO), SENSOR_DATA_EVENT_FLUSH, NULL);
+ if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
+ INFO_PRINT("gyroFlush: flush gyroscope data\n");
+
+ if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO), SENSOR_DATA_EVENT_FLUSH, NULL);
+ SET_STATE(SENSOR_IDLE);
+ return true;
+ }
+
+ T(sendFlushEvt[GYRO]) = true;
+
+ SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ T(pendingFlush[GYRO])++;
return true;
}
@@ -2200,12 +2794,27 @@
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
* lsm6dsm_magnFlush: send magnetometer flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_magnFlush(void *cookie)
{
- INFO_PRINT("magnFlush\n");
+ TDECL();
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL);
+ if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
+ INFO_PRINT("magnFlush: flush magnetometer data\n");
+
+ if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL);
+ SET_STATE(SENSOR_IDLE);
+ return true;
+ }
+
+ T(sendFlushEvt[MAGN]) = true;
+
+ SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ T(pendingFlush[MAGN])++;
return true;
}
@@ -2214,27 +2823,80 @@
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
* lsm6dsm_pressFlush: send pressure flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_pressFlush(void *cookie)
{
+ TDECL();
+
+#if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
+ if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
+ INFO_PRINT("pressFlush: flush pressure data\n");
+
+ if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL);
+ SET_STATE(SENSOR_IDLE);
+ return true;
+ }
+
+ T(sendFlushEvt[PRESS]) = true;
+
+ SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ T(pendingFlush[PRESS])++;
+#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+ INFO_PRINT("pressFlush: flush pressure data\n");
+
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL);
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
return true;
}
/*
* lsm6dsm_tempFlush: send temperature flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_tempFlush(void *cookie)
{
+ TDECL();
+
+#if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
+ if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
+ INFO_PRINT("tempFlush: flush temperature data\n");
+
+ if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL);
+ SET_STATE(SENSOR_IDLE);
+ return true;
+ }
+
+ T(sendFlushEvt[TEMP]) = true;
+
+ SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ T(pendingFlush[TEMP])++;
+#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+ INFO_PRINT("tempFlush: flush temperature data\n");
+
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL);
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
return true;
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
/*
* lsm6dsm_stepDetectorFlush: send step detector flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_stepDetectorFlush(void *cookie)
{
- INFO_PRINT("stepDetectorFlush\n");
+ TDECL();
+
+ INFO_PRINT("stepDetectorFlush: flush step detector data\n");
osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), SENSOR_DATA_EVENT_FLUSH, NULL);
@@ -2243,10 +2905,13 @@
/*
* lsm6dsm_stepCounterFlush: send step counter flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_stepCounterFlush(void *cookie)
{
- INFO_PRINT("stepCounterFlush\n");
+ TDECL();
+
+ INFO_PRINT("stepCounterFlush: flush step counter data\n");
osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), SENSOR_DATA_EVENT_FLUSH, NULL);
@@ -2255,10 +2920,13 @@
/*
* lsm6dsm_signMotionFlush: send significant motion flush event
+ * @cookie: private data.
*/
static bool lsm6dsm_signMotionFlush(void *cookie)
{
- INFO_PRINT("signMotionFlush\n");
+ TDECL();
+
+ INFO_PRINT("signMotionFlush: flush significant motion data\n");
osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), SENSOR_DATA_EVENT_FLUSH, NULL);
@@ -2267,6 +2935,8 @@
/*
* lsm6dsm_stepCounterSendLastData: send last number of steps
+ * @cookie: private data.
+ * @tid: task id.
*/
static bool lsm6dsm_stepCounterSendLastData(void *cookie, uint32_t tid)
{
@@ -2280,7 +2950,7 @@
}
/*
- * lsm6dsm_sensorInit: initial sensor configuration
+ * lsm6dsm_sensorInit: initial sensors configuration
*/
static void lsm6dsm_sensorInit(void)
{
@@ -2317,6 +2987,7 @@
SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, LSM6DSM_INT1_CTRL_BASE);
+ SPI_WRITE(LSM6DSM_WAKE_UP_DUR_ADDR, LSM6DSM_WAKE_UP_DUR_BASE);
#ifdef LSM6DSM_I2C_MASTER_ENABLED
T(initState) = INIT_I2C_MASTER_REGS_CONF;
@@ -2374,7 +3045,7 @@
/* I2C-2 configuration */
buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV2_ADDR */
buffer[1] = AK09916_STATUS_DATA_ADDR; /* LSM6DSM_EMBEDDED_SLV2_SUBADDR */
- buffer[2] = 1; /* LSM6DSM_EMBEDDED_SLV2_CONFIG */
+ buffer[2] = 0x01; /* LSM6DSM_EMBEDDED_SLV2_CONFIG */
SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR, buffer, 3);
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
@@ -2405,8 +3076,8 @@
/* Enable accelerometer and sensor-hub to initialize slave sensor */
SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE);
- SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, LSM6DSM_CTRL10_C_BASE | LSM6DSM_ENABLE_DIGITAL_FUNC);
SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE | LSM6DSM_MASTER_CONFIG_MASTER_ON);
+ T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR, LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE, SENSOR_HZ(104.0f), MAGN, 20000);
@@ -2464,8 +3135,8 @@
/* Disable accelerometer and sensor-hub */
SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
- SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, LSM6DSM_CTRL10_C_BASE);
SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
+ T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
break;
@@ -2484,13 +3155,9 @@
TDECL();
enum SensorIndex i;
- if (T(pendingInt[LSM6DSM_INT1_INDEX])) {
- T(pendingInt[LSM6DSM_INT1_INDEX]) = false;
- lsm6dsm_readStatusReg(false);
- return;
- }
+ SET_STATE(SENSOR_IDLE);
- for (i = ACCEL; i < NUM_SENSORS; i++) {
+ for (i = 0; i < NUM_SENSORS; i++) {
if (T(pendingEnableConfig[i])) {
T(pendingEnableConfig[i]) = false;
LSM6DSMSensorOps[i].sensorPower(T(sensors[i]).pConfig.enable, (void *)i);
@@ -2502,181 +3169,501 @@
LSM6DSMSensorOps[i].sensorSetRate(T(sensors[i]).pConfig.rate, T(sensors[i]).pConfig.latency, (void *)i);
return;
}
+
+ if (T(pendingFlush[i]) > 0) {
+ T(pendingFlush[i])--;
+ LSM6DSMSensorOps[i].sensorFlush((void *)i);
+ return;
+ }
}
+
+ if (T(pendingTimeSyncTask)) {
+ T(pendingTimeSyncTask) = false;
+ lsm6dsm_timeSyncTask();
+ return;
+ }
+
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ if (T(pendingBaroTimerTask)) {
+ T(pendingBaroTimerTask) = false;
+ lsm6dsm_baroTimerTask();
+ return;
+ }
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+
+ if (T(pendingInt)) {
+ T(pendingInt) = false;
+ lsm6dsm_readStatusReg(false);
+ return;
+ }
+
+ if (gpioGet(T(int1)))
+ lsm6dsm_readStatusReg(false);
}
/*
- * lsm6dsm_processSensorThreeAxisData: elaborate three axis sensors data
+ * lsm6dsm_allocateThreeAxisDataEvt: allocate slab for three axis sensor data
+ * @mSensor: sensor info.
+ * @rtcTime: time of first sample in this block.
*/
-static bool lsm6dsm_processSensorThreeAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data)
+static bool lsm6dsm_allocateThreeAxisDataEvt(struct LSM6DSMSensor *mSensor, uint64_t rtcTime)
{
TDECL();
- float kScale = 1.0f, x, y, z;
- switch (mSensor->idx) {
- case ACCEL:
- kScale = LSM6DSM_ACCEL_KSCALE;
- break;
- case GYRO:
- kScale = LSM6DSM_GYRO_KSCALE;
- break;
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- case MAGN:
- kScale = LSM6DSM_MAGN_KSCALE;
- break;
-#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
- default:
+ mSensor->tADataEvt = slabAllocatorAlloc(T(mDataSlabThreeAxis));
+ if (!mSensor->tADataEvt) {
+ ERROR_PRINT("Failed to allocate memory!\n");
return false;
}
- x = ((int16_t)(data[1] << 8) | data[0]) * kScale;
- y = ((int16_t)(data[3] << 8) | data[2]) * kScale;
- z = ((int16_t)(data[5] << 8) | data[4]) * kScale;
+ memset(&mSensor->tADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
+ mSensor->tADataEvt->referenceTime = rtcTime;
+ mSensor->pushedTimestamp = rtcTime;
- mSensor->tADataEvt->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
- mSensor->tADataEvt->samples[0].firstSample.numSamples = 1;
-
- switch (mSensor->idx) {
- case ACCEL:
- case GYRO:
- mSensor->tADataEvt->samples[0].x = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ROT_MATRIX);
- mSensor->tADataEvt->samples[0].y = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ROT_MATRIX);
- mSensor->tADataEvt->samples[0].z = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ROT_MATRIX);
- break;
-
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- case MAGN:
- mSensor->tADataEvt->samples[0].x = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX);
- mSensor->tADataEvt->samples[0].y = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX);
- mSensor->tADataEvt->samples[0].z = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX);
- break;
-#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
-
- default:
- break;
- }
-
- if (mSensor->samplesToDiscard == 0) {
- mSensor->samplesCounter++;
-
- if (mSensor->samplesCounter >= mSensor->samplesDecimator) {
- switch (mSensor->idx) {
- case ACCEL:
-#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
- accelCalRun(&T(accelCal), T(timestampInt[LSM6DSM_INT1_INDEX]),
- mSensor->tADataEvt->samples[0].x,
- mSensor->tADataEvt->samples[0].y,
- mSensor->tADataEvt->samples[0].z, T(currentTemperature));
-
- accelCalBiasRemove(&T(accelCal),
- &mSensor->tADataEvt->samples[0].x,
- &mSensor->tADataEvt->samples[0].y,
- &mSensor->tADataEvt->samples[0].z);
-#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
-
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- if (T(sensors[GYRO].enabled)) {
- gyroCalUpdateAccel(&T(gyroCal), T(timestampInt[LSM6DSM_INT1_INDEX]),
- mSensor->tADataEvt->samples[0].x,
- mSensor->tADataEvt->samples[0].y,
- mSensor->tADataEvt->samples[0].z);
- }
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
- break;
-
- case GYRO:
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- gyroCalUpdateGyro(&T(gyroCal), T(timestampInt[LSM6DSM_INT1_INDEX]),
- mSensor->tADataEvt->samples[0].x,
- mSensor->tADataEvt->samples[0].y,
- mSensor->tADataEvt->samples[0].z, T(currentTemperature));
-
- gyroCalRemoveBias(&T(gyroCal),
- mSensor->tADataEvt->samples[0].x,
- mSensor->tADataEvt->samples[0].y,
- mSensor->tADataEvt->samples[0].z,
- &mSensor->tADataEvt->samples[0].x,
- &mSensor->tADataEvt->samples[0].y,
- &mSensor->tADataEvt->samples[0].z);
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
- break;
-
-#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- case MAGN: ;
- float magnOffX, magnOffY, magnOffZ;
-
- magCalRemoveSoftiron(&T(magnCal),
- mSensor->tADataEvt->samples[0].x,
- mSensor->tADataEvt->samples[0].y,
- mSensor->tADataEvt->samples[0].z,
- &magnOffX, &magnOffY, &magnOffZ);
-
- T(newMagnCalibData) = magCalUpdate(&T(magnCal), NS_TO_US(T(timestampInt[LSM6DSM_INT1_INDEX])), magnOffX, magnOffY, magnOffZ);
-
- magCalRemoveBias(&T(magnCal), magnOffX, magnOffY, magnOffZ,
- &mSensor->tADataEvt->samples[0].x,
- &mSensor->tADataEvt->samples[0].y,
- &mSensor->tADataEvt->samples[0].z);
- break;
-#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
-
- default:
- break;
- }
-
- mSensor->samplesCounter = 0;
- return true;
- }
- } else
- mSensor->samplesToDiscard--;
-
- return false;
+ return true;
}
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
- * lsm6dsm_processSensorOneAxisData: elaborate single axis sensors data
+ * lsm6dsm_threeAxisDataEvtFree: deallocate slab of three axis sensor.
+ * @ptr: sensor data pointer.
*/
-static bool lsm6dsm_processSensorOneAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, void *store)
+static void lsm6dsm_threeAxisDataEvtFree(void *ptr)
{
TDECL();
- struct SingleAxisDataEvent *pressData = (struct SingleAxisDataEvent *)store;
- union EmbeddedDataPoint *tempData = (union EmbeddedDataPoint *)store;
- if (mSensor->samplesToDiscard == 0) {
- mSensor->samplesCounter++;
+ slabAllocatorFree(T(mDataSlabThreeAxis), (struct TripleAxisDataEvent *)ptr);
+}
- if (mSensor->samplesCounter >= mSensor->samplesDecimator) {
- switch (mSensor->idx) {
- case PRESS:
- pressData->samples[0].fdata = ((data[2] << 16) | (data[1] << 8) | data[0]) * LSM6DSM_PRESS_KSCALE;
- pressData->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
- pressData->samples[0].firstSample.numSamples = 1;
- break;
- case TEMP:
- tempData->fdata = ((int16_t)(data[1] << 8) | data[0]) * LSM6DSM_TEMP_KSCALE;
- break;
- default:
- return false;
- }
+#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+/*
+ * lsm6dsm_allocateOneAxisDataEvt: allocate slab for one axis sensor data
+ * @mSensor: sensor info.
+ * @rtcTime: time of first sample in this block.
+ */
+static bool lsm6dsm_allocateOneAxisDataEvt(struct LSM6DSMSensor *mSensor, uint64_t rtcTime)
+{
+ TDECL();
- mSensor->samplesCounter = 0;
- return true;
- }
- } else
- mSensor->samplesToDiscard--;
+ mSensor->sADataEvt = slabAllocatorAlloc(T(mDataSlabOneAxis));
+ if (!mSensor->sADataEvt) {
+ ERROR_PRINT("Failed to allocate memory!\n");
+ return false;
+ }
- return false;
+ memset(&mSensor->sADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
+ mSensor->sADataEvt->referenceTime = rtcTime;
+ mSensor->pushedTimestamp = rtcTime;
+
+ return true;
+}
+
+/*
+ * lsm6dsm_oneAxisDataEvtFree: deallocate slab of one axis sensor
+ * @ptr: sensor data pointer.
+ */
+static void lsm6dsm_oneAxisDataEvtFree(void *ptr)
+{
+ TDECL();
+
+ slabAllocatorFree(T(mDataSlabOneAxis), (struct SingleAxisDataEvent *)ptr);
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
/*
+ * lsm6dsm_processSensorThreeAxisData: process three axis sensors data
+ * @mSensor: sensor info.
+ * @data: sensor data.
+ * @sampleNum: number of samples in the current slab.
+ * @timestamp: current sample timestamp;
+ */
+static bool lsm6dsm_processSensorThreeAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, uint16_t *sampleNum, uint64_t *timestamp)
+{
+ TDECL();
+ int16_t x, y, z;
+ float x_remap, y_remap, z_remap;
+ struct TripleAxisDataPoint *samples;
+
+ if (*timestamp == 0)
+ return false;
+
+ if (mSensor->tADataEvt == NULL) {
+ if (!lsm6dsm_allocateThreeAxisDataEvt(mSensor, *timestamp))
+ return false;
+ }
+ samples = mSensor->tADataEvt->samples;
+
+ x = (int16_t)(data[1] << 8) | data[0];
+ y = (int16_t)(data[3] << 8) | data[2];
+ z = (int16_t)(data[5] << 8) | data[4];
+
+ switch (mSensor->idx) {
+ case ACCEL:
+ x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE;
+ y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE;
+ z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE;
+
+#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
+ accelCalRun(&T(accelCal), *timestamp, x_remap, y_remap, z_remap, T(currentTemperature));
+ accelCalBiasRemove(&T(accelCal), &x_remap, &y_remap, &z_remap);
+
+ if (accelCalUpdateBias(&T(accelCal), &samples[*sampleNum].x, &samples[*sampleNum].y, &samples[*sampleNum].z)) {
+ if (!samples->firstSample.biasCurrent) {
+ samples->firstSample.biasCurrent = true;
+ samples->firstSample.biasPresent = 1;
+ samples->firstSample.biasSample = *sampleNum;
+
+ if (*sampleNum > 0)
+ samples[*sampleNum].deltaTime = 0;
+
+ *sampleNum += 1;
+ }
+ }
+#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
+
+#ifdef LSM6DSM_GYRO_CALIB_ENABLED
+ if (T(sensors[GYRO].enabled))
+ gyroCalUpdateAccel(&T(gyroCal), *timestamp, x_remap, y_remap, z_remap);
+#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
+
+ break;
+
+ case GYRO:
+ x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE;
+ y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE;
+ z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE;
+
+#ifdef LSM6DSM_GYRO_CALIB_ENABLED
+ gyroCalUpdateGyro(&T(gyroCal), *timestamp, x_remap, y_remap, z_remap, T(currentTemperature));
+
+#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
+ overTempCalSetTemperature(&T(overTempCal), *timestamp, T(currentTemperature));
+#else /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
+ gyroCalRemoveBias(&T(gyroCal), x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap);
+#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
+
+ if (gyroCalNewBiasAvailable(&T(gyroCal))) {
+ float biasTemperature, gyroOffset[3] = { 0.0f, 0.0f, 0.0f };
+
+ gyroCalGetBias(&T(gyroCal), &gyroOffset[0], &gyroOffset[1], &gyroOffset[2], &biasTemperature);
+
+ if (!samples->firstSample.biasCurrent) {
+ samples->firstSample.biasCurrent = true;
+ samples->firstSample.biasPresent = 1;
+ samples->firstSample.biasSample = *sampleNum;
+
+ if (*sampleNum > 0)
+ samples[*sampleNum].deltaTime = 0;
+
+ samples[*sampleNum].x = gyroOffset[0];
+ samples[*sampleNum].y = gyroOffset[1];
+ samples[*sampleNum].z = gyroOffset[2];
+
+ *sampleNum += 1;
+ }
+
+#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
+ overTempCalUpdateSensorEstimate(&T(overTempCal), *timestamp, gyroOffset, biasTemperature);
+ overTempCalRemoveOffset(&T(overTempCal), *timestamp, x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap);
+#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
+ } else {
+#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
+ overTempCalRemoveOffset(&T(overTempCal), *timestamp, x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap);
+#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
+ }
+#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
+ break;
+
+#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
+ case MAGN: ;
+#ifdef LSM6DSM_MAGN_CALIB_ENABLED
+ bool newMagnCalibData;
+ float magnOffX, magnOffY, magnOffZ;
+#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
+
+ x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE;
+ y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE;
+ z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE;
+
+#ifdef LSM6DSM_MAGN_CALIB_ENABLED
+ magCalRemoveSoftiron(&T(magnCal), x_remap, y_remap, z_remap, &magnOffX, &magnOffY, &magnOffZ);
+ newMagnCalibData = magCalUpdate(&T(magnCal), NS_TO_US(*timestamp), magnOffX, magnOffY, magnOffZ);
+ magCalRemoveBias(&T(magnCal), magnOffX, magnOffY, magnOffZ, &x_remap, &y_remap, &z_remap);
+
+ if (newMagnCalibData && !samples->firstSample.biasCurrent) {
+ samples->firstSample.biasCurrent = true;
+ samples->firstSample.biasPresent = 1;
+ samples->firstSample.biasSample = *sampleNum;
+
+ if (*sampleNum > 0)
+ samples[*sampleNum].deltaTime = 0;
+
+ magCalGetBias(&T(magnCal), &samples[*sampleNum].x, &samples[*sampleNum].y, &samples[*sampleNum].z);
+
+ *sampleNum += 1;
+ }
+#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
+
+ break;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
+ default:
+ return false;
+ }
+
+ if (++mSensor->samplesDecimatorCounter >= mSensor->samplesDecimator) {
+ samples[*sampleNum].x = x_remap;
+ samples[*sampleNum].y = y_remap;
+ samples[*sampleNum].z = z_remap;
+
+ if (*sampleNum > 0) {
+ samples[*sampleNum].deltaTime = *timestamp - mSensor->pushedTimestamp;
+ mSensor->pushedTimestamp = *timestamp;
+ }
+
+ *sampleNum += 1;
+
+ mSensor->samplesDecimatorCounter = 0;
+ }
+
+ return true;
+}
+
+#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+/*
+ * lsm6dsm_processSensorOneAxisData: process single axis sensors data
+ * @mSensor: sensor info.
+ * @data: sensor data.
+ * @sampleNum: number of samples in the current slab.
+ * @timestamp: current sample timestamp;
+ */
+static bool lsm6dsm_processSensorOneAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, uint16_t *sampleNum, uint64_t *timestamp)
+{
+ TDECL();
+
+ if (*timestamp == 0)
+ return false;
+
+ if (++mSensor->samplesDecimatorCounter >= mSensor->samplesDecimator) {
+ if (mSensor->sADataEvt == NULL) {
+ if (!lsm6dsm_allocateOneAxisDataEvt(mSensor, *timestamp))
+ return false;
+ }
+
+ switch (mSensor->idx) {
+ case PRESS:
+ mSensor->sADataEvt->samples[*sampleNum].fdata = ((data[2] << 16) | (data[1] << 8) | data[0]) * LSM6DSM_PRESS_KSCALE;
+ break;
+ default:
+ return false;
+ }
+
+ if (*sampleNum > 0) {
+ mSensor->sADataEvt->samples[*sampleNum].deltaTime = *timestamp - mSensor->pushedTimestamp;
+ mSensor->pushedTimestamp = *timestamp;
+ }
+
+ *sampleNum += 1;
+
+ mSensor->samplesDecimatorCounter = 0;
+ }
+
+ return true;
+}
+#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+
+/*
+ * lsm6dsm_pushData: push slab to nanohub
+ * @sidx: sensor index.
+ * @numSamples: number of samples in the slab.
+ */
+static void lsm6dsm_pushData(enum SensorIndex sidx, uint16_t *numSamples)
+{
+ TDECL();
+ bool triaxial = true;
+
+#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ if (sidx == PRESS)
+ triaxial = false;
+#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+
+ if (triaxial) {
+ T(sensors[sidx]).tADataEvt->samples[0].firstSample.numSamples = *numSamples;
+ osEnqueueEvtOrFree(sensorGetMyEventType(LSM6DSMSensorInfo[sidx].sensorType), T(sensors[sidx]).tADataEvt, lsm6dsm_threeAxisDataEvtFree);
+ T(sensors[sidx]).tADataEvt = NULL;
+ } else {
+#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ T(sensors[sidx]).sADataEvt->samples[0].firstSample.numSamples = *numSamples;
+ osEnqueueEvtOrFree(sensorGetMyEventType(LSM6DSMSensorInfo[sidx].sensorType), T(sensors[sidx]).sADataEvt, lsm6dsm_oneAxisDataEvtFree);
+ T(sensors[sidx]).sADataEvt = NULL;
+#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+ }
+
+ *numSamples = 0;
+}
+
+/*
+ * lsm6dsm_parseFifoData: processing FIFO data.
+ * @data: FIFO data.
+ * @numPattern: number of pattern inside data.
+ */
+static void lsm6dsm_parseFifoData(uint8_t *data, uint16_t numPattern)
+{
+ TDECL();
+ uint16_t j, fifoCounter = 0, samplesCounter[FIFO_NUM] = { 0 };
+ struct LSM6DSMSensor *sensor;
+ uint32_t sampleTimestamp;
+ int32_t timestampDiffLSB;
+ uint64_t timestamp = 0;
+ enum SensorIndex sidx;
+ uint8_t i, n;
+
+ for (j = 0; j < numPattern; j++) {
+ for (i = 0; i < T(fifoCntl).maxMinDecimator; i++) {
+ sampleTimestamp = ((data[fifoCounter + T(fifoCntl).timestampPosition[i] + 1] << 16) |
+ (data[fifoCounter + T(fifoCntl).timestampPosition[i]] << 8) |
+ data[fifoCounter + T(fifoCntl).timestampPosition[i] + 3]);
+
+ if (T(time).sampleTimestampFromFifoLSB > 0) {
+ timestampDiffLSB = (int32_t)sampleTimestamp - (int32_t)(T(time).sampleTimestampFromFifoLSB & LSM6DSM_MASK_24BIT_TIMESTAMP);
+
+ if ((timestampDiffLSB < 0) || (timestampDiffLSB > (T(time).theoreticalDeltaTimeLSB + T(time).deltaTimeMarginLSB))) {
+ if (timestampDiffLSB < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB) {
+ T(time).sampleTimestampFromFifoLSB += (UINT32_MAX >> 8) + 1;
+ } else {
+ if (T(time).timestampIsValid)
+ sampleTimestamp = (T(time).sampleTimestampFromFifoLSB & LSM6DSM_MASK_24BIT_TIMESTAMP) + T(time).theoreticalDeltaTimeLSB;
+ else
+ sampleTimestamp = 0;
+ }
+ } else
+ T(time).timestampIsValid = true;
+ }
+
+ T(time).sampleTimestampFromFifoLSB = (T(time).sampleTimestampFromFifoLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sampleTimestamp;
+
+ if (T(time).timestampIsValid) {
+ if (!time_sync_estimate_time1(&T(time).sensorTimeToRtcData, (uint64_t)T(time).sampleTimestampFromFifoLSB * LSM6DSM_TIME_RESOLUTION, ×tamp)) {
+ timestamp = 0;
+ } else {
+ if (T(time).lastSampleTimestamp > 0) {
+ if ((int64_t)timestamp <= (int64_t)T(time).lastSampleTimestamp)
+ timestamp = 0;
+ }
+
+ T(time).lastSampleTimestamp = timestamp > 0 ? timestamp : T(time).lastSampleTimestamp;
+
+ }
+ }
+
+ for (n = 0; n < FIFO_NUM; n++) {
+ if ((T(fifoCntl).decimators[n] > 0) && ((i % (T(fifoCntl).decimators[n] / T(fifoCntl).minDecimator)) == 0)) {
+ sidx = T(fifoCntl).decimatorsIdx[n];
+ if (sidx != EMBEDDED_TIMESTAMP) {
+ sensor = &T(sensors[sidx]);
+
+ if (sensor->samplesToDiscard == 0) {
+ if (++sensor->samplesFifoDecimatorCounter >= sensor->samplesFifoDecimator) {
+ switch (sidx) {
+ case GYRO:
+ case ACCEL:
+#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
+ case MAGN:
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+ lsm6dsm_processSensorThreeAxisData(sensor, &data[fifoCounter], &samplesCounter[sidx], ×tamp);
+ break;
+
+#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
+ case PRESS:
+ if (T(sensors[PRESS]).enabled)
+ lsm6dsm_processSensorOneAxisData(sensor, &data[fifoCounter], &samplesCounter[sidx], ×tamp);
+
+ if (T(sensors[TEMP]).enabled) {
+ union EmbeddedDataPoint tempData;
+
+ tempData.fdata = ((int16_t)(data[fifoCounter + LSM6DSM_PRESS_OUTDATA_LEN + 1] << 8) |
+ data[fifoCounter + LSM6DSM_PRESS_OUTDATA_LEN]) * LSM6DSM_TEMP_KSCALE;
+
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL);
+ }
+
+ break;
+#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED, LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
+ default:
+ break;
+ }
+
+ sensor->samplesFifoDecimatorCounter = 0;
+
+ if (samplesCounter[sidx] >= (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE - 1))
+ lsm6dsm_pushData(sidx, &samplesCounter[sidx]);
+ }
+ } else
+ sensor->samplesToDiscard--;
+ } else {
+ if (T(sensors[STEP_COUNTER].enabled) && !T(readSteps)) {
+ uint16_t steps = data[fifoCounter + 4] | (data[fifoCounter + 5] << 8);
+
+ if (steps != T(totalNumSteps)) {
+ union EmbeddedDataPoint stepCntData;
+
+ stepCntData.idata = steps;
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), stepCntData.vptr, NULL);
+ DEBUG_PRINT("Step Counter update: %ld steps\n", stepCntData.idata);
+ T(totalNumSteps) = stepCntData.idata;
+ }
+ }
+ }
+
+ fifoCounter += LSM6DSM_ONE_SAMPLE_BYTE;
+ }
+ }
+ }
+ }
+
+ for (n = 0; n < FIFO_NUM; n++) {
+ sidx = T(fifoCntl).decimatorsIdx[n];
+
+ if (samplesCounter[sidx])
+ lsm6dsm_pushData(sidx, &samplesCounter[sidx]);
+ }
+}
+
+/*
+ * lsm6dsm_updateSyncTaskValues: read timestamp used for time calibration and temperature
+ */
+static inline void lsm6dsm_updateSyncTaskValues(void)
+{
+ TDECL();
+ uint32_t sensorTimestamp;
+
+ sensorTimestamp = ((T_SLAVE_INTERFACE(timestampDataBuffer[1]) << 0) |
+ (T_SLAVE_INTERFACE(timestampDataBuffer[2]) << 8) |
+ (T_SLAVE_INTERFACE(timestampDataBuffer[3]) << 16));
+
+ if (T(time).timestampSyncTaskLSB > 0) {
+ if (((int32_t)sensorTimestamp - (int32_t)(T(time).timestampSyncTaskLSB & LSM6DSM_MASK_24BIT_TIMESTAMP)) < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB)
+ T(time).timestampSyncTaskLSB += (UINT32_MAX >> 8) + 1;
+ }
+
+ T(time).timestampSyncTaskLSB = (T(time).timestampSyncTaskLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sensorTimestamp;
+
+ time_sync_add(&T(time).sensorTimeToRtcData, T(time).timeSyncRtcTime, (uint64_t)T(time).timestampSyncTaskLSB * LSM6DSM_TIME_RESOLUTION);
+
+#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
+ T(currentTemperature) = LSM6DSM_TEMP_OFFSET +
+ (float)((int16_t)((T_SLAVE_INTERFACE(tempDataBuffer[2]) << 8) | T_SLAVE_INTERFACE(tempDataBuffer[1]))) / 256.0f;
+#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
+}
+
+/*
* lsm6dsm_handleSpiDoneEvt: all SPI operation fall back here
+ * @evtData: event data.
*/
static void lsm6dsm_handleSpiDoneEvt(const void *evtData)
{
TDECL();
- bool returnIdle = false, validData;
+ bool returnIdle = false, resetFIFO = false;
struct LSM6DSMSensor *mSensor;
int i;
@@ -2707,12 +3694,13 @@
case SENSOR_INITIALIZATION:
if (T(initState) == INIT_DONE) {
- for (i = 0; i < NUM_SENSORS; i++)
+ for (i = 0; i < NUM_SENSORS; i++) {
sensorRegisterInitComplete(T(sensors[i]).handle);
+ }
- returnIdle = true;
- } else
- lsm6dsm_sensorInit();
+ returnIdle = true;
+ } else
+ lsm6dsm_sensorInit();
break;
@@ -2735,194 +3723,228 @@
case SENSOR_CONFIG_CHANGING:
mSensor = (struct LSM6DSMSensor *)evtData;
- sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency);
+ sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate[mSensor->idx], mSensor->latency);
+ returnIdle = true;
+ break;
+
+ case SENSOR_CONFIG_WATERMARK_CHANGING:
returnIdle = true;
break;
case SENSOR_INT1_STATUS_REG_HANDLING:
- if (T(sensors[STEP_DETECTOR].enabled)) {
- if (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_DETECTED) {
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), NULL, NULL);
- DEBUG_PRINT("Step Detected!\n");
- }
+ if (T(sensors[STEP_DETECTOR].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_DETECTED)) {
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), NULL, NULL);
+ DEBUG_PRINT("Step Detected!\n");
}
- if (T(sensors[STEP_COUNTER].enabled)) {
- if (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA) {
+ if (T(sensors[SIGN_MOTION].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_SIGN_MOTION)) {
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), NULL, NULL);
+ DEBUG_PRINT("Significant Motion event!\n");
+ }
+
+ if ((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_ERROR) == 0) {
+ T(fifoDataToRead) = (((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_CTRL2_FTH_MASK) << 8) | T_SLAVE_INTERFACE(fifoStatusRegBuffer[1])) * 2;
+
+ if (T(fifoDataToRead) > LSM6DSM_SPI_FIFO_SIZE) {
+ T(fifoDataToReadPending) = T(fifoDataToRead);
+ T(fifoDataToRead) = LSM6DSM_SPI_FIFO_SIZE - (LSM6DSM_SPI_FIFO_SIZE % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE));
+ T(fifoDataToReadPending) -= T(fifoDataToRead);
+ } else {
+ T(fifoDataToReadPending) = 0;
+
+ if (T(fifoDataToRead) >= (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE))
+ T(fifoDataToRead) -= T(fifoDataToRead) % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE);
+ else
+ T(fifoDataToRead) = 0;
+ }
+
+ if (T(fifoDataToRead) > 0) {
+ if (T(time).status == TIME_SYNC_DURING_FIFO_READ) {
+ uint64_t time = sensorGetTime();
+ if ((time - T(time).noTimer.lastTimestampDataAvlRtcTime) > LSM6DSM_SYNC_DELTA_INTERVAL) {
+ T(time).noTimer.newTimestampDataAvl = true;
+ T(time).noTimer.lastTimestampDataAvlRtcTime = time;
+
+ SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBuffer));
+#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
+ SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer));
+#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
+ }
+ }
+
+ SPI_READ(LSM6DSM_FIFO_DATA_OUT_L_ADDR, T(fifoDataToRead), &T_SLAVE_INTERFACE(fifoDataBuffer));
+ }
+ } else {
+ T(fifoDataToRead) = 0;
+
+ if ((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART) ||
+ (T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN)) {
+ resetFIFO = true;
+ SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_BYPASS_MODE, 25);
+ SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_CONTINUOS_MODE);
+ }
+
+ if (T(sensors[STEP_COUNTER].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA)) {
T(readSteps) = true;
SPI_READ(LSM6DSM_STEP_COUNTER_L_ADDR, 2, &T_SLAVE_INTERFACE(stepCounterDataBuffer));
}
}
- if (T(sensors[SIGN_MOTION].enabled)) {
- if (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_SIGN_MOTION) {
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), NULL, NULL);
- DEBUG_PRINT("Significant Motion event!\n");
+ if (!T(readSteps) && (T(fifoDataToRead) == 0)) {
+ for (i = 0; i < NUM_SENSORS; i++) {
+ if (T(sendFlushEvt[i])) {
+ osEnqueueEvt(sensorGetMyEventType(LSM6DSMSensorInfo[i].sensorType), SENSOR_DATA_EVENT_FLUSH, NULL);
+ T(sendFlushEvt[i]) = false;
+ }
}
- }
-#ifdef LSM6DSM_I2C_MASTER_ENABLED
- if (T(masterConfigRegister) & LSM6DSM_MASTER_CONFIG_MASTER_ON) {
- T(statusRegisterSH) = T_SLAVE_INTERFACE(statusRegBuffer[1]) & LSM6DSM_FUNC_SRC_SENSOR_HUB_END_OP;
- if (T(statusRegisterSH))
- SPI_READ(LSM6DSM_SENSORHUB1_REG_ADDR, LSM6DSM_SH_READ_BYTE_NUM, &T_SLAVE_INTERFACE(SHDataBuffer));
- }
-#endif /* LSM6DSM_I2C_MASTER_ENABLED */
+ if (resetFIFO) {
+ SET_STATE(SENSOR_INVALID_STATE);
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ } else
+ returnIdle = true;
-#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
- T(statusRegisterTDA) = T_SLAVE_INTERFACE(statusRegBuffer[1]) & LSM6DSM_STATUS_REG_TDA;
- if (T(statusRegisterTDA))
- SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer));
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
-
- T(statusRegisterDA) = T_SLAVE_INTERFACE(statusRegBuffer[1]) & (LSM6DSM_STATUS_REG_XLDA | LSM6DSM_STATUS_REG_GDA);
-
- switch(T(statusRegisterDA)) {
- case LSM6DSM_STATUS_REG_XLDA:
- SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(accelDataBuffer));
- break;
-
- case LSM6DSM_STATUS_REG_GDA:
- SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(gyroDataBuffer));
- break;
-
- case (LSM6DSM_STATUS_REG_XLDA | LSM6DSM_STATUS_REG_GDA):
- SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(accelDataBuffer));
- SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(gyroDataBuffer));
- break;
-
- default:
- if (!T(readSteps)) {
- SET_STATE(SENSOR_IDLE);
- lsm6dsm_processPendingEvt();
- return;
- }
break;
}
SET_STATE(SENSOR_INT1_OUTPUT_DATA_HANDLING);
+
+ if (T(fifoDataToRead) > 0) {
+ T(lastFifoReadTimestamp) = sensorGetTime();
+
+ if (T(time).noTimer.newTimestampDataAvl)
+ T(time).timeSyncRtcTime = T(lastFifoReadTimestamp);
+ }
+
lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+
break;
case SENSOR_INT1_OUTPUT_DATA_HANDLING:
- if (T(readSteps)) {
- union EmbeddedDataPoint step_cnt;
-
- step_cnt.idata = T_SLAVE_INTERFACE(stepCounterDataBuffer[1]) | (T_SLAVE_INTERFACE(stepCounterDataBuffer[2]) << 8);
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), step_cnt.vptr, NULL);
- DEBUG_PRINT("Step Counter update: %ld steps\n", step_cnt.idata);
- T(totalNumSteps) = step_cnt.idata;
- T(readSteps) = false;
- }
-
-#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
- if (T(statusRegisterTDA)) {
- T(currentTemperature) = LSM6DSM_TEMP_OFFSET +
- (float)((int16_t)((T_SLAVE_INTERFACE(tempDataBuffer[2]) << 8) | T_SLAVE_INTERFACE(tempDataBuffer[1]))) / 256.0f;
- }
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
-
-#ifdef LSM6DSM_I2C_MASTER_ENABLED
- if (T(statusRegisterSH)) {
- T(statusRegisterSH) = 0;
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- if (T(sensors[MAGN].enabled)) {
- validData = lsm6dsm_processSensorThreeAxisData(&T(sensors[MAGN]), &T_SLAVE_INTERFACE(SHDataBuffer[1]));
- if (validData) {
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), T(sensors[MAGN]).tADataEvt, NULL);
-
-#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- if T(newMagnCalibData) {
- magCalGetBias(&T(magnCal), &T(magnCalDataEvt)->samples[0].x,
- &T(magnCalDataEvt)->samples[0].y, &T(magnCalDataEvt)->samples[0].z);
-
- T(newMagnCalibData) = false;
- T(magnCalDataEvt)->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG_BIAS), T(magnCalDataEvt), NULL);
- }
-#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
- }
- }
-#endif /* LSM6DSM_MAGN_CALIB_MAGNETOMETER_ENABLED */
-
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- if (T(sensors[PRESS].enabled)) {
- validData = lsm6dsm_processSensorOneAxisData(&T(sensors[PRESS]),
- &T_SLAVE_INTERFACE(SHDataBuffer[LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN + 1]), T(sensors[PRESS]).sADataEvt);
- if (validData)
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), T(sensors[PRESS]).sADataEvt, NULL);
+ if (T(fifoDataToRead) > 0) {
+ if (T(time).noTimer.newTimestampDataAvl) {
+ T(time).noTimer.newTimestampDataAvl = false;
+ lsm6dsm_updateSyncTaskValues();
}
- if (T(sensors[TEMP].enabled)) {
- union EmbeddedDataPoint tempData;
+ lsm6dsm_parseFifoData(&T_SLAVE_INTERFACE(fifoDataBuffer[1]), (T(fifoDataToRead) / 6) / T(fifoCntl).totalSip);
- validData = lsm6dsm_processSensorOneAxisData(&T(sensors[TEMP]),
- &T_SLAVE_INTERFACE(SHDataBuffer[LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN + LSM6DSM_PRESS_OUTDATA_LEN + 1]), &tempData);
- if (validData)
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL);
- }
-#endif /* LSM6DSM_MAGN_CALIB_BAROMETER_ENABLED */
- }
-#endif /* LSM6DSM_I2C_MASTER_ENABLED */
+ if (T(fifoDataToReadPending) > 0) {
+ T(fifoDataToRead) = T(fifoDataToReadPending);
- if (T(statusRegisterDA) & LSM6DSM_STATUS_REG_XLDA) {
- validData = lsm6dsm_processSensorThreeAxisData(&T(sensors[ACCEL]), &T_SLAVE_INTERFACE(accelDataBuffer[1]));
- if (validData) {
- if (T(sensors[ACCEL].enabled)) {
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), T(sensors[ACCEL]).tADataEvt, NULL);
+ if (T(fifoDataToRead) > LSM6DSM_SPI_FIFO_SIZE) {
+ T(fifoDataToReadPending) = T(fifoDataToRead);
+ T(fifoDataToRead) = LSM6DSM_SPI_FIFO_SIZE - (LSM6DSM_SPI_FIFO_SIZE % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE));
+ T(fifoDataToReadPending) -= T(fifoDataToRead);
} else {
-#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
- if (accelCalUpdateBias(&T(accelCal), &T(accelBiasDataEvt)->samples[0].x,
- &T(accelBiasDataEvt)->samples[0].y, &T(accelBiasDataEvt)->samples[0].z)) {
- T(accelBiasDataEvt)->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL_BIAS), T(accelBiasDataEvt), NULL);
- }
-#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
+ T(fifoDataToReadPending) = 0;
+
+ if (T(fifoDataToRead) >= (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE))
+ T(fifoDataToRead) -= T(fifoDataToRead) % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE);
+ else
+ T(fifoDataToRead) = 0;
}
+
+ if (T(fifoDataToRead) > 0) {
+ SPI_READ(LSM6DSM_FIFO_DATA_OUT_L_ADDR, T(fifoDataToRead), &T_SLAVE_INTERFACE(fifoDataBuffer));
+ lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
+ return;
+ }
+ } else
+ T(fifoDataToRead) = 0;
+ }
+
+ for (i = 0; i < NUM_SENSORS; i++) {
+ if (T(sendFlushEvt[i])) {
+ osEnqueueEvt(sensorGetMyEventType(LSM6DSMSensorInfo[i].sensorType), SENSOR_DATA_EVENT_FLUSH, NULL);
+ T(sendFlushEvt[i]) = false;
}
}
- if (T(statusRegisterDA) & LSM6DSM_STATUS_REG_GDA) {
- validData = lsm6dsm_processSensorThreeAxisData(&T(sensors[GYRO]), &T_SLAVE_INTERFACE(gyroDataBuffer[1]));
- if (validData) {
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO), T(sensors[GYRO]).tADataEvt, NULL);
+ if (T(readSteps)) {
+ union EmbeddedDataPoint stepCntData;
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- if (gyroCalNewBiasAvailable(&T(gyroCal))) {
- float gyroTemp = 0;
- gyroCalGetBias(&T(gyroCal), &T(gyroBiasDataEvt)->samples[0].x,
- &T(gyroBiasDataEvt)->samples[0].y, &T(gyroBiasDataEvt)->samples[0].z, &gyroTemp);
-
- T(gyroBiasDataEvt)->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
- osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO_BIAS), T(gyroBiasDataEvt), NULL);
- }
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
- }
+ stepCntData.idata = T_SLAVE_INTERFACE(stepCounterDataBuffer[1]) | (T_SLAVE_INTERFACE(stepCounterDataBuffer[2]) << 8);
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), stepCntData.vptr, NULL);
+ DEBUG_PRINT("Step Counter update: %ld steps\n", stepCntData.idata);
+ T(totalNumSteps) = stepCntData.idata;
+ T(readSteps) = false;
}
returnIdle = true;
break;
+ case SENSOR_TIME_SYNC: ;
+ lsm6dsm_updateSyncTaskValues();
+
+ if (T(time).status == TIME_SYNC_TIMER) {
+ if (timTimerSet(LSM6DSM_SYNC_DELTA_INTERVAL, 100, 100, lsm6dsm_timerSyncCallback, NULL, true) == 0)
+ ERROR_PRINT("Failed to set a timer for time sync\n");
+ }
+
+ returnIdle = true;
+ break;
+
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ case SENSOR_BARO_READ_DATA: ;
+ uint16_t samplesCounter = 0;
+ uint32_t sensorTimestamp;
+ uint64_t timestamp;
+
+ sensorTimestamp = ((T_SLAVE_INTERFACE(timestampDataBufferBaro[1]) << 0) |
+ (T_SLAVE_INTERFACE(timestampDataBufferBaro[2]) << 8) |
+ (T_SLAVE_INTERFACE(timestampDataBufferBaro[3]) << 16));
+
+ if (T(time).timestampBaroLSB > 0) {
+ if (((int32_t)sensorTimestamp - (int32_t)(T(time).timestampBaroLSB & LSM6DSM_MASK_24BIT_TIMESTAMP)) < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB)
+ T(time).timestampBaroLSB += (UINT32_MAX >> 8) + 1;
+ }
+
+ T(time).timestampBaroLSB = (T(time).timestampBaroLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sensorTimestamp;
+
+ if (time_sync_estimate_time1(&T(time).sensorTimeToRtcData, (uint64_t)T(time).timestampBaroLSB * LSM6DSM_TIME_RESOLUTION, ×tamp)) {
+ if (T(sensors[PRESS]).enabled) {
+ lsm6dsm_processSensorOneAxisData(&T(sensors[PRESS]), &T_SLAVE_INTERFACE(baroDataBuffer[1]), &samplesCounter, ×tamp);
+ lsm6dsm_pushData(PRESS, &samplesCounter);
+ }
+ }
+
+ if (T(sensors[TEMP]).enabled) {
+ union EmbeddedDataPoint tempData;
+
+ tempData.fdata = ((int16_t)(T_SLAVE_INTERFACE(baroDataBuffer[LSM6DSM_PRESS_OUTDATA_LEN + 2]) << 8) |
+ T_SLAVE_INTERFACE(baroDataBuffer[LSM6DSM_PRESS_OUTDATA_LEN + 1])) * LSM6DSM_TEMP_KSCALE;
+
+ osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL);
+ }
+
+ returnIdle = true;
+ break;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
+
default:
+ returnIdle = true;
break;
}
- if (returnIdle) {
- SET_STATE(SENSOR_IDLE);
+ if (returnIdle)
lsm6dsm_processPendingEvt();
- }
}
/*
* lsm6dsm_handleEvent: handle driver events
+ * @evtType: event type.
+ * @evtData: event data.
*/
static void lsm6dsm_handleEvent(uint32_t evtType, const void *evtData)
{
TDECL();
- uint64_t currTime;
+ struct LSM6DSMSensor *mSensor;
switch (evtType) {
- case EVT_APP_START:
+ case EVT_APP_START: ;
+ uint64_t currTime;
+
T(mRetryLeft) = LSM6DSM_RETRY_CNT_WAI;
SET_STATE(SENSOR_BOOT);
osEventUnsubscribe(T(tid), EVT_APP_START);
@@ -2934,7 +3956,7 @@
break;
}
- /* If 100ms already passed, just fall through next step */
+ /* If 100ms already passed just fall through next step */
case EVT_SPI_DONE:
lsm6dsm_handleSpiDoneEvt(evtData);
break;
@@ -2943,9 +3965,36 @@
lsm6dsm_readStatusReg(false);
break;
+ case EVT_SENSOR_POWERING_UP:
+ mSensor = (struct LSM6DSMSensor *)evtData;
+
+ mSensor->enabled = true;
+ sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
+ lsm6dsm_processPendingEvt();
+ break;
+
+ case EVT_SENSOR_POWERING_DOWN:
+ mSensor = (struct LSM6DSMSensor *)evtData;
+
+ mSensor->enabled = false;
+ sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
+ lsm6dsm_processPendingEvt();
+ break;
+
+ case EVT_SENSOR_CONFIG_CHANGING:
+ mSensor = (struct LSM6DSMSensor *)evtData;
+
+ sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate[mSensor->idx], mSensor->latency);
+ lsm6dsm_processPendingEvt();
+ break;
+
case EVT_APP_FROM_HOST:
break;
+ case EVT_TIME_SYNC:
+ lsm6dsm_timeSyncTask();
+ break;
+
default:
break;
}
@@ -2953,56 +4002,52 @@
/*
* lsm6dsm_initSensorStruct: initialize sensor struct variable
+ * @sensor: sensor info.
+ * @idx: sensor index.
*/
static void lsm6dsm_initSensorStruct(struct LSM6DSMSensor *sensor, enum SensorIndex idx)
{
+ TDECL();
+ uint8_t i;
+
+ for (i = 0; i < NUM_SENSORS; i++) {
+ if (i == idx)
+ sensor->dependenciesRequireData[i] = true;
+ else
+ sensor->dependenciesRequireData[i] = false;
+
+ sensor->rate[i] = 0;
+ }
+
sensor->idx = idx;
- sensor->rate = 0;
sensor->hwRate = 0;
- sensor->latency = 0;
+ sensor->latency = UINT64_MAX;
sensor->enabled = false;
sensor->samplesToDiscard = 0;
sensor->samplesDecimator = 1;
- sensor->samplesCounter = 0;
+ sensor->samplesDecimatorCounter = 0;
+ sensor->samplesFifoDecimator = 1;
+ sensor->samplesFifoDecimatorCounter = 0;
sensor->tADataEvt = NULL;
+#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
sensor->sADataEvt = NULL;
-}
-
-/*
- * lsm6dsm_calculateSlabNumItems: calculate number of items needed to allocate memory
- */
-static uint8_t lsm6dsm_calculateSlabNumItems()
-{
- uint8_t slabElements = 2;
-
-#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
- slabElements++;
-#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- slabElements++;
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- slabElements++;
-#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
-#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- slabElements++;
-#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
-
- return slabElements;
+#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
}
/*
* lsm6dsm_startTask: first function executed when App start
+ * @taskId: task id.
*/
-static bool lsm6dsm_startTask(uint32_t task_id)
+static bool lsm6dsm_startTask(uint32_t taskId)
{
TDECL();
enum SensorIndex i;
+ size_t slabSize;
int err;
- DEBUG_PRINT("IMU: %lu\n", task_id);
+ DEBUG_PRINT("IMU: %lu\n", taskId);
- T(tid) = task_id;
+ T(tid) = taskId;
T(int1) = gpioRequest(LSM6DSM_INT1_GPIO);
T(isr1).func = lsm6dsm_isr1;
@@ -3014,7 +4059,7 @@
T_SLAVE_INTERFACE(mode).format = SPI_FORMAT_MSB_FIRST;
T_SLAVE_INTERFACE(cs) = LSM6DSM_SPI_SLAVE_CS_GPIO;
- DEBUG_PRINT("Requested SPI on bus #%d @ %dHz, int1 on gpio#%d\n",
+ DEBUG_PRINT("Requested SPI on bus #%d @%dHz, int1 on gpio#%d\n",
LSM6DSM_SPI_SLAVE_BUS_ID, LSM6DSM_SPI_SLAVE_FREQUENCY_HZ, LSM6DSM_INT1_GPIO);
err = spiMasterRequest(LSM6DSM_SPI_SLAVE_BUS_ID, &T_SLAVE_INTERFACE(spiDev));
@@ -3026,25 +4071,27 @@
T(int1Register) = LSM6DSM_INT1_CTRL_BASE;
T(int2Register) = LSM6DSM_INT2_CTRL_BASE;
T(embeddedFunctionsRegister) = LSM6DSM_CTRL10_C_BASE;
- T(accelSensorDependencies) = 0;
- T(embeddedFunctionsDependencies) = 0;
- T(pendingInt[LSM6DSM_INT1_INDEX]) = false;
- T(pendingInt[LSM6DSM_INT2_INDEX]) = false;
- T(timestampInt[LSM6DSM_INT1_INDEX]) = 0;
- T(timestampInt[LSM6DSM_INT2_INDEX]) = 0;
+ T(pedometerDependencies) = 0;
+ T(pendingInt) = false;
+ T(pendingTimeSyncTask) = false;
+ T(lastFifoReadTimestamp) = 0;
T(totalNumSteps) = 0;
- T(triggerRate) = SENSOR_HZ_RATE_TO_US(SENSOR_HZ(26.0f / 2.0f));
+ T(time).status = TIME_SYNC_DISABLED;
#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
T(currentTemperature) = 0;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
-#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- T(newMagnCalibData) = false;
-#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_ENABLED
T(masterConfigRegister) = LSM6DSM_MASTER_CONFIG_BASE;
+ T(masterConfigDependencies) = 0;
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
+#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
+ T(baroTimerId) = 0;
+ T(pendingBaroTimerTask) = false;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
- T(mDataSlabThreeAxis) = slabAllocatorNew(sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint), 4, lsm6dsm_calculateSlabNumItems());
+ slabSize = sizeof(struct TripleAxisDataEvent) + (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE * sizeof(struct TripleAxisDataPoint));
+
+ T(mDataSlabThreeAxis) = slabAllocatorNew(slabSize, 4, 20);
if (!T(mDataSlabThreeAxis)) {
ERROR_PRINT("Failed to allocate mDataSlabThreeAxis memory\n");
spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
@@ -3052,7 +4099,9 @@
}
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- T(mDataSlabOneAxis) = slabAllocatorNew(sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint), 4, 10);
+ slabSize = sizeof(struct SingleAxisDataEvent) + (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE * sizeof(struct SingleAxisDataPoint));
+
+ T(mDataSlabOneAxis) = slabAllocatorNew(slabSize, 4, 20);
if (!T(mDataSlabOneAxis)) {
ERROR_PRINT("Failed to allocate mDataSlabOneAxis memory\n");
slabAllocatorDestroy(T(mDataSlabThreeAxis));
@@ -3061,167 +4110,93 @@
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
-#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
- T(accelBiasDataEvt) = slabAllocatorAlloc(T(mDataSlabThreeAxis));
- if (!T(accelBiasDataEvt)) {
- ERROR_PRINT("Failed to allocate accelBiasDataEvt memory");
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- slabAllocatorDestroy(T(mDataSlabOneAxis));
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
- slabAllocatorDestroy(T(mDataSlabThreeAxis));
- spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
- return false;
- }
-
- memset(&T(accelBiasDataEvt)->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
- T(accelBiasDataEvt)->samples[0].firstSample.biasCurrent = true;
- T(accelBiasDataEvt)->samples[0].firstSample.biasPresent = 1;
- T(accelBiasDataEvt)->samples[0].firstSample.biasSample = 0;
- T(accelBiasDataEvt)->samples[0].firstSample.numSamples = 1;
-#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
-
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- T(gyroBiasDataEvt) = slabAllocatorAlloc(T(mDataSlabThreeAxis));
- if (!T(gyroBiasDataEvt)) {
- ERROR_PRINT("Failed to allocate gyroBiasDataEvt memory");
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- slabAllocatorDestroy(T(mDataSlabOneAxis));
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
- slabAllocatorDestroy(T(mDataSlabThreeAxis));
- spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
- return false;
- }
-
- memset(&T(gyroBiasDataEvt)->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
- T(gyroBiasDataEvt)->samples[0].firstSample.biasCurrent = true;
- T(gyroBiasDataEvt)->samples[0].firstSample.biasPresent = 1;
- T(gyroBiasDataEvt)->samples[0].firstSample.biasSample = 0;
- T(gyroBiasDataEvt)->samples[0].firstSample.numSamples = 1;
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
-
-#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- T(magnCalDataEvt) = slabAllocatorAlloc(T(mDataSlabThreeAxis));
- if (!T(magnCalDataEvt)) {
- ERROR_PRINT("Failed to allocate magnCalDataEvt memory");
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- slabAllocatorDestroy(T(mDataSlabOneAxis));
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
- slabAllocatorDestroy(T(mDataSlabThreeAxis));
- spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
- return false;
- }
-
- memset(&T(magnCalDataEvt)->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
- T(magnCalDataEvt)->samples[0].firstSample.biasCurrent = true;
- T(magnCalDataEvt)->samples[0].firstSample.biasPresent = 1;
- T(magnCalDataEvt)->samples[0].firstSample.biasSample = 0;
- T(magnCalDataEvt)->samples[0].firstSample.numSamples = 1;
-#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
-
for (i = 0; i < NUM_SENSORS; i++) {
T(pendingEnableConfig[i]) = false;
T(pendingRateConfig[i]) = false;
+ T(pendingFlush[i]) = 0;
+ T(sendFlushEvt[i]) = false;
lsm6dsm_initSensorStruct(&T(sensors[i]), i);
-
-#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
- if ((i == ACCEL) || (i == GYRO) || (i == MAGN)) {
-#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
- if ((i == ACCEL) || (i == GYRO)) {
-#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
- T(sensors[i]).tADataEvt = slabAllocatorAlloc(T(mDataSlabThreeAxis));
- if (!T(sensors[i]).tADataEvt) {
- ERROR_PRINT("Failed to allocate tADataEvt memory");
- goto unregister_sensors;
- }
-
- memset(&T(sensors[i]).tADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
- }
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- if (i == PRESS) {
- T(sensors[i]).sADataEvt = slabAllocatorAlloc(T(mDataSlabOneAxis));
- if (!T(sensors[i]).sADataEvt) {
- ERROR_PRINT("Failed to allocate sADataEvt memory");
- goto unregister_sensors;
- }
-
- memset(&T(sensors[i]).sADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
- }
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
-
T(sensors[i]).handle = sensorRegister(&LSM6DSMSensorInfo[i], &LSM6DSMSensorOps[i], NULL, false);
}
+ T(fifoCntl).decimatorsIdx[FIFO_GYRO] = GYRO;
+ T(fifoCntl).decimatorsIdx[FIFO_ACCEL] = ACCEL;
+ T(fifoCntl).decimatorsIdx[FIFO_DS3] = NUM_SENSORS;
+ T(fifoCntl).decimatorsIdx[FIFO_DS4] = EMBEDDED_TIMESTAMP;
+
+#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
+ T(fifoCntl).decimatorsIdx[FIFO_DS3] = MAGN;
+#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
+ T(fifoCntl).decimatorsIdx[FIFO_DS3] = PRESS;
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
+
#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
accelCalInit(&T(accelCal),
- 800000000, /* stillness period = 0.8 seconds [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 */
+ 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 */
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
gyroCalInit(&T(gyroCal),
- 5e9, /* min stillness period = 5 seconds [ns] */
- 6e9, /* max stillness period = 6 seconds [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 [C] */
- true); /* gyro calibration enabled */
+ 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 */
#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 */
+#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
+
#ifdef LSM6DSM_MAGN_CALIB_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 */
+ 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 */
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
/* Initialize index used to fill/get data from buffer */
T_SLAVE_INTERFACE(mWbufCnt) = 0;
T_SLAVE_INTERFACE(mRegCnt) = 0;
+ time_sync_init(&T(time).sensorTimeToRtcData);
+
osEventSubscribe(T(tid), EVT_APP_START);
DEBUG_PRINT("Enabling gpio#%d connected to int1\n", LSM6DSM_INT1_GPIO);
lsm6dsm_enableInterrupt(T(int1), &T(isr1));
return true;
-
-unregister_sensors:
- for (i--; i >= ACCEL; i--)
- sensorUnregister(T(sensors[i]).handle);
-
-#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
- accelCalDestroy(&T(accelCal));
-#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
-#ifdef LSM6DSM_MAGN_CALIB_ENABLED
- magCalDestroy(&T(magnCal));
-#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
-#ifdef LSM6DSM_GYRO_CALIB_ENABLED
- gyroCalDestroy(&T(gyroCal));
-#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
-#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
- slabAllocatorDestroy(T(mDataSlabOneAxis));
-#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
- slabAllocatorDestroy(T(mDataSlabThreeAxis));
- spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
- return false;
}
/*
diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_ak09916_slave.h b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_ak09916_slave.h
index caaff54..9574d07 100644
--- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_ak09916_slave.h
+++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_ak09916_slave.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 STMicroelectronics
+ * Copyright (C) 2016-2017 STMicroelectronics
*
* Author: Denis Ciocca <denis.ciocca@st.com>
*
@@ -21,42 +21,44 @@
#ifdef LSM6DSM_I2C_MASTER_AK09916
#ifndef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
-#define LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1
+#define LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#error "Another magnetometer is already selected! One magn per time can be used."
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
-#define AK09916_KSCALE 0.15 /* MAGN scale in uT/LSB */
-#define AK09916_I2C_ADDRESS (0x0c)
+#define AK09916_KSCALE 0.15 /* MAGN scale in uT/LSB */
+#define AK09916_I2C_ADDRESS (0x0c)
/* AK09916 registers */
-#define AK09916_WAI_ADDR (0x00)
-#define AK09916_CNTL2_ADDR (0x31)
-#define AK09916_CNTL3_ADDR (0x32)
-#define AK09916_OUTDATA_ADDR (0x11)
-#define AK09916_STATUS_DATA_ADDR (0x18)
+#define AK09916_WAI_ADDR (0x00)
+#define AK09916_CNTL2_ADDR (0x31)
+#define AK09916_CNTL3_ADDR (0x32)
+#define AK09916_OUTDATA_ADDR (0x11)
+#define AK09916_STATUS_DATA_ADDR (0x18)
-#define AK09916_SW_RESET (0x01)
-#define AK09916_POWER_ON_VALUE (0x02)
-#define AK09916_POWER_OFF_VALUE (0x00)
-#define AK09916_OUTDATA_LEN (0x06)
+#define AK09916_SW_RESET (0x01)
+#define AK09916_POWER_ON_VALUE (0x02)
+#define AK09916_POWER_OFF_VALUE (0x00)
+#define AK09916_OUTDATA_LEN (0x06)
/* AK09916 default base registers status */
/* AK09916_CNTL2_BASE: control register 2 default settings */
-#define AK09916_CNTL2_BASE ((0 << 7) | /* (0) */ \
- (0 << 6) | /* (0) */ \
- (0 << 5) | /* (0) */ \
- (0 << 4) | /* MODE4 */ \
- (0 << 3) | /* MODE3 */ \
- (0 << 2) | /* MODE2 */ \
- (0 << 1) | /* MODE1 */ \
- (0 << 0)) /* MODE0 */
+#define AK09916_CNTL2_BASE ((0 << 7) | /* (0) */ \
+ (0 << 6) | /* (0) */ \
+ (0 << 5) | /* (0) */ \
+ (0 << 4) | /* MODE4 */ \
+ (0 << 3) | /* MODE3 */ \
+ (0 << 2) | /* MODE2 */ \
+ (0 << 1) | /* MODE1 */ \
+ (0 << 0)) /* MODE0 */
#ifdef LSM6DSM_I2C_MASTER_AK09916
/* MUST BE SAME LENGTH OF LSM6DSMMagnRates */
static uint8_t AK09916MagnRatesRegValue[] = {
+ 0x02, /* Expected 0.8125Hz, ODR = 10Hz */
+ 0x02, /* Expected 1.625Hz, ODR = 10Hz */
0x02, /* Expected 3.25Hz, ODR = 10Hz */
0x02, /* Expected 6.5Hz, ODR = 10Hz */
0x04, /* Expected 12.5Hz, ODR = 20Hz */
diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lis3mdl_slave.h b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lis3mdl_slave.h
index 6650e75..bbf445e 100644
--- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lis3mdl_slave.h
+++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lis3mdl_slave.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 STMicroelectronics
+ * Copyright (C) 2016-2017 STMicroelectronics
*
* Author: Denis Ciocca <denis.ciocca@st.com>
*
@@ -21,77 +21,79 @@
#ifdef LSM6DSM_I2C_MASTER_LIS3MDL
#ifndef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
-#define LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1
+#define LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#error "Another magnetometer is already selected! One magn per time can be used."
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#endif /* LSM6DSM_I2C_MASTER_LIS3MDL */
-#define LIS3MDL_KSCALE 0.043840420868f /* MAGN scale @12G in uT/LSB */
-#define LIS3MDL_I2C_ADDRESS (0x1c) /* SDO = 1 : 0x1e --- SDO = 0 : 0x1c */
+#define LIS3MDL_KSCALE 0.043840420868f /* MAGN scale @12G in uT/LSB */
+#define LIS3MDL_I2C_ADDRESS (LSM6DSM_LIS3MDL_I2C_ADDRESS)
/* LIS3MDL registers */
-#define LIS3MDL_WAI_ADDR (0x0f)
-#define LIS3MDL_CTRL1_ADDR (0x20)
-#define LIS3MDL_CTRL2_ADDR (0x21)
-#define LIS3MDL_CTRL3_ADDR (0x22)
-#define LIS3MDL_CTRL4_ADDR (0x23)
-#define LIS3MDL_CTRL5_ADDR (0x24)
-#define LIS3MDL_OUTDATA_ADDR (0x28)
+#define LIS3MDL_WAI_ADDR (0x0f)
+#define LIS3MDL_CTRL1_ADDR (0x20)
+#define LIS3MDL_CTRL2_ADDR (0x21)
+#define LIS3MDL_CTRL3_ADDR (0x22)
+#define LIS3MDL_CTRL4_ADDR (0x23)
+#define LIS3MDL_CTRL5_ADDR (0x24)
+#define LIS3MDL_OUTDATA_ADDR (0x28)
-#define LIS3MDL_SW_RESET (0x04)
-#define LIS3MDL_POWER_ON_VALUE (0x00)
-#define LIS3MDL_POWER_OFF_VALUE (0x03)
-#define LIS3MDL_OUTDATA_LEN (0x06)
+#define LIS3MDL_SW_RESET (0x04)
+#define LIS3MDL_POWER_ON_VALUE (0x00)
+#define LIS3MDL_POWER_OFF_VALUE (0x03)
+#define LIS3MDL_OUTDATA_LEN (0x06)
/* LIS3MDL default base registers status */
/* LIS3MDL_CTRL1_BASE: control register 1 default settings */
-#define LIS3MDL_CTRL1_BASE ((0 << 7) | /* TEMP_EN */ \
- (1 << 6) | /* OM1 */ \
- (1 << 5) | /* OM0 */ \
- (0 << 4) | /* DO2 */ \
- (0 << 3) | /* DO1 */ \
- (0 << 2) | /* DO0 */ \
- (0 << 1) | /* FAST_ODR */ \
- (0 << 0)) /* ST */
+#define LIS3MDL_CTRL1_BASE ((0 << 7) | /* TEMP_EN */ \
+ (1 << 6) | /* OM1 */ \
+ (1 << 5) | /* OM0 */ \
+ (0 << 4) | /* DO2 */ \
+ (0 << 3) | /* DO1 */ \
+ (0 << 2) | /* DO0 */ \
+ (0 << 1) | /* FAST_ODR */ \
+ (0 << 0)) /* ST */
/* LIS3MDL_CTRL2_BASE: control register 2 default settings */
-#define LIS3MDL_CTRL2_BASE ((0 << 7) | /* (0) */ \
- (1 << 6) | /* FS1 */ \
- (0 << 5) | /* FS0 */ \
- (0 << 4) | /* (0) */ \
- (0 << 3) | /* REBOOT */ \
- (0 << 2) | /* SOFT_RST */ \
- (0 << 1) | /* (0) */ \
- (0 << 0)) /* (0) */
+#define LIS3MDL_CTRL2_BASE ((0 << 7) | /* (0) */ \
+ (1 << 6) | /* FS1 */ \
+ (0 << 5) | /* FS0 */ \
+ (0 << 4) | /* (0) */ \
+ (0 << 3) | /* REBOOT */ \
+ (0 << 2) | /* SOFT_RST */ \
+ (0 << 1) | /* (0) */ \
+ (0 << 0)) /* (0) */
/* LIS3MDL_CTRL3_BASE: control register 3 default settings */
-#define LIS3MDL_CTRL3_BASE (0x00)
+#define LIS3MDL_CTRL3_BASE (0x00)
/* LIS3MDL_CTRL4_BASE: control register 4 default settings */
-#define LIS3MDL_CTRL4_BASE ((0 << 7) | /* (0) */ \
- (0 << 6) | /* (0) */ \
- (0 << 5) | /* (0) */ \
- (0 << 4) | /* (0) */ \
- (1 << 3) | /* OMZ1 */ \
- (1 << 2) | /* OMZ0 */ \
- (0 << 1) | /* BLE */ \
- (0 << 0)) /* (0) */
+#define LIS3MDL_CTRL4_BASE ((0 << 7) | /* (0) */ \
+ (0 << 6) | /* (0) */ \
+ (0 << 5) | /* (0) */ \
+ (0 << 4) | /* (0) */ \
+ (1 << 3) | /* OMZ1 */ \
+ (1 << 2) | /* OMZ0 */ \
+ (0 << 1) | /* BLE */ \
+ (0 << 0)) /* (0) */
/* LIS3MDL_CTRL5_BASE: control register 5 default settings */
-#define LIS3MDL_CTRL5_BASE ((0 << 7) | /* FAST_READ */ \
- (1 << 6) | /* BDU */ \
- (0 << 5) | /* (0) */ \
- (0 << 4) | /* (0) */ \
- (0 << 3) | /* (0) */ \
- (0 << 2) | /* (0) */ \
- (0 << 1) | /* (0) */ \
- (0 << 0)) /* (0) */
+#define LIS3MDL_CTRL5_BASE ((0 << 7) | /* FAST_READ */ \
+ (1 << 6) | /* BDU */ \
+ (0 << 5) | /* (0) */ \
+ (0 << 4) | /* (0) */ \
+ (0 << 3) | /* (0) */ \
+ (0 << 2) | /* (0) */ \
+ (0 << 1) | /* (0) */ \
+ (0 << 0)) /* (0) */
#ifdef LSM6DSM_I2C_MASTER_LIS3MDL
/* MUST BE SAME LENGTH OF LSM6DSMMagnRates */
static uint8_t LIS3MDLMagnRatesRegValue[] = {
+ 0x0c, /* Expected 0.8125Hz, ODR = 5Hz */
+ 0x0c, /* Expected 1.625Hz, ODR = 5Hz */
0x0c, /* Expected 3.25Hz, ODR = 5Hz */
0x10, /* Expected 6.5Hz, ODR = 10Hz */
0x14, /* Expected 12.5Hz, ODR = 20Hz */
diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lps22hb_slave.h b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lps22hb_slave.h
index 6ecedcc..cd4cd50 100644
--- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lps22hb_slave.h
+++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lps22hb_slave.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 STMicroelectronics
+ * Copyright (C) 2016-2017 STMicroelectronics
*
* Author: Denis Ciocca <denis.ciocca@st.com>
*
@@ -21,54 +21,56 @@
#ifdef LSM6DSM_I2C_MASTER_LPS22HB
#ifndef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
-#define LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 1
+#define LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 1
#else /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
#error "Another barometer is already selected! One baro per time can be used."
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
#endif /* LSM6DSM_I2C_MASTER_LPS22HB */
-#define LPS22HB_PRESS_KSCALE 0.000244140625f /* PRESS scale @1260hPa in hPa/LSB */
-#define LPS22HB_TEMP_KSCALE 0.01f /* TEMP scale in *C/LSB */
-#define LPS22HB_I2C_ADDRESS (0x5d) /* SDO = 1 : 0x5d --- SDO = 0 : 0x5c */
+#define LPS22HB_PRESS_KSCALE 0.000244140625f /* PRESS scale @1260hPa in hPa/LSB */
+#define LPS22HB_TEMP_KSCALE 0.01f /* TEMP scale in *C/LSB */
+#define LPS22HB_I2C_ADDRESS (LSM6DSM_LPS22HB_I2C_ADDRESS)
/* LPS22HB registers */
-#define LPS22HB_WAI_ADDR (0x0f)
-#define LPS22HB_CTRL1_ADDR (0x10)
-#define LPS22HB_CTRL2_ADDR (0x11)
-#define LPS22HB_OUTDATA_ADDR (0x28)
+#define LPS22HB_WAI_ADDR (0x0f)
+#define LPS22HB_CTRL1_ADDR (0x10)
+#define LPS22HB_CTRL2_ADDR (0x11)
+#define LPS22HB_OUTDATA_ADDR (0x28)
-#define LPS22HB_SW_RESET (0x04)
-#define LPS22HB_POWER_ON_VALUE (0x00)
-#define LPS22HB_POWER_OFF_VALUE (0x00)
-#define LPS22HB_OUTDATA_LEN (0x05)
-#define LPS22HB_OUTDATA_PRESS_BYTE 3
-#define LPS22HB_OUTDATA_TEMP_BYTE 2
+#define LPS22HB_SW_RESET (0x04)
+#define LPS22HB_POWER_ON_VALUE (0x00)
+#define LPS22HB_POWER_OFF_VALUE (0x00)
+#define LPS22HB_OUTDATA_LEN (0x05)
+#define LPS22HB_OUTDATA_PRESS_BYTE 3
+#define LPS22HB_OUTDATA_TEMP_BYTE 2
/* LPS22HB default base registers status */
/* LPS22HB_CTRL1_BASE: control register 1 default settings */
-#define LPS22HB_CTRL1_BASE ((0 << 7) | /* (0) */ \
- (0 << 6) | /* ODR2 */ \
- (0 << 5) | /* ODR1 */ \
- (0 << 4) | /* ODR0 */ \
- (0 << 3) | /* EN_LPFP */ \
- (0 << 2) | /* LPF_CFG */ \
- (1 << 1) | /* BDU */ \
- (0 << 0)) /* SIM */
+#define LPS22HB_CTRL1_BASE ((0 << 7) | /* (0) */ \
+ (0 << 6) | /* ODR2 */ \
+ (0 << 5) | /* ODR1 */ \
+ (0 << 4) | /* ODR0 */ \
+ (0 << 3) | /* EN_LPFP */ \
+ (0 << 2) | /* LPF_CFG */ \
+ (1 << 1) | /* BDU */ \
+ (0 << 0)) /* SIM */
/* LPS22HB_CTRL2_BASE: control register 2 default settings */
-#define LPS22HB_CTRL2_BASE ((0 << 7) | /* BOOT */ \
- (0 << 6) | /* FIFO_EN */ \
- (0 << 5) | /* STOP_ON_FTH */ \
- (1 << 4) | /* IF_ADD_INC */ \
- (0 << 3) | /* I2C_DIS */ \
- (0 << 2) | /* SWRESET */ \
- (0 << 1) | /* (0) */ \
- (0 << 0)) /* ONE_SHOT */
+#define LPS22HB_CTRL2_BASE ((0 << 7) | /* BOOT */ \
+ (0 << 6) | /* FIFO_EN */ \
+ (0 << 5) | /* STOP_ON_FTH */ \
+ (1 << 4) | /* IF_ADD_INC */ \
+ (0 << 3) | /* I2C_DIS */ \
+ (0 << 2) | /* SWRESET */ \
+ (0 << 1) | /* (0) */ \
+ (0 << 0)) /* ONE_SHOT */
#ifdef LSM6DSM_I2C_MASTER_LPS22HB
/* MUST BE SAME LENGTH OF LSM6DSMMagnRates */
static uint8_t LPS22HBBaroRatesRegValue[] = {
+ 0x20, /* Expected 0.8125Hz, ODR = 10Hz */
+ 0x20, /* Expected 1.625Hz, ODR = 10Hz */
0x20, /* Expected 3.25Hz, ODR = 10Hz */
0x20, /* Expected 6.5Hz, ODR = 10Hz */
0x30, /* Expected 12.5Hz, ODR = 25Hz */
diff --git a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lsm303agr_slave.h b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lsm303agr_slave.h
index 9881fb1..5cf144e 100644
--- a/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lsm303agr_slave.h
+++ b/firmware/os/drivers/st_lsm6dsm/st_lsm6dsm_lsm303agr_slave.h
@@ -1,5 +1,5 @@
/*
- * Copyright (C) 2016 STMicroelectronics
+ * Copyright (C) 2016-2017 STMicroelectronics
*
* Author: Denis Ciocca <denis.ciocca@st.com>
*
@@ -21,51 +21,53 @@
#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
#ifndef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
-#define LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1
+#define LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#error "Another magnetometer is already selected! One magn per time can be used."
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */
-#define LSM303AGR_KSCALE 0.15f /* MAGN scale in uT/LSB */
-#define LSM303AGR_I2C_ADDRESS (0x1e)
+#define LSM303AGR_KSCALE 0.15f /* MAGN scale in uT/LSB */
+#define LSM303AGR_I2C_ADDRESS (0x1e)
/* LSM303AGR registers */
-#define LSM303AGR_WAI_ADDR (0x4f)
-#define LSM303AGR_CFG_REG_A_M_ADDR (0x60)
-#define LSM303AGR_CFG_REG_C_M_ADDR (0x62)
-#define LSM303AGR_OUTDATA_ADDR (0x68)
+#define LSM303AGR_WAI_ADDR (0x4f)
+#define LSM303AGR_CFG_REG_A_M_ADDR (0x60)
+#define LSM303AGR_CFG_REG_C_M_ADDR (0x62)
+#define LSM303AGR_OUTDATA_ADDR (0x68)
-#define LSM303AGR_SW_RESET (0x20)
-#define LSM303AGR_POWER_ON_VALUE (0x00)
-#define LSM303AGR_POWER_OFF_VALUE (0x03)
-#define LSM303AGR_OUTDATA_LEN (0x06)
+#define LSM303AGR_SW_RESET (0x20)
+#define LSM303AGR_POWER_ON_VALUE (0x00)
+#define LSM303AGR_POWER_OFF_VALUE (0x03)
+#define LSM303AGR_OUTDATA_LEN (0x06)
/* LSM303AGR default base registers status */
/* LSM303AGR_CFG_REG_A_M_BASE: configuration register 1 default settings */
-#define LSM303AGR_CFG_REG_A_M_BASE ((0 << 7) | /* (0) */ \
- (0 << 6) | /* (0) */ \
- (0 << 5) | /* SOFT_RST */ \
- (0 << 4) | /* LP */ \
- (0 << 3) | /* ODR1 */ \
- (0 << 2) | /* ODR0 */ \
- (0 << 1) | /* MD1 */ \
- (0 << 0)) /* MD0 */
+#define LSM303AGR_CFG_REG_A_M_BASE ((0 << 7) | /* (0) */ \
+ (0 << 6) | /* (0) */ \
+ (0 << 5) | /* SOFT_RST */ \
+ (0 << 4) | /* LP */ \
+ (0 << 3) | /* ODR1 */ \
+ (0 << 2) | /* ODR0 */ \
+ (0 << 1) | /* MD1 */ \
+ (0 << 0)) /* MD0 */
/* LSM303AGR_CFC_REG_C_M_BASE: configuration register 3 default settings */
-#define LSM303AGR_CFG_REG_C_M_BASE ((0 << 7) | /* (0) */ \
- (0 << 6) | /* INT_MAG_PIN */ \
- (0 << 5) | /* I2C_DIS */ \
- (1 << 4) | /* BDU */ \
- (0 << 3) | /* BLE */ \
- (0 << 2) | /* (0) */ \
- (0 << 1) | /* SELFT_TEST */ \
- (0 << 0)) /* INT_MAG */
+#define LSM303AGR_CFG_REG_C_M_BASE ((0 << 7) | /* (0) */ \
+ (0 << 6) | /* INT_MAG_PIN */ \
+ (0 << 5) | /* I2C_DIS */ \
+ (1 << 4) | /* BDU */ \
+ (0 << 3) | /* BLE */ \
+ (0 << 2) | /* (0) */ \
+ (0 << 1) | /* SELFT_TEST */ \
+ (0 << 0)) /* INT_MAG */
#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
/* MUST BE SAME LENGTH OF LSM6DSMMagnRates */
static uint8_t LSM303AGRMagnRatesRegValue[] = {
+ 0x00, /* Expected 0.8125Hz, ODR = 10Hz */
+ 0x00, /* Expected 1.625Hz, ODR = 10Hz */
0x00, /* Expected 3.25Hz, ODR = 10Hz */
0x00, /* Expected 6.5Hz, ODR = 10Hz */
0x04, /* Expected 12.5Hz, ODR = 20Hz */
