1 /*
2 * Copyright (C) 2016 STMicroelectronics
3 *
4 * Author: Denis Ciocca <denis.ciocca@st.com>
5 *
6 * Licensed under the Apache License, Version 2.0 (the "License");
7 * you may not use this file except in compliance with the License.
8 * You may obtain a copy of the License at
9 *
10 * http://www.apache.org/licenses/LICENSE-2.0
11 *
12 * Unless required by applicable law or agreed to in writing, software
13 * distributed under the License is distributed on an "AS IS" BASIS,
14 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15 * See the License for the specific language governing permissions and
16 * limitations under the License.
17 */
18
19 #include <stdlib.h>
20 #include <string.h>
21 #include <sensors.h>
22 #include <slab.h>
23 #include <spi.h>
24 #include <gpio.h>
25 #include <atomic.h>
26 #include <timer.h>
27 #include <printf.h>
28 #include <isr.h>
29 #include <hostIntf.h>
30 #include <nanohubPacket.h>
31 #include <cpu/cpuMath.h>
32 #include <variant/sensType.h>
33 #include <plat/gpio.h>
34 #include <plat/syscfg.h>
35 #include <plat/exti.h>
36 #include <plat/rtc.h>
37 #include <algos/accel_cal.h>
38 #include <algos/gyro_cal.h>
39 #include <algos/mag_cal.h>
40
41 #include "st_lsm6dsm_lis3mdl_slave.h"
42 #include "st_lsm6dsm_lsm303agr_slave.h"
43 #include "st_lsm6dsm_ak09916_slave.h"
44 #include "st_lsm6dsm_lps22hb_slave.h"
45
46 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) || defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
47 #define LSM6DSM_I2C_MASTER_ENABLED 1
48 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
49
50 #if defined(LSM6DSM_MAGN_CALIB_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
51 #pragma message("LSM6DSM_MAGN_CALIB_ENABLED can not be used if no magnetometer sensors are enabled on I2C master. Disabling it!")
52 #undef LSM6DSM_MAGN_CALIB_ENABLED
53 #endif /* LSM6DSM_MAGN_CALIB_ENABLED, LSM6DSM_I2C_MASTER_ENABLED */
54
55 #if defined(LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP) && !defined(LSM6DSM_I2C_MASTER_ENABLED)
56 #pragma message("LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP has no meaning if no sensors are enabled on I2C master. Discarding!")
57 #endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP, LSM6DSM_I2C_MASTER_ENABLED */
58
59 #define LSM6DSM_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 0)
60
61 #define LSM6DSM_WAI_VALUE (0x6a)
62 #define LSM6DSM_RETRY_CNT_WAI 5
63 #define LSM6DSM_ACCEL_KSCALE 0.00239364f /* Accel scale @8g in (m/s^2)/LSB */
64 #define LSM6DSM_GYRO_KSCALE 0.00122173f /* Gyro scale @2000dps in (rad/sec)/LSB */
65 #define LSM6DSM_ONE_SAMPLE_BYTE 6 /* One sample on triaxial sensor generate 6 byte */
66 #define LSM6DSM_TEMP_SAMPLE_BYTE 2 /* One sample on temperature sensor generate 2 byte */
67 #define LSM6DSM_TEMP_OFFSET (25.0f)
68
69 /* Sensors orientation */
70 #define LSM6DSM_ROT_MATRIX 1, 0, 0, 0, 1, 0, 0, 0, 1
71 #define LSM6DSM_MAGN_ROT_MATRIX 1, 0, 0, 0, 1, 0, 0, 0, 1
72
73 /* SPI slave connection */
74 #define LSM6DSM_SPI_SLAVE_BUS_ID 1
75 #define LSM6DSM_SPI_SLAVE_FREQUENCY_HZ 10000000
76 #define LSM6DSM_SPI_SLAVE_CS_GPIO GPIO_PB(12)
77
78 /* LSM6DSM status check registers */
79 #define LSM6DSM_STATUS_REG_XLDA (0x01)
80 #define LSM6DSM_STATUS_REG_GDA (0x02)
81 #define LSM6DSM_STATUS_REG_TDA (0x04)
82 #define LSM6DSM_FUNC_SRC_STEP_DETECTED (0x10)
83 #define LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA (0x80)
84 #define LSM6DSM_FUNC_SRC_SIGN_MOTION (0x40)
85 #define LSM6DSM_FUNC_SRC_SENSOR_HUB_END_OP (0x01)
86
87 /* LSM6DSM ODR related */
88 #define LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON 80000
89 #define LSM6DSM_ODR_12HZ_ACCEL_STD 1
90 #define LSM6DSM_ODR_26HZ_ACCEL_STD 1
91 #define LSM6DSM_ODR_52HZ_ACCEL_STD 1
92 #define LSM6DSM_ODR_104HZ_ACCEL_STD 1
93 #define LSM6DSM_ODR_208HZ_ACCEL_STD 1
94 #define LSM6DSM_ODR_416HZ_ACCEL_STD 1
95 #define LSM6DSM_ODR_12HZ_GYRO_STD 2
96 #define LSM6DSM_ODR_26HZ_GYRO_STD 3
97 #define LSM6DSM_ODR_52HZ_GYRO_STD 3
98 #define LSM6DSM_ODR_104HZ_GYRO_STD 3
99 #define LSM6DSM_ODR_208HZ_GYRO_STD 3
100 #define LSM6DSM_ODR_416HZ_GYRO_STD 3
101
102 #define LSM6DSM_ODR_12HZ_REG_VALUE (0x10)
103 #define LSM6DSM_ODR_26HZ_REG_VALUE (0x20)
104 #define LSM6DSM_ODR_52HZ_REG_VALUE (0x30)
105 #define LSM6DSM_ODR_104HZ_REG_VALUE (0x40)
106 #define LSM6DSM_ODR_208HZ_REG_VALUE (0x50)
107 #define LSM6DSM_ODR_416HZ_REG_VALUE (0x60)
108
109 /* Interrupts */
110 #define LSM6DSM_INT_IRQ EXTI9_5_IRQn
111 #define LSM6DSM_INT1_GPIO GPIO_PB(6)
112 #define LSM6DSM_INT1_INDEX 0
113 #define LSM6DSM_INT2_INDEX 1
114 #define LSM6DSM_INT_NUM 2
115
116 #define LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE (0x01)
117 #define LSM6DSM_INT_GYRO_ENABLE_REG_VALUE (0x02)
118 #define LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE (0x80)
119 #define LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE (0x80)
120 #define LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE (0x40)
121
122 /* LSM6DSM registers */
123 #define LSM6DSM_FUNC_CFG_ACCESS_ADDR (0x01)
124 #define LSM6DSM_DRDY_PULSE_CFG_ADDR (0x0b)
125 #define LSM6DSM_INT1_CTRL_ADDR (0x0d)
126 #define LSM6DSM_INT2_CTRL_ADDR (0x0e)
127 #define LSM6DSM_WAI_ADDR (0x0f)
128 #define LSM6DSM_CTRL1_XL_ADDR (0x10)
129 #define LSM6DSM_CTRL2_G_ADDR (0x11)
130 #define LSM6DSM_CTRL3_C_ADDR (0x12)
131 #define LSM6DSM_CTRL4_C_ADDR (0x13)
132 #define LSM6DSM_EBD_STEP_COUNT_DELTA_ADDR (0x15)
133 #define LSM6DSM_CTRL10_C_ADDR (0x19)
134 #define LSM6DSM_MASTER_CONFIG_ADDR (0x1a)
135 #define LSM6DSM_STATUS_REG_ADDR (0x1e)
136 #define LSM6DSM_OUTX_L_G_ADDR (0x22)
137 #define LSM6DSM_OUTX_L_XL_ADDR (0x28)
138 #define LSM6DSM_OUT_TEMP_L_ADDR (0x20)
139 #define LSM6DSM_SENSORHUB1_REG_ADDR (0x2e)
140 #define LSM6DSM_STEP_COUNTER_L_ADDR (0x4b)
141 #define LSM6DSM_FUNC_SRC_ADDR (0x53)
142
143 #define LSM6DSM_SW_RESET (0x01)
144 #define LSM6DSM_RESET_PEDOMETER (0x02)
145 #define LSM6DSM_ENABLE_FUNC_CFG_ACCESS (0x80)
146 #define LSM6DSM_ENABLE_DIGITAL_FUNC (0x04)
147 #define LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC (0x10)
148 #define LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC (0x01)
149 #define LSM6DSM_ENABLE_TIMER_DIGITAL_FUNC (0x20)
150 #define LSM6DSM_MASTER_CONFIG_PULL_UP_EN (0x08)
151 #define LSM6DSM_MASTER_CONFIG_MASTER_ON (0x01)
152 #define LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1 (0x80)
153
154 /* LSM6DSM embedded registers */
155 #define LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR (0x02)
156 #define LSM6DSM_EMBEDDED_SLV0_SUBADDR_ADDR (0x03)
157 #define LSM6DSM_EMBEDDED_SLV0_CONFIG_ADDR (0x04)
158 #define LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR (0x05)
159 #define LSM6DSM_EMBEDDED_SLV1_SUBADDR_ADDR (0x06)
160 #define LSM6DSM_EMBEDDED_SLV1_CONFIG_ADDR (0x07)
161 #define LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR (0x08)
162 #define LSM6DSM_EMBEDDED_SLV2_SUBADDR_ADDR (0x09)
163 #define LSM6DSM_EMBEDDED_SLV2_CONFIG_ADDR (0x0a)
164 #define LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR (0x0b)
165 #define LSM6DSM_EMBEDDED_SLV3_SUBADDR_ADDR (0x0c)
166 #define LSM6DSM_EMBEDDED_SLV3_CONFIG_ADDR (0x0d)
167 #define LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR (0x0e)
168 #define LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR (0x15)
169
170 #define LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB (0x01)
171 #define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONLY_WRITE (0x00)
172 #define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR (0x10)
173 #define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR (0x20)
174 #define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR (0x30)
175 #define LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE (0x20)
176 #define LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP (0x07)
177
178 /* LSM6DSM I2C master - slave devices */
179 #ifdef LSM6DSM_I2C_MASTER_LIS3MDL
180 #define LSM6DSM_MAGN_KSCALE LIS3MDL_KSCALE
181 #define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LIS3MDL_I2C_ADDRESS
182 #define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LIS3MDL_WAI_ADDR
183 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LIS3MDL_CTRL2_ADDR
184 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LIS3MDL_SW_RESET
185 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LIS3MDL_CTRL3_ADDR
186 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LIS3MDL_CTRL3_BASE
187 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LIS3MDL_POWER_ON_VALUE
188 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LIS3MDL_POWER_OFF_VALUE
189 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LIS3MDL_CTRL1_ADDR
190 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LIS3MDL_CTRL1_BASE
191 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LIS3MDL_OUTDATA_ADDR
192 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LIS3MDL_OUTDATA_LEN
193 #define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LIS3MDLMagnRatesRegValue[i]
194 #endif /* LSM6DSM_I2C_MASTER_LIS3MDL */
195
196 #ifdef LSM6DSM_I2C_MASTER_AK09916
197 #define LSM6DSM_MAGN_KSCALE AK09916_KSCALE
198 #define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT AK09916_I2C_ADDRESS
199 #define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR AK09916_WAI_ADDR
200 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR AK09916_CNTL3_ADDR
201 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE AK09916_SW_RESET
202 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR AK09916_CNTL2_ADDR
203 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE AK09916_CNTL2_BASE
204 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE AK09916_POWER_ON_VALUE
205 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE AK09916_POWER_OFF_VALUE
206 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR AK09916_CNTL2_ADDR
207 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE AK09916_CNTL2_BASE
208 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR AK09916_OUTDATA_ADDR
209 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN AK09916_OUTDATA_LEN
210 #define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) AK09916MagnRatesRegValue[i]
211 #endif /* LSM6DSM_I2C_MASTER_AK09916 */
212
213 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR
214 #define LSM6DSM_MAGN_KSCALE LSM303AGR_KSCALE
215 #define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LSM303AGR_I2C_ADDRESS
216 #define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LSM303AGR_WAI_ADDR
217 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LSM303AGR_CFG_REG_A_M_ADDR
218 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LSM303AGR_SW_RESET
219 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LSM303AGR_CFG_REG_A_M_ADDR
220 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LSM303AGR_CFG_REG_A_M_BASE
221 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LSM303AGR_POWER_ON_VALUE
222 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LSM303AGR_POWER_OFF_VALUE
223 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LSM303AGR_CFG_REG_A_M_ADDR
224 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LSM303AGR_CFG_REG_A_M_BASE
225 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LSM303AGR_OUTDATA_ADDR
226 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LSM303AGR_OUTDATA_LEN
227 #define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LSM303AGRMagnRatesRegValue[i]
228 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */
229
230 #ifdef LSM6DSM_I2C_MASTER_LPS22HB
231 #define LSM6DSM_PRESS_KSCALE LPS22HB_PRESS_KSCALE
232 #define LSM6DSM_TEMP_KSCALE LPS22HB_TEMP_KSCALE
233 #define LSM6DSM_PRESS_OUTDATA_LEN LPS22HB_OUTDATA_PRESS_BYTE
234 #define LSM6DSM_TEMP_OUTDATA_LEN LPS22HB_OUTDATA_TEMP_BYTE
235 #define LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT LPS22HB_I2C_ADDRESS
236 #define LSM6DSM_SENSOR_SLAVE_BARO_DUMMY_REG_ADDR LPS22HB_WAI_ADDR
237 #define LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR LPS22HB_CTRL2_ADDR
238 #define LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE LPS22HB_SW_RESET
239 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR LPS22HB_CTRL1_ADDR
240 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE LPS22HB_CTRL1_BASE
241 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ON_VALUE LPS22HB_POWER_ON_VALUE
242 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE LPS22HB_POWER_OFF_VALUE
243 #define LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR LPS22HB_CTRL1_ADDR
244 #define LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE LPS22HB_CTRL1_BASE
245 #define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR LPS22HB_OUTDATA_ADDR
246 #define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN LPS22HB_OUTDATA_LEN
247 #define LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i) LPS22HBBaroRatesRegValue[i]
248 #endif /* LSM6DSM_I2C_MASTER_LPS22HB */
249
250 #ifndef LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN
251 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN 0
252 #endif /* LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN */
253 #ifndef LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN
254 #define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN 0
255 #endif /* LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN */
256
257 #define LSM6DSM_SH_READ_BYTE_NUM (LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN + \
258 LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN)
259
260 /* SPI buffers */
261 #define LSM6DSM_SPI_PACKET_SIZE 70
262 #define LSM6DSM_OUTPUT_DATA_READ_SIZE ((2 * LSM6DSM_ONE_SAMPLE_BYTE) + LSM6DSM_SH_READ_BYTE_NUM + 2)
263 #define LSM6DSM_BUF_MARGIN 120
264 #define SPI_BUF_SIZE (LSM6DSM_OUTPUT_DATA_READ_SIZE + LSM6DSM_BUF_MARGIN)
265
266 /* Magn & Baro both enabled */
267 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
268 #ifdef LSM6DSM_I2C_MASTER_AK09916
269 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR
270 #else /* LSM6DSM_I2C_MASTER_AK09916 */
271 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
272 #endif /* LSM6DSM_I2C_MASTER_AK09916 */
273 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
274
275 /* Magn only enabled */
276 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
277 #ifdef LSM6DSM_I2C_MASTER_AK09916
278 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
279 #else /* LSM6DSM_I2C_MASTER_AK09916 */
280 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
281 #endif /* LSM6DSM_I2C_MASTER_AK09916 */
282 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
283
284 /* Baro only enabled */
285 #if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
286 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
287 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
288
289 /* LSM6DSM default base registers status */
290 /* LSM6DSM_FUNC_CFG_ACCESS_BASE: enable embedded functions register */
291 #define LSM6DSM_FUNC_CFG_ACCESS_BASE (0x00)
292
293 /* LSM6DSM_DRDY_PULSE_CFG_BASE: enable pulsed interrupt register */
294 #define LSM6DSM_DRDY_PULSE_CFG_BASE (0x80)
295
296 /* LSM6DSM_INT1_CTRL_BASE: interrupt 1 control register default settings */
297 #define LSM6DSM_INT1_CTRL_BASE ((0 << 7) | /* INT1_STEP_DETECTOR */ \
298 (0 << 6) | /* INT1_SIGN_MOT */ \
299 (0 << 5) | /* INT1_FULL_FLAG */ \
300 (0 << 4) | /* INT1_FIFO_OVR */ \
301 (0 << 3) | /* INT1_FTH */ \
302 (0 << 2) | /* INT1_BOOT */ \
303 (0 << 1) | /* INT1_DRDY_G */ \
304 (0 << 0)) /* INT1_DRDY_XL */
305
306 /* LSM6DSM_INT2_CTRL_BASE: interrupt 2 control register default settings */
307 #define LSM6DSM_INT2_CTRL_BASE ((0 << 7) | /* INT2_STEP_DELTA */ \
308 (0 << 6) | /* INT2_STEP_OV */ \
309 (0 << 5) | /* INT2_FULL_FLAG */ \
310 (0 << 4) | /* INT2_FIFO_OVR */ \
311 (0 << 3) | /* INT2_FTH */ \
312 (0 << 2) | /* INT2_DRDY_TEMP */ \
313 (0 << 1) | /* INT2_DRDY_G */ \
314 (0 << 0)) /* INT2_DRDY_XL */
315
316 /* LSM6DSM_CTRL1_XL_BASE: accelerometer sensor register default settings */
317 #define LSM6DSM_CTRL1_XL_BASE ((0 << 7) | /* ODR_XL3 */ \
318 (0 << 6) | /* ODR_XL2 */ \
319 (0 << 5) | /* ODR_XL1 */ \
320 (0 << 4) | /* ODR_XL0 */ \
321 (1 << 3) | /* FS_XL1 */ \
322 (1 << 2) | /* FS_XL0 */ \
323 (0 << 1) | /* LPF1_BW_SEL */ \
324 (0 << 0)) /* (0) */
325
326 /* LSM6DSM_CTRL2_G_BASE: gyroscope sensor register default settings */
327 #define LSM6DSM_CTRL2_G_BASE ((0 << 7) | /* ODR_G3 */ \
328 (0 << 6) | /* ODR_G2 */ \
329 (0 << 5) | /* ODR_G1 */ \
330 (0 << 4) | /* ODR_G0 */ \
331 (1 << 3) | /* FS_G1 */ \
332 (1 << 2) | /* FS_G0 */ \
333 (0 << 1) | /* FS_125 */ \
334 (0 << 0)) /* (0) */
335
336 /* LSM6DSM_CTRL3_C_BASE: control register 3 default settings */
337 #define LSM6DSM_CTRL3_C_BASE ((0 << 7) | /* BOOT */ \
338 (1 << 6) | /* BDU */ \
339 (0 << 5) | /* H_LACTIVE */ \
340 (0 << 4) | /* PP_OD */ \
341 (0 << 3) | /* SIM */ \
342 (1 << 2) | /* IF_INC */ \
343 (0 << 1) | /* BLE */ \
344 (0 << 0)) /* SW_RESET */
345
346 /* LSM6DSM_CTRL4_C_BASE: control register 4 default settings */
347 #define LSM6DSM_CTRL4_C_BASE ((0 << 7) | /* DEN_XL_EN */ \
348 (0 << 6) | /* SLEEP */ \
349 (1 << 5) | /* INT2_on_INT1 */ \
350 (0 << 4) | /* DEN_DRDY_MASK */ \
351 (0 << 3) | /* DRDY_MASK */ \
352 (1 << 2) | /* I2C_disable */ \
353 (0 << 1) | /* LPF1_SEL_G */ \
354 (0 << 0)) /* (0) */
355
356 /* LSM6DSM_CTRL10_C_BASE: control register 10 default settings */
357 #define LSM6DSM_CTRL10_C_BASE ((0 << 7) | /* (0) */ \
358 (0 << 6) | /* (0) */ \
359 (0 << 5) | /* TIMER_EN */ \
360 (0 << 4) | /* PEDO_EN */ \
361 (0 << 3) | /* TILT_EN */ \
362 (0 << 2) | /* FUNC_EN */ \
363 (0 << 1) | /* PEDO_RST_STEP */ \
364 (0 << 0)) /* SIGN_MOTION_EN */
365
366 /* LSM6DSM_MASTER_CONFIG_BASE: I2C master configuration register default value */
367 #ifdef LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP
368 #define LSM6DSM_MASTER_CONFIG_BASE (LSM6DSM_MASTER_CONFIG_PULL_UP_EN)
369 #else /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */
370 #define LSM6DSM_MASTER_CONFIG_BASE (0x00)
371 #endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */
372
373 #define LSM6DSM_X_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
374 ((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \
375 (r21 == 1 ? y : (r21 == -1 ? -y : 0)) + \
376 (r31 == 1 ? z : (r31 == -1 ? -z : 0)))
377
378 #define LSM6DSM_Y_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
379 ((r12 == 1 ? x : (r12 == -1 ? -x : 0)) + \
380 (r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \
381 (r32 == 1 ? z : (r32 == -1 ? -z : 0)))
382
383 #define LSM6DSM_Z_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
384 ((r13 == 1 ? x : (r13 == -1 ? -x : 0)) + \
385 (r23 == 1 ? y : (r23 == -1 ? -y : 0)) + \
386 (r33 == 1 ? z : (r33 == -1 ? -z : 0)))
387
388 #define LSM6DSM_REMAP_X_DATA(...) LSM6DSM_X_MAP(__VA_ARGS__)
389 #define LSM6DSM_REMAP_Y_DATA(...) LSM6DSM_Y_MAP(__VA_ARGS__)
390 #define LSM6DSM_REMAP_Z_DATA(...) LSM6DSM_Z_MAP(__VA_ARGS__)
391
392 enum SensorIndex {
393 ACCEL = 0,
394 GYRO,
395 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
396 MAGN,
397 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
398 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
399 PRESS,
400 TEMP,
401 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
402 STEP_DETECTOR,
403 STEP_COUNTER,
404 SIGN_MOTION,
405 NUM_SENSORS
406 };
407
408 enum InitState {
409 RESET_LSM6DSM = 0,
410 INIT_LSM6DSM,
411 #ifdef LSM6DSM_I2C_MASTER_ENABLED
412 INIT_I2C_MASTER_REGS_CONF,
413 INIT_I2C_MASTER_SENSOR_RESET,
414 INIT_I2C_MASTER_MAGN_SENSOR,
415 INIT_I2C_MASTER_BARO_SENSOR,
416 INIT_I2C_MASTER_SENSOR_END,
417 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
418 INIT_DONE,
419 };
420
421 enum SensorEvents {
422 NO_EVT = -1,
423 EVT_SPI_DONE = EVT_APP_START + 1,
424 EVT_SENSOR_INTERRUPT_1
425 };
426
427 enum SensorState {
428 SENSOR_BOOT = 0,
429 SENSOR_VERIFY_WAI,
430 SENSOR_INITIALIZATION,
431 SENSOR_IDLE,
432 SENSOR_POWERING_UP,
433 SENSOR_POWERING_DOWN,
434 SENSOR_CONFIG_CHANGING,
435 SENSOR_INT1_STATUS_REG_HANDLING,
436 SENSOR_INT1_OUTPUT_DATA_HANDLING
437 };
438
439 static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay);
440 static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay);
441 static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay);
442
443 #define SPI_MULTIWRITE_0(addr, data, size) lsm6dsm_spiQueueMultiwrite(addr, data, size, 2)
444 #define SPI_MULTIWRITE_1(addr, data, size, delay) lsm6dsm_spiQueueMultiwrite(addr, data, size, delay)
445 #define GET_SPI_MULTIWRITE_MACRO(_1, _2, _3, _4, NAME, ...) NAME
446 #define SPI_MULTIWRITE(...) GET_SPI_MULTIWRITE_MACRO(__VA_ARGS__, SPI_MULTIWRITE_1, SPI_MULTIWRITE_0)(__VA_ARGS__)
447
448 #define SPI_WRITE_0(addr, data) lsm6dsm_spiQueueWrite(addr, data, 2)
449 #define SPI_WRITE_1(addr, data, delay) lsm6dsm_spiQueueWrite(addr, data, delay)
450 #define GET_SPI_WRITE_MACRO(_1, _2, _3, NAME, ...) NAME
451 #define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__)
452
453 #define SPI_READ_0(addr, size, buf) lsm6dsm_spiQueueRead(addr, size, buf, 0)
454 #define SPI_READ_1(addr, size, buf, delay) lsm6dsm_spiQueueRead(addr, size, buf, delay)
455 #define GET_SPI_READ_MACRO(_1, _2, _3, _4, NAME, ...) NAME
456 #define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__)
457
458 #ifdef LSM6DSM_I2C_MASTER_ENABLED
459 static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si);
460
461 #define SPI_WRITE_SS_REGISTER_0(addr, value, accelRate, si) lsm6dsm_writeSlaveRegister(addr, value, accelRate, 0, si)
462 #define SPI_WRITE_SS_REGISTER_1(addr, value, accelRate, si, delay) lsm6dsm_writeSlaveRegister(addr, value, accelRate, delay, si)
463 #define GET_SPI_WRITE_SS_MACRO(_1, _2, _3, _4, _5, NAME, ...) NAME
464 #define SPI_WRITE_SLAVE_SENSOR_REGISTER(...) GET_SPI_WRITE_SS_MACRO(__VA_ARGS__, SPI_WRITE_SS_REGISTER_1, \
465 SPI_WRITE_SS_REGISTER_0)(__VA_ARGS__)
466 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
467
468 #define INFO_PRINT(fmt, ...) \
469 do { \
470 osLog(LOG_INFO, "%s " fmt, "[LSM6DSM]", ##__VA_ARGS__); \
471 } while (0);
472
473 #define DEBUG_PRINT(fmt, ...) \
474 do { \
475 if (LSM6DSM_DBG_ENABLED) { \
476 osLog(LOG_DEBUG, "%s " fmt, "[LSM6DSM]", ##__VA_ARGS__); \
477 } \
478 } while (0);
479
480 #define ERROR_PRINT(fmt, ...) \
481 do { \
482 osLog(LOG_ERROR, "%s " fmt, "[LSM6DSM]", ##__VA_ARGS__); \
483 } while (0);
484
485 /* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */
486 #ifndef LSM6DSM_DBG_ENABLED
487 #define LSM6DSM_DBG_ENABLED 0
488 #endif /* LSM6DSM_DBG_ENABLED */
489
490
491 /* struct LSM6DSMSPISlaveInterface: SPI slave data interface
492 * @packets: spi packets to perform read/write operations.
493 * @txrxBuffer: spi data buffer.
494 * @spiDev: spi device pointer.
495 * @mode: spi mode (frequency, polarity, etc).
496 * @cs: chip select used by SPI slave.
497 * @mWbufCnt: counter of total data in spi buffer.
498 * @statusRegBuffer: pointer to txrxBuffer to access status register data.
499 * @funcSrcBuffer: pointer to txrxBuffer to access func source register data.
500 * @tmpDataBuffer: pointer to txrxBuffer to access sporadic read.
501 * @accelDataBuffer: pointer to txrxBuffer to access accelerometer data.
502 * @gyroDataBuffer: pointer to txrxBuffer to access gyroscope data.
503 * @SHDataBuffer: pointer to txrxBuffer to access magnetometer data.
504 * @stepCounterDataBuffer: pointer to txrxBuffer to access step counter data.
505 * @tempCounterDataBuffer: pointer to txrxBuffer to access temperature data.
506 * @mRegCnt: spi packet num counter.
507 * @spiInUse: flag used to check if SPI is currently busy.
508 */
509 struct LSM6DSMSPISlaveInterface {
510 struct SpiPacket packets[LSM6DSM_SPI_PACKET_SIZE];
511 uint8_t txrxBuffer[SPI_BUF_SIZE];
512 struct SpiDevice *spiDev;
513 struct SpiMode mode;
514 spi_cs_t cs;
515 uint16_t mWbufCnt;
516 uint8_t *statusRegBuffer;
517 uint8_t *funcSrcBuffer;
518 uint8_t *tmpDataBuffer;
519 uint8_t *accelDataBuffer;
520 uint8_t *gyroDataBuffer;
521 #ifdef LSM6DSM_I2C_MASTER_ENABLED
522 uint8_t *SHDataBuffer;
523 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
524 uint8_t *stepCounterDataBuffer;
525 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
526 uint8_t *tempDataBuffer;
527 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
528 uint8_t mRegCnt;
529 bool spiInUse;
530 };
531
532 /*
533 * struct LSM6DSMConfigStatus: temporary data of pending events
534 * @latency: value to be used in next setRate operation [ns].
535 * @rate: value to be used in next setRate operation [Hz * 1024].
536 * @enable: value to be used in next setEnable.
537 */
538 struct LSM6DSMConfigStatus {
539 uint64_t latency;
540 uint32_t rate;
541 bool enable;
542 };
543
544 /* struct LSM6DSMSensor: sensor status data
545 * @pConfig: temporary data of pending events.
546 * @tADataEvt: store three axis sensor data to send to nanohub.
547 * @sADataEvt: store one axis sensor data to send to nanohub.
548 * @latency: current value of latency [n].
549 * @handle: sensor handle obtained by sensorRegister.
550 * @rate: current value of rate [Hz * 1024].
551 * @hwRate: current value of physical rate [Hz * 1024].
552 * @idx: enum SensorIndex.
553 * @samplesToDiscard: samples to discard after enable or odr switch.
554 * @samplesDecimator: decimator factor to achieve lower odr not available in hw.
555 * @samplesCounter: samples counter by decimation operation.
556 * enabled: current status of sensor.
557 */
558 struct LSM6DSMSensor {
559 struct LSM6DSMConfigStatus pConfig;
560 struct TripleAxisDataEvent *tADataEvt;
561 struct SingleAxisDataEvent *sADataEvt;
562 uint64_t latency;
563 uint32_t handle;
564 uint32_t rate;
565 uint32_t hwRate;
566 enum SensorIndex idx;
567 uint8_t samplesToDiscard;
568 uint8_t samplesDecimator;
569 uint8_t samplesCounter;
570 bool enabled;
571 };
572
573 /* struct LSM6DSMTask: task data
574 * @sensors: sensor status data list.
575 * @slaveConn: slave interface / communication data.
576 * @accelCal: accelerometer calibration algo data.
577 * @gyroCal: gyroscope calibration algo data.
578 * @gmagnCal: magnetometer calibration algo data.
579 * @int1: int1 gpio data.
580 * @isr1: isr1 data.
581 * @mDataSlabThreeAxis: sensors data memory three axis sensors.
582 * @mDataSlabOneAxis: sensors data memory one axis sensors.
583 * @currentTemperature: sensors temperature data value used by gyroscope/accelerometer bias calibration libs.
584 * @timestampInt: timestamp value from isr.
585 * @tid: task id.
586 * @totalNumSteps: total number of steps of step counter sensor.
587 * @triggerRate: max ODR value between accel or gyro.
588 * @initState: initialization id done in several steps (enum InitState).
589 * @state: task state, driver manage operations using a state machine (enum SensorState).
590 * @mRetryLeft: counter used to retry operations #n times before return a failure.
591 * @statusRegisterDA: acceleromter/gyroscope status register (data available).
592 * @statusRegisterTDA: temperature status register (data available).
593 * @statusRegisterSH: sensor-hub status register (slave data available).
594 * @accelSensorDependencies: dependencies mask of sensors that are using accelerometer.
595 * @embeddedFunctionsDependencies: dependencies mask of sensors that are using embedded functions.
596 * @int1Register: interrupt 1 register status content (addr: 0x0d).
597 * @int2Register: interrupt 2 register status content (addr: 0x0e).
598 * @embeddedFunctionsRegister: embedded register status content (addr: 0x19).
599 * @masterConfigRegister: i2c master register status content (addr: 0x1a).
600 * @newMagnCalibData: this flag indicate if new magnetometer calibration data are available.
601 * @readSteps: flag used to indicate if interrupt task need to read number of steps.
602 * @pendingEnableConfig: pending setEnable operations to be executed.
603 * @pendingRateConfig: pending setRate operations to be executed.
604 * @pendingInt: pending interrupt task to be executed.
605 */
606 typedef struct LSM6DSMTask {
607 struct LSM6DSMSensor sensors[NUM_SENSORS];
608 struct LSM6DSMSPISlaveInterface slaveConn;
609
610 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
611 struct accelCal_t accelCal;
612 struct TripleAxisDataEvent *accelBiasDataEvt;
613 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
614
615 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
616 struct gyroCal_t gyroCal;
617 struct TripleAxisDataEvent *gyroBiasDataEvt;
618 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
619
620 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
621 struct MagCal magnCal;
622 struct TripleAxisDataEvent *magnCalDataEvt;
623 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
624
625 struct Gpio *int1;
626 struct ChainedIsr isr1;
627 struct SlabAllocator *mDataSlabThreeAxis;
628 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
629 struct SlabAllocator *mDataSlabOneAxis;
630 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
631
632 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
633 float currentTemperature;
634 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
635
636 uint64_t timestampInt[LSM6DSM_INT_NUM];
637
638 uint32_t tid;
639 uint32_t totalNumSteps;
640 uint32_t triggerRate;
641
642 enum InitState initState;
643 volatile uint8_t state;
644
645 uint8_t mRetryLeft;
646 uint8_t statusRegisterDA;
647 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
648 uint8_t statusRegisterTDA;
649 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
650 #ifdef LSM6DSM_I2C_MASTER_ENABLED
651 uint8_t statusRegisterSH;
652 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
653 uint8_t accelSensorDependencies;
654 uint8_t embeddedFunctionsDependencies;
655 uint8_t int1Register;
656 uint8_t int2Register;
657 uint8_t embeddedFunctionsRegister;
658
659 #ifdef LSM6DSM_I2C_MASTER_ENABLED
660 uint8_t masterConfigRegister;
661 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
662
663 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
664 bool newMagnCalibData;
665 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
666
667 bool readSteps;
668 bool pendingEnableConfig[NUM_SENSORS];
669 bool pendingRateConfig[NUM_SENSORS];
670 bool pendingInt[LSM6DSM_INT_NUM];
671 } LSM6DSMTask;
672
673 static LSM6DSMTask mTask;
674
675 #define TASK LSM6DSMTask* const _task
676 #define TDECL() TASK = &mTask; (void)_task
677 #define T(v) (_task->v)
678 #define T_SLAVE_INTERFACE(v) (_task->slaveConn.v)
679
680 #define BIT(x) (0x01 << x)
681 #define SENSOR_HZ_RATE_TO_US(x) (1024000000UL / x)
682 #define NS_TO_US(ns) cpuMathU64DivByU16(ns, 1000)
683
684 /* Atomic get state */
685 #define GET_STATE() (atomicReadByte(&(_task->state)))
686
687 /* Atomic set state, this set the state to arbitrary value, use with caution */
688 #define SET_STATE(s) \
689 do { \
690 atomicWriteByte(&(_task->state), (s)); \
691 } while (0)
692
trySwitchState_(TASK,enum SensorState newState)693 static bool trySwitchState_(TASK, enum SensorState newState)
694 {
695 return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
696 }
697 #define trySwitchState(s) trySwitchState_(_task, (s))
698
699 static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext);
700 #define lsm6dsm_readStatusReg(a) lsm6dsm_readStatusReg_(_task, (a))
701
702 #define DEC_INFO(name, type, axis, inter, samples) \
703 .sensorName = name, \
704 .sensorType = type, \
705 .numAxis = axis, \
706 .interrupt = inter, \
707 .minSamples = samples
708
709 #define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
710 DEC_INFO(name, type, axis, inter, samples), \
711 .supportedRates = rates
712
713 #define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \
714 DEC_INFO(name, type, axis, inter, samples), \
715 .supportedRates = rates, \
716 .flags1 = SENSOR_INFO_FLAGS1_BIAS, \
717 .biasType = bias
718
719 #define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \
720 DEC_INFO(name, type, axis, inter, samples), \
721 .supportedRates = rates, \
722 .flags1 = SENSOR_INFO_FLAGS1_RAW, \
723 .rawType = raw, \
724 .rawScale = scale
725
726 #define DEC_INFO_RATE_RAW_BIAS(name, rates, type, axis, inter, samples, raw, scale, bias) \
727 DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale), \
728 .flags1 = SENSOR_INFO_FLAGS1_RAW | SENSOR_INFO_FLAGS1_BIAS, \
729 .biasType = bias
730
731 /* LSM6DSMImuRates: supported frequencies by accelerometer and gyroscope sensors
732 * LSM6DSMImuRatesRegValue, LSM6DSMRatesSamplesToDiscardGyroPowerOn, LSM6DSMAccelRatesSamplesToDiscard,
733 * LSM6DSMGyroRatesSamplesToDiscard must have same length.
734 */
735 static uint32_t LSM6DSMImuRates[] = {
736 SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */
737 SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */
738 SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */
739 SENSOR_HZ(26.0f), /* 26Hz */
740 SENSOR_HZ(52.0f), /* 52Hz */
741 SENSOR_HZ(104.0f), /* 104Hz */
742 SENSOR_HZ(208.0f), /* 208Hz */
743 SENSOR_HZ(416.0f), /* 416Hz */
744 0,
745 };
746
747 static uint8_t LSM6DSMImuRatesRegValue[] = {
748 LSM6DSM_ODR_12HZ_REG_VALUE, /* 3.25Hz - do not exist, use 12.5Hz */
749 LSM6DSM_ODR_12HZ_REG_VALUE, /* 6.5Hz - do not exist, use 12.5Hz */
750 LSM6DSM_ODR_12HZ_REG_VALUE, /* 12.5Hz */
751 LSM6DSM_ODR_26HZ_REG_VALUE, /* 26Hz */
752 LSM6DSM_ODR_52HZ_REG_VALUE, /* 52Hz */
753 LSM6DSM_ODR_104HZ_REG_VALUE, /* 104Hz */
754 LSM6DSM_ODR_208HZ_REG_VALUE, /* 208Hz */
755 LSM6DSM_ODR_416HZ_REG_VALUE, /* 416Hz */
756 };
757
758 /* When sensors switch status from power-down, constant boottime must be considered, some samples should be discarded */
759 static uint8_t LSM6DSMRatesSamplesToDiscardGyroPowerOn[] = {
760 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 3.25Hz - do not exist, use 12.5Hz = 80000us */
761 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 6.5Hz - do not exist, use 12.5Hz = 80000us */
762 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 12.5Hz = 80000us */
763 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 38461, /* 26Hz = 38461us */
764 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 19230, /* 52Hz = 19230s */
765 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 9615, /* 104Hz = 9615us */
766 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 4807, /* 208Hz = 4807us */
767 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 2403, /* 416Hz = 2403us */
768 };
769
770 /* When accelerometer change odr but sensor is already on, few samples should be discarded */
771 static uint8_t LSM6DSMAccelRatesSamplesToDiscard[] = {
772 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 3.25Hz - do not exist, use 12.5Hz */
773 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 6.5Hz - do not exist, use 12.5Hz */
774 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 12.5Hz */
775 LSM6DSM_ODR_26HZ_ACCEL_STD, /* 26Hz */
776 LSM6DSM_ODR_52HZ_ACCEL_STD, /* 52Hz */
777 LSM6DSM_ODR_104HZ_ACCEL_STD, /* 104Hz */
778 LSM6DSM_ODR_208HZ_ACCEL_STD, /* 208Hz */
779 LSM6DSM_ODR_416HZ_ACCEL_STD, /* 416Hz */
780 };
781
782 /* When gyroscope change odr but sensor is already on, few samples should be discarded */
783 static uint8_t LSM6DSMGyroRatesSamplesToDiscard[] = {
784 LSM6DSM_ODR_12HZ_GYRO_STD, /* 3.25Hz - do not exist, use 12.5Hz */
785 LSM6DSM_ODR_12HZ_GYRO_STD, /* 6.5Hz - do not exist, use 12.5Hz */
786 LSM6DSM_ODR_12HZ_GYRO_STD, /* 12.5Hz */
787 LSM6DSM_ODR_26HZ_GYRO_STD, /* 26Hz */
788 LSM6DSM_ODR_52HZ_GYRO_STD, /* 52Hz */
789 LSM6DSM_ODR_104HZ_GYRO_STD, /* 104Hz */
790 LSM6DSM_ODR_208HZ_GYRO_STD, /* 208Hz */
791 LSM6DSM_ODR_416HZ_GYRO_STD, /* 416Hz */
792 };
793
794 #ifdef LSM6DSM_I2C_MASTER_ENABLED
795 static uint32_t LSM6DSMSHRates[] = {
796 SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */
797 SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */
798 SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */
799 SENSOR_HZ(26.0f), /* 26Hz */
800 SENSOR_HZ(52.0f), /* 52Hz */
801 SENSOR_HZ(104.0f), /* 104Hz */
802 0,
803 };
804 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
805
806 #define LSM6DSM_SC_DELTA_TIME_PERIOD_SEC (1.6384f)
807
808 static uint32_t LSM6DSMStepCounterRates[] = {
809 SENSOR_HZ(1.0f / (128 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 209.715 sec */
810 SENSOR_HZ(1.0f / (64 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 104.857 sec */
811 SENSOR_HZ(1.0f / (32 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 52.4288 sec */
812 SENSOR_HZ(1.0f / (16 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 26.1574 sec */
813 SENSOR_HZ(1.0f / (8 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 13.0787 sec */
814 SENSOR_HZ(1.0f / (4 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 6.53936 sec */
815 SENSOR_HZ(1.0f / (2 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 3.26968 sec */
816 SENSOR_HZ(1.0f / (1 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 1.63840 sec */
817 SENSOR_RATE_ONCHANGE,
818 0,
819 };
820
821 static const struct SensorInfo LSM6DSMSensorInfo[NUM_SENSORS] = {
822 {
823 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
824 DEC_INFO_RATE_RAW_BIAS("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
825 NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE, SENS_TYPE_ACCEL_BIAS)
826 #else /* LSM6DSM_ACCEL_CALIB_ENABLED */
827 DEC_INFO_RATE_RAW("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
828 NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE)
829 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
830 },
831 {
832 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
833 DEC_INFO_RATE_BIAS("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_GYRO_BIAS)
834 #else /* LSM6DSM_GYRO_CALIB_ENABLED */
835 DEC_INFO_RATE("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1)
836 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
837 },
838 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
839 {
840 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
841 DEC_INFO_RATE_BIAS("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1, SENS_TYPE_MAG_BIAS)
842 #else /* LSM6DSM_MAGN_CALIB_ENABLED */
843 DEC_INFO_RATE("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 1)
844 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
845 },
846 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
847 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
848 {
849 DEC_INFO_RATE("Pressure", LSM6DSMSHRates, SENS_TYPE_BARO, NUM_AXIS_ONE, NANOHUB_INT_NONWAKEUP, 1)
850 },
851 {
852 DEC_INFO_RATE("Temperature", LSM6DSMSHRates, SENS_TYPE_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 1)
853 },
854 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
855 {
856 DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 1)
857 },
858 {
859 DEC_INFO_RATE("Step Counter", LSM6DSMStepCounterRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 1)
860 },
861 {
862 DEC_INFO("Significant Motion", SENS_TYPE_SIG_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_WAKEUP, 1)
863 },
864 };
865
866 #define DEC_OPS(power, firmware, rate, flush) \
867 .sensorPower = power, \
868 .sensorFirmwareUpload = firmware, \
869 .sensorSetRate = rate, \
870 .sensorFlush = flush
871
872 #define DEC_OPS_SEND(power, firmware, rate, flush, send) \
873 .sensorPower = power, \
874 .sensorFirmwareUpload = firmware, \
875 .sensorSetRate = rate, \
876 .sensorFlush = flush, \
877 .sensorSendOneDirectEvt = send
878
879 static bool lsm6dsm_setAccelPower(bool on, void *cookie);
880 static bool lsm6dsm_setGyroPower(bool on, void *cookie);
881 static bool lsm6dsm_setStepDetectorPower(bool on, void *cookie);
882 static bool lsm6dsm_setStepCounterPower(bool on, void *cookie);
883 static bool lsm6dsm_setSignMotionPower(bool on, void *cookie);
884 static bool lsm6dsm_accelFirmwareUpload(void *cookie);
885 static bool lsm6dsm_gyroFirmwareUpload(void *cookie);
886 static bool lsm6dsm_stepDetectorFirmwareUpload(void *cookie);
887 static bool lsm6dsm_stepCounterFirmwareUpload(void *cookie);
888 static bool lsm6dsm_signMotionFirmwareUpload(void *cookie);
889 static bool lsm6dsm_setAccelRate(uint32_t rate, uint64_t latency, void *cookie);
890 static bool lsm6dsm_setGyroRate(uint32_t rate, uint64_t latency, void *cookie);
891 static bool lsm6dsm_setStepDetectorRate(uint32_t rate, uint64_t latency, void *cookie);
892 static bool lsm6dsm_setStepCounterRate(uint32_t rate, uint64_t latency, void *cookie);
893 static bool lsm6dsm_setSignMotionRate(uint32_t rate, uint64_t latency, void *cookie);
894 static bool lsm6dsm_accelFlush(void *cookie);
895 static bool lsm6dsm_gyroFlush(void *cookie);
896 static bool lsm6dsm_stepDetectorFlush(void *cookie);
897 static bool lsm6dsm_stepCounterFlush(void *cookie);
898 static bool lsm6dsm_signMotionFlush(void *cookie);
899 static bool lsm6dsm_stepCounterSendLastData(void *cookie, uint32_t tid);
900
901 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
902 static bool lsm6dsm_setMagnPower(bool on, void *cookie);
903 static bool lsm6dsm_magnFirmwareUpload(void *cookie);
904 static bool lsm6dsm_setMagnRate(uint32_t rate, uint64_t latency, void *cookie);
905 static bool lsm6dsm_magnFlush(void *cookie);
906 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
907
908 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
909 static bool lsm6dsm_setPressPower(bool on, void *cookie);
910 static bool lsm6dsm_pressFirmwareUpload(void *cookie);
911 static bool lsm6dsm_setPressRate(uint32_t rate, uint64_t latency, void *cookie);
912 static bool lsm6dsm_pressFlush(void *cookie);
913 static bool lsm6dsm_setTempPower(bool on, void *cookie);
914 static bool lsm6dsm_tempFirmwareUpload(void *cookie);
915 static bool lsm6dsm_setTempRate(uint32_t rate, uint64_t latency, void *cookie);
916 static bool lsm6dsm_tempFlush(void *cookie);
917 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
918
919 static const struct SensorOps LSM6DSMSensorOps[NUM_SENSORS] = {
920 { DEC_OPS(lsm6dsm_setAccelPower, lsm6dsm_accelFirmwareUpload, lsm6dsm_setAccelRate, lsm6dsm_accelFlush) },
921 { DEC_OPS(lsm6dsm_setGyroPower, lsm6dsm_gyroFirmwareUpload, lsm6dsm_setGyroRate, lsm6dsm_gyroFlush) },
922 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
923 { DEC_OPS(lsm6dsm_setMagnPower, lsm6dsm_magnFirmwareUpload, lsm6dsm_setMagnRate, lsm6dsm_magnFlush) },
924 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
925 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
926 { DEC_OPS(lsm6dsm_setPressPower, lsm6dsm_pressFirmwareUpload, lsm6dsm_setPressRate, lsm6dsm_pressFlush) },
927 { DEC_OPS(lsm6dsm_setTempPower, lsm6dsm_tempFirmwareUpload, lsm6dsm_setTempRate, lsm6dsm_tempFlush) },
928 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
929 { DEC_OPS(lsm6dsm_setStepDetectorPower, lsm6dsm_stepDetectorFirmwareUpload, lsm6dsm_setStepDetectorRate, lsm6dsm_stepDetectorFlush) },
930 { DEC_OPS_SEND(lsm6dsm_setStepCounterPower, lsm6dsm_stepCounterFirmwareUpload, lsm6dsm_setStepCounterRate, lsm6dsm_stepCounterFlush, lsm6dsm_stepCounterSendLastData) },
931 { DEC_OPS(lsm6dsm_setSignMotionPower, lsm6dsm_signMotionFirmwareUpload, lsm6dsm_setSignMotionRate, lsm6dsm_signMotionFlush) },
932 };
933
lsm6dsm_spiQueueRead(uint8_t addr,size_t size,uint8_t ** buf,uint32_t delay)934 static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
935 {
936 TDECL();
937
938 if (T_SLAVE_INTERFACE(spiInUse)) {
939 ERROR_PRINT("SPI in use, cannot queue read (addr=%d len=%d)\n", (int)addr, (int)size);
940 return;
941 }
942
943 *buf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
944
945 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = size + 1;
946 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
947 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = *buf;
948 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000;
949
950 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr | 0x80;
951 T_SLAVE_INTERFACE(mWbufCnt) += size;
952 T_SLAVE_INTERFACE(mRegCnt)++;
953 }
954
lsm6dsm_spiQueueWrite(uint8_t addr,uint8_t data,uint32_t delay)955 static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
956 {
957 TDECL();
958
959 if (T_SLAVE_INTERFACE(spiInUse)) {
960 ERROR_PRINT("SPI in use, cannot queue write (addr=%d data=%d)\n", (int)addr, (int)data);
961 return;
962 }
963
964 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = 2;
965 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
966 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
967 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000;
968
969 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr;
970 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = data;
971 T_SLAVE_INTERFACE(mRegCnt)++;
972 }
973
lsm6dsm_spiQueueMultiwrite(uint8_t addr,uint8_t * data,size_t size,uint32_t delay)974 static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay)
975 {
976 TDECL();
977 uint8_t i;
978
979 if (T_SLAVE_INTERFACE(spiInUse)) {
980 ERROR_PRINT("SPI in use, cannot queue multiwrite (addr=%d size=%d)\n", (int)addr, (int)size);
981 return;
982 }
983
984 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = 1 + size;
985 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
986 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
987 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000;
988
989 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr;
990
991 for (i = 0; i < size; i++)
992 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = data[i];
993
994 T_SLAVE_INTERFACE(mRegCnt)++;
995 }
996
lsm6dsm_spiBatchTxRx(struct SpiMode * mode,SpiCbkF callback,void * cookie,const char * src)997 static void lsm6dsm_spiBatchTxRx(struct SpiMode *mode,
998 SpiCbkF callback, void *cookie, const char *src)
999 {
1000 TDECL();
1001 uint8_t regCount;
1002
1003 if (T_SLAVE_INTERFACE(mWbufCnt) > SPI_BUF_SIZE) {
1004 ERROR_PRINT("No enough SPI buffer space, dropping transaction\n");
1005 return;
1006 }
1007
1008 if (T_SLAVE_INTERFACE(mRegCnt) > LSM6DSM_SPI_PACKET_SIZE) {
1009 ERROR_PRINT("spiBatchTxRx too many packets!\n");
1010 return;
1011 }
1012
1013 /* Reset variables before issuing SPI transaction.
1014 SPI may finish before spiMasterRxTx finish */
1015 regCount = T_SLAVE_INTERFACE(mRegCnt);
1016 T_SLAVE_INTERFACE(spiInUse) = true;
1017 T_SLAVE_INTERFACE(mRegCnt) = 0;
1018 T_SLAVE_INTERFACE(mWbufCnt) = 0;
1019
1020 if (spiMasterRxTx(T_SLAVE_INTERFACE(spiDev), T_SLAVE_INTERFACE(cs), T_SLAVE_INTERFACE(packets), regCount, mode, callback, cookie)) {
1021 ERROR_PRINT("spiBatchTxRx failed!\n");
1022 }
1023 }
1024
lsm6dsm_timerCallback(uint32_t timerId,void * data)1025 static void lsm6dsm_timerCallback(uint32_t timerId, void *data)
1026 {
1027 osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid);
1028 }
1029
lsm6dsm_spiCallback(void * cookie,int err)1030 static void lsm6dsm_spiCallback(void *cookie, int err)
1031 {
1032 TDECL();
1033
1034 T_SLAVE_INTERFACE(spiInUse) = false;
1035 osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid);
1036 }
1037
lsm6dsm_readStatusReg_(TASK,bool isInterruptContext)1038 static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext)
1039 {
1040 if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
1041 SPI_READ(LSM6DSM_STATUS_REG_ADDR, 1, &T_SLAVE_INTERFACE(statusRegBuffer));
1042 SPI_READ(LSM6DSM_FUNC_SRC_ADDR, 1, &T_SLAVE_INTERFACE(funcSrcBuffer));
1043 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
1044 } else {
1045 if (isInterruptContext)
1046 osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid));
1047 else
1048 T(pendingInt[LSM6DSM_INT1_INDEX]) = true;
1049 }
1050 }
1051
1052 /*
1053 * lsm6dsm_isr1: INT-1 line service routine
1054 */
lsm6dsm_isr1(struct ChainedIsr * isr)1055 static bool lsm6dsm_isr1(struct ChainedIsr *isr)
1056 {
1057 TDECL();
1058
1059 if (!extiIsPendingGpio(T(int1)))
1060 return false;
1061
1062 T(timestampInt[LSM6DSM_INT1_INDEX]) = rtcGetTime();
1063 lsm6dsm_readStatusReg(true);
1064
1065 extiClearPendingGpio(T(int1));
1066
1067 return true;
1068 }
1069
1070 /*
1071 * lsm6dsm_enableInterrupt: enable driver interrupt capability
1072 */
lsm6dsm_enableInterrupt(struct Gpio * pin,struct ChainedIsr * isr)1073 static void lsm6dsm_enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
1074 {
1075 gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
1076 syscfgSetExtiPort(pin);
1077 extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
1078 extiChainIsr(LSM6DSM_INT_IRQ, isr);
1079 }
1080
1081 /*
1082 * lsm6dsm_disableInterrupt: disable driver interrupt capability
1083 */
lsm6dsm_disableInterrupt(struct Gpio * pin,struct ChainedIsr * isr)1084 static void lsm6dsm_disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
1085 {
1086 extiUnchainIsr(LSM6DSM_INT_IRQ, isr);
1087 extiDisableIntGpio(pin);
1088 }
1089
1090 /*
1091 * lsm6dsm_writeEmbeddedRegister: write embedded register of sensor
1092 */
lsm6dsm_writeEmbeddedRegister(uint8_t addr,uint8_t value)1093 static void lsm6dsm_writeEmbeddedRegister(uint8_t addr, uint8_t value)
1094 {
1095 TDECL();
1096
1097 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
1098 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);
1099
1100 SPI_WRITE(addr, value);
1101
1102 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
1103 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1104 }
1105
1106 #ifdef LSM6DSM_I2C_MASTER_ENABLED
1107 /*
1108 * lsm6dsm_writeSlaveRegister: write I2C slave register using sensor-hub feature
1109 */
lsm6dsm_writeSlaveRegister(uint8_t addr,uint8_t value,uint32_t accelRate,uint32_t delay,enum SensorIndex si)1110 static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si)
1111 {
1112 TDECL();
1113 uint8_t slave_addr, buffer[3];
1114 uint32_t sh_op_complete_time;
1115
1116 switch (si) {
1117 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1118 case MAGN:
1119 slave_addr = LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT;
1120 break;
1121 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1122 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
1123 case PRESS:
1124 case TEMP:
1125 slave_addr = LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT;
1126 break;
1127 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
1128 default:
1129 return;
1130 }
1131
1132 if (accelRate > SENSOR_HZ(104.0f))
1133 sh_op_complete_time = SENSOR_HZ_RATE_TO_US(SENSOR_HZ(104.0f));
1134 else
1135 sh_op_complete_time = SENSOR_HZ_RATE_TO_US(accelRate);
1136
1137 /* Perform write to slave sensor and wait write is done (1 accel ODR) */
1138 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
1139 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);
1140
1141 buffer[0] = slave_addr << 1; /* LSM6DSM_EMBEDDED_SLV0_ADDR */
1142 buffer[1] = addr; /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */
1143 buffer[2] = LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONLY_WRITE; /* LSM6DSM_EMBEDDED_SLV0_CONFIG */
1144 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 3);
1145 SPI_WRITE(LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR, value);
1146
1147 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
1148 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister), (3 * sh_op_complete_time) / 2);
1149
1150 /* After write is completed slave 0 must be set to sleep mode */
1151 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
1152 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);
1153
1154 buffer[0] = LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP; /* LSM6DSM_EMBEDDED_SLV0_ADDR */
1155 buffer[1] = addr; /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */
1156 buffer[2] = LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE; /* LSM6DSM_EMBEDDED_SLV0_CONFIG */
1157 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 3);
1158
1159 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
1160 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1161 }
1162 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
1163
1164 /*
1165 * lsm6dsm_computeOdr: get index of LSM6DSMImuRates array based on selected rate
1166 */
lsm6dsm_computeOdr(uint32_t rate)1167 static uint8_t lsm6dsm_computeOdr(uint32_t rate)
1168 {
1169 int i;
1170
1171 for (i = 0; i < (ARRAY_SIZE(LSM6DSMImuRates) - 1); i++) {
1172 if (LSM6DSMImuRates[i] == rate)
1173 break;
1174 }
1175 if (i == (ARRAY_SIZE(LSM6DSMImuRates) -1 )) {
1176 ERROR_PRINT("ODR not valid! Selected smallest ODR available\n");
1177 i = 0;
1178 }
1179
1180 return i;
1181 }
1182
1183 /*
1184 * lsm6dsm_getAccelHwMinOdr: verify minimum odr needed by accel in order to satisfy dependencies
1185 */
lsm6dsm_getAccelHwMinOdr()1186 static uint8_t lsm6dsm_getAccelHwMinOdr()
1187 {
1188 TDECL();
1189 uint32_t minRate = SENSOR_HZ(26.0f / 2.0f);
1190
1191 if (T(accelSensorDependencies) & BIT(ACCEL)) {
1192 if (minRate < T(sensors[ACCEL]).rate)
1193 minRate = T(sensors[ACCEL]).rate;
1194 }
1195
1196 /* Embedded functions are enabled, min odr required is 26Hz */
1197 if (T(embeddedFunctionsDependencies)) {
1198 if (minRate < SENSOR_HZ(26.0f))
1199 minRate = SENSOR_HZ(26.0f);
1200 }
1201
1202 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
1203 if (T(accelSensorDependencies) & BIT(GYRO)) {
1204 if (minRate < T(sensors[GYRO].rate))
1205 minRate = T(sensors[GYRO].rate);
1206 }
1207 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
1208
1209 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1210 if (T(accelSensorDependencies) & BIT(MAGN)) {
1211 if (minRate < T(sensors[MAGN].rate))
1212 minRate = T(sensors[MAGN].rate);
1213 }
1214 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1215
1216 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
1217 if (T(accelSensorDependencies) & BIT(PRESS)) {
1218 if (minRate < T(sensors[PRESS].rate))
1219 minRate = T(sensors[PRESS].rate);
1220 }
1221
1222 if (T(accelSensorDependencies) & BIT(TEMP)) {
1223 if (minRate < T(sensors[TEMP].rate))
1224 minRate = T(sensors[TEMP].rate);
1225 }
1226 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
1227
1228 /* This call return index of LSM6DSMImuRates struct element */
1229 return lsm6dsm_computeOdr(minRate);
1230 }
1231
1232 /*
1233 * lsm6dsm_setTriggerRate: detect between accel & gyro fastest odr
1234 */
lsm6dsm_setTriggerRate()1235 static void lsm6dsm_setTriggerRate()
1236 {
1237 TDECL();
1238 uint8_t i;
1239
1240 i = lsm6dsm_getAccelHwMinOdr();
1241
1242 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
1243 T(triggerRate) = SENSOR_HZ_RATE_TO_US(LSM6DSMImuRates[i]);
1244 #else /* LSM6DSM_GYRO_CALIB_ENABLED */
1245 uint32_t maxRate = LSM6DSMImuRates[i];
1246
1247 if (maxRate < T(sensors[GYRO]).hwRate)
1248 maxRate = T(sensors[GYRO]).hwRate;
1249
1250 T(triggerRate) = SENSOR_HZ_RATE_TO_US(maxRate);
1251 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
1252 }
1253
1254 /*
1255 * lsm6dsm_updateAccelOdr: update accel odr based on enabled dependencies
1256 */
lsm6dsm_updateAccelOdr()1257 static bool lsm6dsm_updateAccelOdr()
1258 {
1259 TDECL();
1260 uint8_t i;
1261
1262 /* If no one is using accel disable it. If dependencies are using accel try to reduce ODR */
1263 if (T(accelSensorDependencies) == 0) {
1264 DEBUG_PRINT("updateAccelOdr: no one is using accel, disabling it\n");
1265 T(sensors[ACCEL]).hwRate = 0;
1266 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
1267 lsm6dsm_setTriggerRate();
1268 } else {
1269 i = lsm6dsm_getAccelHwMinOdr();
1270
1271 T(sensors[ACCEL]).samplesDecimator = LSM6DSMImuRates[i] / T(sensors[ACCEL]).rate;
1272 T(sensors[ACCEL]).samplesCounter = T(sensors[ACCEL]).samplesDecimator - 1;
1273 T(sensors[ACCEL]).samplesToDiscard = LSM6DSMAccelRatesSamplesToDiscard[i];
1274 T(sensors[ACCEL]).hwRate = LSM6DSMImuRates[i];
1275
1276 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1277 if (T(accelSensorDependencies) & BIT(MAGN)) {
1278 if (T(sensors[ACCEL]).hwRate > SENSOR_HZ(104.0f))
1279 T(sensors[MAGN]).samplesDecimator = SENSOR_HZ(104.0f) / T(sensors[MAGN]).rate;
1280 else
1281 T(sensors[MAGN]).samplesDecimator = T(sensors[ACCEL]).hwRate / T(sensors[MAGN]).rate;
1282
1283 T(sensors[MAGN]).samplesCounter = T(sensors[MAGN]).samplesDecimator - 1;
1284 T(sensors[MAGN]).samplesToDiscard = 1;
1285 }
1286 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1287
1288 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
1289 if (T(accelSensorDependencies) & BIT(PRESS)) {
1290 if (T(sensors[ACCEL]).hwRate > SENSOR_HZ(104.0f))
1291 T(sensors[PRESS]).samplesDecimator = SENSOR_HZ(104.0f) / T(sensors[PRESS]).rate;
1292 else
1293 T(sensors[PRESS]).samplesDecimator = T(sensors[ACCEL]).hwRate / T(sensors[PRESS]).rate;
1294
1295 T(sensors[PRESS]).samplesCounter = T(sensors[PRESS]).samplesDecimator - 1;
1296 T(sensors[PRESS]).samplesToDiscard = 1;
1297 }
1298
1299 if (T(accelSensorDependencies) & BIT(TEMP)) {
1300 if (T(sensors[ACCEL]).hwRate > SENSOR_HZ(104.0f))
1301 T(sensors[TEMP]).samplesDecimator = SENSOR_HZ(104.0f) / T(sensors[TEMP]).rate;
1302 else
1303 T(sensors[TEMP]).samplesDecimator = T(sensors[ACCEL]).hwRate / T(sensors[TEMP]).rate;
1304
1305 T(sensors[TEMP]).samplesCounter = T(sensors[TEMP]).samplesDecimator - 1;
1306 T(sensors[TEMP]).samplesToDiscard = 1;
1307 }
1308 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
1309
1310 lsm6dsm_setTriggerRate();
1311
1312 DEBUG_PRINT("updateAccelOdr: accel in use, updating odr to %dHz\n", (int)(T(sensors[ACCEL]).hwRate / 1024));
1313
1314 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSMImuRatesRegValue[i]);
1315 }
1316
1317 return true;
1318 }
1319
1320 /*
1321 * lsm6dsm_setAccelPower: enable/disable accelerometer sensor
1322 */
lsm6dsm_setAccelPower(bool on,void * cookie)1323 static bool lsm6dsm_setAccelPower(bool on, void *cookie)
1324 {
1325 TDECL();
1326
1327 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1328 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1329 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1330 INFO_PRINT("setAccelPower: %s\n", on ? "enable" : "disable");
1331
1332 if (on) {
1333 T(accelSensorDependencies) |= BIT(ACCEL);
1334 T(int1Register) |= LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
1335
1336 /* Discard same samples if interrupts are generated during INT enable */
1337 T(sensors[ACCEL]).samplesToDiscard = 255;
1338
1339 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1340 } else {
1341 T(accelSensorDependencies) &= ~BIT(ACCEL);
1342 T(int1Register) &= ~LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
1343
1344 lsm6dsm_updateAccelOdr();
1345
1346 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1347 }
1348
1349 /* If enable, set only INT bit enable (sensor will be switched on by setRate function). If disable, it depends on accelSensorDependencies */
1350 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
1351 } else {
1352 T(pendingEnableConfig[ACCEL]) = true;
1353 T(sensors[ACCEL]).pConfig.enable = on;
1354 }
1355
1356 return true;
1357 }
1358
1359 /*
1360 * lsm6dsm_setGyroPower: enable/disable gyroscope sensor
1361 */
lsm6dsm_setGyroPower(bool on,void * cookie)1362 static bool lsm6dsm_setGyroPower(bool on, void *cookie)
1363 {
1364 TDECL();
1365
1366 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1367 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1368 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1369 INFO_PRINT("setGyroPower: %s\n", on ? "enable" : "disable");
1370
1371 if (on) {
1372 T(int1Register) |= LSM6DSM_INT_GYRO_ENABLE_REG_VALUE;
1373
1374 /* Discard same samples if interrupts are generated during INT enable */
1375 T(sensors[GYRO]).samplesToDiscard = 255;
1376
1377 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1378 } else {
1379 T(int1Register) &= ~LSM6DSM_INT_GYRO_ENABLE_REG_VALUE;
1380
1381 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
1382 T(accelSensorDependencies) &= ~BIT(GYRO);
1383
1384 if (!T(sensors[ACCEL].enabled))
1385 T(int1Register) &= ~LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
1386 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
1387
1388 T(sensors[GYRO]).hwRate = 0;
1389
1390 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1391 SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE);
1392
1393 lsm6dsm_updateAccelOdr();
1394 }
1395
1396 /* 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) */
1397 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
1398 } else {
1399 T(pendingEnableConfig[GYRO]) = true;
1400 T(sensors[GYRO]).pConfig.enable = on;
1401 }
1402
1403 return true;
1404 }
1405
1406 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1407 /*
1408 * lsm6dsm_setMagnPower: enable/disable magnetometer sensor
1409 */
lsm6dsm_setMagnPower(bool on,void * cookie)1410 static bool lsm6dsm_setMagnPower(bool on, void *cookie)
1411 {
1412 TDECL();
1413
1414 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1415 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1416 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1417 INFO_PRINT("setMagnPower: %s\n", on ? "enable" : "disable");
1418
1419 if (on) {
1420 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1421
1422 /* Discard same samples if interrupts are generated during INT enable before switch on sensor */
1423 T(sensors[MAGN]).samplesToDiscard = 255;
1424
1425 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1426 } else {
1427 T(accelSensorDependencies) &= ~BIT(MAGN);
1428 T(embeddedFunctionsDependencies) &= ~BIT(MAGN);
1429
1430 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR,
1431 LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE,
1432 T(sensors[ACCEL]).hwRate, MAGN);
1433
1434 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
1435 if (!(T(sensors[PRESS].enabled) || T(sensors[TEMP].enabled))) {
1436 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
1437 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1438
1439 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1440 }
1441 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
1442
1443 if (T(embeddedFunctionsDependencies) == 0) {
1444 DEBUG_PRINT("setMagnPower: no embedded sensors on, disabling digital functions\n");
1445 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
1446 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1447 }
1448
1449 T(sensors[MAGN]).hwRate = 0;
1450
1451 lsm6dsm_updateAccelOdr();
1452 }
1453
1454 /* If enable, set only INT bit enable (sensor will be switched on by setRate function).
1455 If disable, disable INT bit, disable sensor-hub and disable ODR (power-off sensor) */
1456 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
1457 } else {
1458 T(pendingEnableConfig[MAGN]) = true;
1459 T(sensors[MAGN]).pConfig.enable = on;
1460 }
1461
1462 return true;
1463 }
1464 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1465
1466 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
1467 /*
1468 * lsm6dsm_setPressPower: enable/disable pressure sensor
1469 */
lsm6dsm_setPressPower(bool on,void * cookie)1470 static bool lsm6dsm_setPressPower(bool on, void *cookie)
1471 {
1472 TDECL();
1473 uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;
1474
1475 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1476 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1477 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1478 INFO_PRINT("setPressPower: %s\n", on ? "enable" : "disable");
1479
1480 if (on) {
1481 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1482
1483 /* Discard same samples if interrupts are generated during INT enable before switch on sensor */
1484 T(sensors[PRESS]).samplesToDiscard = 255;
1485
1486 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1487 } else {
1488 T(accelSensorDependencies) &= ~BIT(PRESS);
1489 T(embeddedFunctionsDependencies) &= ~BIT(PRESS);
1490
1491 if (T(sensors[TEMP]).enabled) {
1492 i = lsm6dsm_computeOdr(T(sensors[TEMP]).rate);
1493 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i);
1494 } else
1495 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE;
1496
1497 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value,
1498 T(sensors[PRESS]).hwRate, PRESS);
1499
1500 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1501 if (!(T(sensors[MAGN].enabled) || T(sensors[TEMP].enabled))) {
1502 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
1503 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1504
1505 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1506 }
1507 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1508 if (!T(sensors[TEMP].enabled)) {
1509 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
1510 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1511
1512 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1513 }
1514 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1515
1516 if (T(embeddedFunctionsDependencies) == 0) {
1517 DEBUG_PRINT("setPressPower: no embedded sensors on, disabling digital functions\n");
1518 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
1519 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1520 }
1521
1522 T(sensors[PRESS]).hwRate = 0;
1523
1524 lsm6dsm_updateAccelOdr();
1525 }
1526
1527 /* If enable, set only INT bit enable (sensor will be switched on by setRate function).
1528 If disable, disable INT bit, disable sensor-hub and disable ODR (power-off sensor) */
1529 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
1530 } else {
1531 T(pendingEnableConfig[PRESS]) = true;
1532 T(sensors[PRESS]).pConfig.enable = on;
1533 }
1534
1535 return true;
1536 }
1537
1538 /*
1539 * lsm6dsm_setTempPower: enable/disable temperature sensor
1540 */
lsm6dsm_setTempPower(bool on,void * cookie)1541 static bool lsm6dsm_setTempPower(bool on, void *cookie)
1542 {
1543 TDECL();
1544 uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;
1545
1546 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1547 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1548 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1549 INFO_PRINT("setTempPower: %s\n", on ? "enable" : "disable");
1550
1551 if (on) {
1552 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1553
1554 /* Discard same samples if interrupts are generated during INT enable before switch on sensor */
1555 T(sensors[TEMP]).samplesToDiscard = 255;
1556
1557 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1558 } else {
1559 T(accelSensorDependencies) &= ~BIT(TEMP);
1560 T(embeddedFunctionsDependencies) &= ~BIT(TEMP);
1561
1562 if (T(sensors[PRESS]).enabled) {
1563 i = lsm6dsm_computeOdr(T(sensors[PRESS]).rate);
1564 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i);
1565 } else
1566 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE;
1567
1568 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value,
1569 T(sensors[TEMP]).hwRate, PRESS);
1570
1571 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1572 if (!(T(sensors[MAGN].enabled) || T(sensors[PRESS].enabled))) {
1573 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
1574 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1575
1576 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1577 }
1578 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1579 if (!T(sensors[PRESS].enabled)) {
1580 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
1581 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_DRDY_ON_INT1;
1582
1583 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
1584 }
1585 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1586
1587 if (T(embeddedFunctionsDependencies) == 0) {
1588 DEBUG_PRINT("setTempPower: no embedded sensors on, disabling digital functions\n");
1589 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
1590 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1591 }
1592
1593 T(sensors[TEMP]).hwRate = 0;
1594
1595 lsm6dsm_updateAccelOdr();
1596 }
1597
1598 /* If enable, set only INT bit enable (sensor will be switched on by setRate function).
1599 If disable, disable INT bit, disable sensor-hub and disable ODR (power-off sensor) */
1600 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
1601 } else {
1602 T(pendingEnableConfig[TEMP]) = true;
1603 T(sensors[TEMP]).pConfig.enable = on;
1604 }
1605
1606 return true;
1607 }
1608 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
1609
1610 /*
1611 * lsm6dsm_setStepDetectorPower: enable/disable step detector sensor
1612 */
lsm6dsm_setStepDetectorPower(bool on,void * cookie)1613 static bool lsm6dsm_setStepDetectorPower(bool on, void *cookie)
1614 {
1615 TDECL();
1616
1617 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1618 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1619 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1620 INFO_PRINT("setStepDetectorPower: %s\n", on ? "enable" : "disable");
1621
1622 if (on) {
1623 T(accelSensorDependencies) |= BIT(STEP_DETECTOR);
1624 T(embeddedFunctionsDependencies) |= BIT(STEP_DETECTOR);
1625 T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC | LSM6DSM_ENABLE_DIGITAL_FUNC);
1626 T(int1Register) |= LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE;
1627
1628 lsm6dsm_updateAccelOdr();
1629
1630 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1631 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1632 } else {
1633 T(accelSensorDependencies) &= ~BIT(STEP_DETECTOR);
1634 T(embeddedFunctionsDependencies) &= ~BIT(STEP_DETECTOR);
1635 T(int1Register) &= ~LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE;
1636
1637 if ((T(embeddedFunctionsDependencies) & (BIT(STEP_COUNTER) | BIT(SIGN_MOTION))) == 0) {
1638 DEBUG_PRINT("setStepDetectorPower: no more need pedometer algo, disabling it\n");
1639 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
1640 }
1641
1642 if (T(embeddedFunctionsDependencies) == 0) {
1643 DEBUG_PRINT("setStepDetectorPower: no embedded sensors on, disabling digital functions\n");
1644 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
1645 }
1646
1647 lsm6dsm_updateAccelOdr();
1648
1649 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1650 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1651 }
1652
1653 /* If enable, set INT bit enable and enable accelerometer sensor @26Hz if disabled. If disable, disable INT bit and disable accelerometer if no one need it */
1654 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[STEP_DETECTOR]), __FUNCTION__);
1655 } else {
1656 T(pendingEnableConfig[STEP_DETECTOR]) = true;
1657 T(sensors[STEP_DETECTOR]).pConfig.enable = on;
1658 }
1659
1660 return true;
1661 }
1662
1663 /*
1664 * lsm6dsm_setStepCounterPower: enable/disable step counter sensor
1665 */
lsm6dsm_setStepCounterPower(bool on,void * cookie)1666 static bool lsm6dsm_setStepCounterPower(bool on, void *cookie)
1667 {
1668 TDECL();
1669
1670 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1671 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1672 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1673 INFO_PRINT("setStepCounterPower: %s\n", on ? "enable" : "disable");
1674
1675 if (on) {
1676 T(readSteps) = false;
1677 T(accelSensorDependencies) |= BIT(STEP_COUNTER);
1678 T(embeddedFunctionsDependencies) |= BIT(STEP_COUNTER);
1679 T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC | LSM6DSM_ENABLE_DIGITAL_FUNC);
1680 T(int2Register) |= LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE;
1681
1682 lsm6dsm_updateAccelOdr();
1683
1684 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1685 SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register));
1686 } else {
1687 T(accelSensorDependencies) &= ~BIT(STEP_COUNTER);
1688 T(embeddedFunctionsDependencies) &= ~BIT(STEP_COUNTER);
1689 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_TIMER_DIGITAL_FUNC;
1690 T(int2Register) &= ~LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE;
1691
1692 if ((T(embeddedFunctionsDependencies) & (BIT(STEP_DETECTOR) | BIT(SIGN_MOTION))) == 0) {
1693 DEBUG_PRINT("setStepCounterPower: no more need pedometer algo, disabling it\n");
1694 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
1695 }
1696
1697 if (T(embeddedFunctionsDependencies) == 0) {
1698 DEBUG_PRINT("setStepCounterPower: no embedded sensors on, disabling digital functions\n");
1699 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
1700 }
1701
1702 lsm6dsm_updateAccelOdr();
1703
1704 SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register));
1705 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1706 }
1707
1708 /* If enable, set INT bit enable and enable accelerometer sensor @26Hz if disabled. If disable, disable INT bit and disable accelerometer if no one need it */
1709 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[STEP_COUNTER]), __FUNCTION__);
1710 } else {
1711 T(pendingEnableConfig[STEP_COUNTER]) = true;
1712 T(sensors[STEP_COUNTER]).pConfig.enable = on;
1713 }
1714
1715 return true;
1716 }
1717
1718 /*
1719 * lsm6dsm_setSignMotionPower: enable/disable significant motion sensor
1720 */
lsm6dsm_setSignMotionPower(bool on,void * cookie)1721 static bool lsm6dsm_setSignMotionPower(bool on, void *cookie)
1722 {
1723 TDECL();
1724
1725 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
1726 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
1727 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
1728 INFO_PRINT("setSignMotionPower: %s\n", on ? "enable" : "disable");
1729
1730 if (on) {
1731 T(accelSensorDependencies) |= BIT(SIGN_MOTION);
1732 T(embeddedFunctionsDependencies) |= BIT(SIGN_MOTION);
1733 T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC | LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC | LSM6DSM_ENABLE_DIGITAL_FUNC);
1734 T(int1Register) |= LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE;
1735
1736 lsm6dsm_updateAccelOdr();
1737
1738 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1739 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1740 } else {
1741 T(accelSensorDependencies) &= ~BIT(SIGN_MOTION);
1742 T(embeddedFunctionsDependencies) &= ~BIT(SIGN_MOTION);
1743 T(int1Register) &= ~LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE;
1744
1745 if ((T(embeddedFunctionsDependencies) & (BIT(STEP_DETECTOR) | BIT(STEP_COUNTER))) == 0) {
1746 DEBUG_PRINT("setSignMotionPower: no more need pedometer algo, disabling it\n");
1747 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC;
1748 }
1749
1750 if (T(embeddedFunctionsDependencies) == 0) {
1751 DEBUG_PRINT("setSignMotionPower: no embedded sensors on, disabling digital functions\n");
1752 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_DIGITAL_FUNC;
1753 }
1754
1755 lsm6dsm_updateAccelOdr();
1756
1757 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register), 50000);
1758 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
1759 }
1760
1761 /* If enable, set INT bit enable and enable accelerometer sensor @26Hz if disabled. If disable, disable INT bit and disable accelerometer if no one need it */
1762 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[SIGN_MOTION]), __FUNCTION__);
1763 } else {
1764 T(pendingEnableConfig[SIGN_MOTION]) = true;
1765 T(sensors[SIGN_MOTION]).pConfig.enable = on;
1766 }
1767
1768 return true;
1769 }
1770
1771 /*
1772 * lsm6dsm_accelFirmwareUpload: upload accelerometer firmware
1773 */
lsm6dsm_accelFirmwareUpload(void * cookie)1774 static bool lsm6dsm_accelFirmwareUpload(void *cookie)
1775 {
1776 TDECL();
1777
1778 sensorSignalInternalEvt(T(sensors[ACCEL]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1779
1780 return true;
1781 }
1782
1783 /*
1784 * lsm6dsm_gyroFirmwareUpload: upload gyroscope firmware
1785 */
lsm6dsm_gyroFirmwareUpload(void * cookie)1786 static bool lsm6dsm_gyroFirmwareUpload(void *cookie)
1787 {
1788 TDECL();
1789
1790 sensorSignalInternalEvt(T(sensors[GYRO]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1791
1792 return true;
1793 }
1794
1795 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1796 /*
1797 * lsm6dsm_magnFirmwareUpload: upload magnetometer firmware
1798 */
lsm6dsm_magnFirmwareUpload(void * cookie)1799 static bool lsm6dsm_magnFirmwareUpload(void *cookie)
1800 {
1801 TDECL();
1802
1803 sensorSignalInternalEvt(T(sensors[MAGN]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1804
1805 return true;
1806 }
1807 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1808
1809 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
1810 /*
1811 * lsm6dsm_pressFirmwareUpload: upload pressure firmware
1812 */
lsm6dsm_pressFirmwareUpload(void * cookie)1813 static bool lsm6dsm_pressFirmwareUpload(void *cookie)
1814 {
1815 TDECL();
1816
1817 sensorSignalInternalEvt(T(sensors[PRESS]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1818
1819 return true;
1820 }
1821
1822 /*
1823 * lsm6dsm_tempFirmwareUpload: upload pressure firmware
1824 */
lsm6dsm_tempFirmwareUpload(void * cookie)1825 static bool lsm6dsm_tempFirmwareUpload(void *cookie)
1826 {
1827 TDECL();
1828
1829 sensorSignalInternalEvt(T(sensors[TEMP]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1830
1831 return true;
1832 }
1833 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
1834
1835 /*
1836 * lsm6dsm_stepDetectorFirmwareUpload: upload step detector firmware
1837 */
lsm6dsm_stepDetectorFirmwareUpload(void * cookie)1838 static bool lsm6dsm_stepDetectorFirmwareUpload(void *cookie)
1839 {
1840 TDECL();
1841
1842 sensorSignalInternalEvt(T(sensors[STEP_DETECTOR]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1843
1844 return true;
1845 }
1846
1847 /*
1848 * lsm6dsm_stepCounterFirmwareUpload: upload step counter firmware
1849 */
lsm6dsm_stepCounterFirmwareUpload(void * cookie)1850 static bool lsm6dsm_stepCounterFirmwareUpload(void *cookie)
1851 {
1852 TDECL();
1853
1854 sensorSignalInternalEvt(T(sensors[STEP_COUNTER]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1855
1856 return true;
1857 }
1858
1859 /*
1860 * lsm6dsm_signMotionFirmwareUpload: upload significant motion firmware
1861 */
lsm6dsm_signMotionFirmwareUpload(void * cookie)1862 static bool lsm6dsm_signMotionFirmwareUpload(void *cookie)
1863 {
1864 TDECL();
1865
1866 sensorSignalInternalEvt(T(sensors[SIGN_MOTION]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
1867
1868 return true;
1869 }
1870
1871 /*
1872 * lsm6dsm_setAccelRate: set accelerometer ODR and report latency (FIFO watermark related)
1873 */
lsm6dsm_setAccelRate(uint32_t rate,uint64_t latency,void * cookie)1874 static bool lsm6dsm_setAccelRate(uint32_t rate, uint64_t latency, void *cookie)
1875 {
1876 TDECL();
1877
1878 if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
1879 INFO_PRINT("setAccelRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
1880
1881 T(sensors[ACCEL]).rate = rate;
1882 T(sensors[ACCEL]).latency = latency;
1883
1884 lsm6dsm_updateAccelOdr();
1885
1886 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
1887 } else {
1888 T(pendingRateConfig[ACCEL]) = true;
1889 T(sensors[ACCEL].pConfig.rate) = rate;
1890 T(sensors[ACCEL]).pConfig.latency = latency;
1891 }
1892
1893 return true;
1894 }
1895
1896 /*
1897 * lsm6dsm_setGyroRate: set gyroscope ODR and report latency (FIFO watermark related)
1898 */
lsm6dsm_setGyroRate(uint32_t rate,uint64_t latency,void * cookie)1899 static bool lsm6dsm_setGyroRate(uint32_t rate, uint64_t latency, void *cookie)
1900 {
1901 TDECL();
1902 uint8_t i;
1903
1904 if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
1905 INFO_PRINT("setGyroRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
1906
1907 /* This call return index of LSM6DSMImuRates struct element */
1908 i = lsm6dsm_computeOdr(rate);
1909
1910 T(sensors[GYRO]).rate = rate;
1911 T(sensors[GYRO]).latency = latency;
1912 T(sensors[GYRO]).samplesToDiscard = LSM6DSMGyroRatesSamplesToDiscard[i];
1913
1914 if (T(sensors[GYRO]).hwRate == 0)
1915 T(sensors[GYRO]).samplesToDiscard += LSM6DSMRatesSamplesToDiscardGyroPowerOn[i];
1916
1917 T(sensors[GYRO]).hwRate = rate < (SENSOR_HZ(26.0f) / 2) ? (SENSOR_HZ(26.0f) / 2) : rate;
1918 T(sensors[GYRO]).samplesDecimator = T(sensors[GYRO]).hwRate / rate;
1919 T(sensors[GYRO]).samplesCounter = T(sensors[GYRO]).samplesDecimator - 1;
1920
1921 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
1922 /* Gyroscope bias calibration library requires accelerometer data, enabling it @26Hz if not enabled */
1923 T(accelSensorDependencies) |= BIT(GYRO);
1924 T(int1Register) |= LSM6DSM_INT_ACCEL_ENABLE_REG_VALUE;
1925
1926 lsm6dsm_updateAccelOdr();
1927
1928 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
1929 #else /* LSM6DSM_GYRO_CALIB_ENABLED */
1930 lsm6dsm_setTriggerRate();
1931 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
1932
1933 SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | LSM6DSMImuRatesRegValue[i]);
1934
1935 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
1936 } else {
1937 T(pendingRateConfig[GYRO]) = true;
1938 T(sensors[GYRO]).pConfig.rate = rate;
1939 T(sensors[GYRO]).pConfig.latency = latency;
1940 }
1941
1942 return true;
1943 }
1944
1945 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
1946 /*
1947 * lsm6dsm_setMagnRate: set magnetometer ODR and report latency (FIFO watermark related)
1948 */
lsm6dsm_setMagnRate(uint32_t rate,uint64_t latency,void * cookie)1949 static bool lsm6dsm_setMagnRate(uint32_t rate, uint64_t latency, void *cookie)
1950 {
1951 TDECL();
1952 uint8_t i, buffer[2];
1953
1954 if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
1955 INFO_PRINT("setMagnRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
1956
1957 T(embeddedFunctionsDependencies) |= BIT(MAGN);
1958 T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_DIGITAL_FUNC;
1959 T(accelSensorDependencies) |= BIT(MAGN);
1960
1961 T(sensors[MAGN]).rate = rate;
1962 T(sensors[MAGN]).latency = latency;
1963
1964 lsm6dsm_updateAccelOdr();
1965
1966 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
1967
1968 buffer[0] = T(embeddedFunctionsRegister); /* LSM6DSM_CTRL10_C */
1969 buffer[1] = T(masterConfigRegister); /* LSM6DSM_MASTER_CONFIG */
1970 SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
1971
1972 /* This call return index of LSM6DSMImuRates struct element */
1973 i = lsm6dsm_computeOdr(rate);
1974 T(sensors[MAGN]).hwRate = LSM6DSMSHRates[i];
1975
1976 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR
1977 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
1978 LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i),
1979 T(sensors[ACCEL]).hwRate, MAGN);
1980 #else /* LSM6DSM_I2C_MASTER_LSM303AGR */
1981 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR,
1982 LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE,
1983 T(sensors[ACCEL]).hwRate, MAGN);
1984 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
1985 LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i),
1986 T(sensors[ACCEL]).hwRate, MAGN);
1987 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */
1988
1989 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
1990 } else {
1991 T(pendingRateConfig[MAGN]) = true;
1992 T(sensors[MAGN]).pConfig.rate = rate;
1993 T(sensors[MAGN]).pConfig.latency = latency;
1994 }
1995
1996 return true;
1997 }
1998 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
1999
2000 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
2001 /*
2002 * lsm6dsm_setPressRate: set pressure ODR and report latency (FIFO watermark related)
2003 */
lsm6dsm_setPressRate(uint32_t rate,uint64_t latency,void * cookie)2004 static bool lsm6dsm_setPressRate(uint32_t rate, uint64_t latency, void *cookie)
2005 {
2006 TDECL();
2007 uint8_t i, buffer[2];
2008
2009 if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
2010 INFO_PRINT("setPressRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
2011
2012 T(embeddedFunctionsDependencies) |= BIT(PRESS);
2013 T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_DIGITAL_FUNC;
2014 T(accelSensorDependencies) |= BIT(PRESS);
2015
2016 T(sensors[PRESS]).rate = rate;
2017 T(sensors[PRESS]).latency = latency;
2018
2019 lsm6dsm_updateAccelOdr();
2020
2021 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
2022
2023 buffer[0] = T(embeddedFunctionsRegister); /* LSM6DSM_CTRL10_C */
2024 buffer[1] = T(masterConfigRegister); /* LSM6DSM_MASTER_CONFIG */
2025 SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
2026
2027 if (T(sensors[TEMP]).enabled) {
2028 if (rate < T(sensors[TEMP]).rate)
2029 rate = T(sensors[TEMP]).rate;
2030 }
2031
2032 /* This call return index of LSM6DSMImuRates struct element */
2033 i = lsm6dsm_computeOdr(rate);
2034 T(sensors[PRESS]).hwRate = LSM6DSMSHRates[i];
2035
2036 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR,
2037 LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i),
2038 T(sensors[ACCEL]).hwRate, PRESS);
2039
2040 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
2041 } else {
2042 T(pendingRateConfig[PRESS]) = true;
2043 T(sensors[PRESS]).pConfig.rate = rate;
2044 T(sensors[PRESS]).pConfig.latency = latency;
2045 }
2046
2047 return true;
2048 }
2049
2050 /*
2051 * lsm6dsm_setTempRate: set temperature ODR and report latency (FIFO watermark related)
2052 */
lsm6dsm_setTempRate(uint32_t rate,uint64_t latency,void * cookie)2053 static bool lsm6dsm_setTempRate(uint32_t rate, uint64_t latency, void *cookie)
2054 {
2055 TDECL();
2056 uint8_t i, buffer[2];
2057
2058 if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
2059 INFO_PRINT("setTempRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
2060
2061 T(embeddedFunctionsDependencies) |= BIT(TEMP);
2062 T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_DIGITAL_FUNC;
2063 T(accelSensorDependencies) |= BIT(TEMP);
2064
2065 T(sensors[TEMP]).rate = rate;
2066 T(sensors[TEMP]).hwRate = rate;
2067 T(sensors[TEMP]).latency = latency;
2068
2069 lsm6dsm_updateAccelOdr();
2070
2071 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;
2072
2073 buffer[0] = T(embeddedFunctionsRegister); /* LSM6DSM_CTRL10_C */
2074 buffer[1] = T(masterConfigRegister); /* LSM6DSM_MASTER_CONFIG */
2075 SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
2076
2077 if (T(sensors[PRESS]).enabled) {
2078 if (rate < T(sensors[PRESS]).rate)
2079 rate = T(sensors[PRESS]).rate;
2080 }
2081
2082 /* This call return index of LSM6DSMImuRates struct element */
2083 i = lsm6dsm_computeOdr(rate);
2084 T(sensors[TEMP]).hwRate = LSM6DSMSHRates[i];
2085
2086 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR,
2087 LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i),
2088 T(sensors[ACCEL]).hwRate, TEMP);
2089
2090 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
2091 } else {
2092 T(pendingRateConfig[TEMP]) = true;
2093 T(sensors[TEMP]).pConfig.rate = rate;
2094 T(sensors[TEMP]).pConfig.latency = latency;
2095 }
2096
2097 return true;
2098 }
2099 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
2100
2101 /*
2102 * lsm6dsm_setStepDetectorRate: set step detector report latency
2103 */
lsm6dsm_setStepDetectorRate(uint32_t rate,uint64_t latency,void * cookie)2104 static bool lsm6dsm_setStepDetectorRate(uint32_t rate, uint64_t latency, void *cookie)
2105 {
2106 TDECL();
2107
2108 INFO_PRINT("setStepDetectorRate: latency=%lldns\n", latency);
2109
2110 T(sensors[STEP_DETECTOR]).rate = rate;
2111 T(sensors[STEP_DETECTOR]).latency = latency;
2112
2113 sensorSignalInternalEvt(T(sensors[STEP_DETECTOR]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
2114
2115 return true;
2116 }
2117
2118 /*
2119 * lsm6dsm_setStepCounterRate: set step counter report latency
2120 */
lsm6dsm_setStepCounterRate(uint32_t rate,uint64_t latency,void * cookie)2121 static bool lsm6dsm_setStepCounterRate(uint32_t rate, uint64_t latency, void *cookie)
2122 {
2123 TDECL();
2124 uint8_t i, step_delta_reg;
2125
2126 if (rate == SENSOR_RATE_ONCHANGE) {
2127 INFO_PRINT("setStepCounterRate: delivery-rate=on_change, latency=%lldns\n", latency);
2128 } else
2129 INFO_PRINT("setStepCounterRate: delivery_rate=%dms, latency=%lldns\n", (int)((1024.0f / rate) * 1000.0f), latency);
2130
2131 if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
2132 T(sensors[STEP_COUNTER]).rate = rate;
2133 T(sensors[STEP_COUNTER]).latency = latency;
2134
2135 for (i = 0; i < ARRAY_SIZE(LSM6DSMStepCounterRates); i++) {
2136 if (rate == LSM6DSMStepCounterRates[i])
2137 break;
2138 }
2139 if (i >= (ARRAY_SIZE(LSM6DSMStepCounterRates) - 2))
2140 step_delta_reg = 0;
2141 else
2142 step_delta_reg = (128 >> i);
2143
2144 T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_TIMER_DIGITAL_FUNC;
2145 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
2146
2147 lsm6dsm_writeEmbeddedRegister(LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR, step_delta_reg);
2148
2149 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
2150 } else {
2151 T(pendingRateConfig[STEP_COUNTER]) = true;
2152 T(sensors[STEP_COUNTER]).pConfig.rate = rate;
2153 T(sensors[STEP_COUNTER]).pConfig.latency = latency;
2154 }
2155
2156 return true;
2157 }
2158
2159 /*
2160 * lsm6dsm_setSignMotionRate: set significant motion report latency
2161 */
lsm6dsm_setSignMotionRate(uint32_t rate,uint64_t latency,void * cookie)2162 static bool lsm6dsm_setSignMotionRate(uint32_t rate, uint64_t latency, void *cookie)
2163 {
2164 TDECL();
2165
2166 DEBUG_PRINT("setSignMotionRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);
2167
2168 T(sensors[SIGN_MOTION]).rate = rate;
2169 T(sensors[SIGN_MOTION]).latency = latency;
2170
2171 sensorSignalInternalEvt(T(sensors[SIGN_MOTION]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
2172
2173 return true;
2174 }
2175
2176 /*
2177 * lsm6dsm_accelFlush: send accelerometer flush event
2178 */
lsm6dsm_accelFlush(void * cookie)2179 static bool lsm6dsm_accelFlush(void *cookie)
2180 {
2181 INFO_PRINT("accelFlush\n");
2182
2183 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL);
2184
2185 return true;
2186 }
2187
2188 /*
2189 * lsm6dsm_gyroFlush: send gyroscope flush event
2190 */
lsm6dsm_gyroFlush(void * cookie)2191 static bool lsm6dsm_gyroFlush(void *cookie)
2192 {
2193 INFO_PRINT("gyroFlush\n");
2194
2195 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO), SENSOR_DATA_EVENT_FLUSH, NULL);
2196
2197 return true;
2198 }
2199
2200 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
2201 /*
2202 * lsm6dsm_magnFlush: send magnetometer flush event
2203 */
lsm6dsm_magnFlush(void * cookie)2204 static bool lsm6dsm_magnFlush(void *cookie)
2205 {
2206 INFO_PRINT("magnFlush\n");
2207
2208 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL);
2209
2210 return true;
2211 }
2212 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
2213
2214 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
2215 /*
2216 * lsm6dsm_pressFlush: send pressure flush event
2217 */
lsm6dsm_pressFlush(void * cookie)2218 static bool lsm6dsm_pressFlush(void *cookie)
2219 {
2220 return true;
2221 }
2222
2223 /*
2224 * lsm6dsm_tempFlush: send temperature flush event
2225 */
lsm6dsm_tempFlush(void * cookie)2226 static bool lsm6dsm_tempFlush(void *cookie)
2227 {
2228 return true;
2229 }
2230 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
2231
2232 /*
2233 * lsm6dsm_stepDetectorFlush: send step detector flush event
2234 */
lsm6dsm_stepDetectorFlush(void * cookie)2235 static bool lsm6dsm_stepDetectorFlush(void *cookie)
2236 {
2237 INFO_PRINT("stepDetectorFlush\n");
2238
2239 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), SENSOR_DATA_EVENT_FLUSH, NULL);
2240
2241 return true;
2242 }
2243
2244 /*
2245 * lsm6dsm_stepCounterFlush: send step counter flush event
2246 */
lsm6dsm_stepCounterFlush(void * cookie)2247 static bool lsm6dsm_stepCounterFlush(void *cookie)
2248 {
2249 INFO_PRINT("stepCounterFlush\n");
2250
2251 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), SENSOR_DATA_EVENT_FLUSH, NULL);
2252
2253 return true;
2254 }
2255
2256 /*
2257 * lsm6dsm_signMotionFlush: send significant motion flush event
2258 */
lsm6dsm_signMotionFlush(void * cookie)2259 static bool lsm6dsm_signMotionFlush(void *cookie)
2260 {
2261 INFO_PRINT("signMotionFlush\n");
2262
2263 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), SENSOR_DATA_EVENT_FLUSH, NULL);
2264
2265 return true;
2266 }
2267
2268 /*
2269 * lsm6dsm_stepCounterSendLastData: send last number of steps
2270 */
lsm6dsm_stepCounterSendLastData(void * cookie,uint32_t tid)2271 static bool lsm6dsm_stepCounterSendLastData(void *cookie, uint32_t tid)
2272 {
2273 TDECL();
2274
2275 INFO_PRINT("stepCounterSendLastData: %lu steps\n", T(totalNumSteps));
2276
2277 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), &T(totalNumSteps), NULL);
2278
2279 return true;
2280 }
2281
2282 /*
2283 * lsm6dsm_sensorInit: initial sensor configuration
2284 */
lsm6dsm_sensorInit(void)2285 static void lsm6dsm_sensorInit(void)
2286 {
2287 TDECL();
2288 uint8_t buffer[4];
2289
2290 switch (T(initState)) {
2291 case RESET_LSM6DSM:
2292 INFO_PRINT("Performing soft-reset\n");
2293
2294 T(initState) = INIT_LSM6DSM;
2295
2296 /* Sensor SW-reset */
2297 SPI_WRITE(LSM6DSM_CTRL3_C_ADDR, LSM6DSM_SW_RESET, 20000);
2298
2299 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2300 break;
2301
2302 case INIT_LSM6DSM:
2303 INFO_PRINT("Initial registers configuration\n");
2304
2305 /* During init, reset all configurable registers to default values */
2306 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
2307 SPI_WRITE(LSM6DSM_DRDY_PULSE_CFG_ADDR, LSM6DSM_DRDY_PULSE_CFG_BASE);
2308
2309 buffer[0] = LSM6DSM_CTRL1_XL_BASE; /* LSM6DSM_CTRL1_XL */
2310 buffer[1] = LSM6DSM_CTRL2_G_BASE; /* LSM6DSM_CTRL2_G */
2311 buffer[2] = LSM6DSM_CTRL3_C_BASE; /* LSM6DSM_CTRL3_C */
2312 buffer[3] = LSM6DSM_CTRL4_C_BASE; /* LSM6DSM_CTRL4_C */
2313 SPI_MULTIWRITE(LSM6DSM_CTRL1_XL_ADDR, buffer, 4);
2314
2315 buffer[0] = LSM6DSM_CTRL10_C_BASE | LSM6DSM_RESET_PEDOMETER; /* LSM6DSM_CTRL10_C */
2316 buffer[1] = LSM6DSM_MASTER_CONFIG_BASE; /* LSM6DSM_MASTER_CONFIG */
2317 SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);
2318
2319 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, LSM6DSM_INT1_CTRL_BASE);
2320
2321 #ifdef LSM6DSM_I2C_MASTER_ENABLED
2322 T(initState) = INIT_I2C_MASTER_REGS_CONF;
2323 #else /* LSM6DSM_I2C_MASTER_ENABLED */
2324 INFO_PRINT("Initialization completed successfully!\n");
2325 T(initState) = INIT_DONE;
2326 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
2327
2328 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2329 break;
2330
2331 #ifdef LSM6DSM_I2C_MASTER_ENABLED
2332 case INIT_I2C_MASTER_REGS_CONF:
2333 INFO_PRINT("Initial I2C master registers configuration\n");
2334
2335 /* Enable access for embedded registers */
2336 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);
2337
2338 /* I2C-0 configuration */
2339 buffer[0] = LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP; /* LSM6DSM_EMBEDDED_SLV0_ADDR */
2340 buffer[1] = 0x00; /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */
2341 buffer[2] = LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE; /* LSM6DSM_EMBEDDED_SLV0_CONFIG */
2342 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 3);
2343
2344 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) /* Magn & Baro both enabled */
2345 /* I2C-1 configuration */
2346 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */
2347 buffer[1] = LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */
2348 buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV1_CONFIG */
2349 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3);
2350
2351 /* I2C-2 configuration */
2352 buffer[0] = (LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV2_ADDR */
2353 buffer[1] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV2_SUBADDR */
2354 buffer[2] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV2_CONFIG */
2355 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR, buffer, 3);
2356
2357 #ifdef LSM6DSM_I2C_MASTER_AK09916
2358 /* I2C-3 configuration */
2359 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV3_ADDR */
2360 buffer[1] = AK09916_STATUS_DATA_ADDR; /* LSM6DSM_EMBEDDED_SLV3_SUBADDR */
2361 buffer[2] = 1; /* LSM6DSM_EMBEDDED_SLV3_CONFIG */
2362 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR, buffer, 3);
2363 #endif /* LSM6DSM_I2C_MASTER_AK09916 */
2364 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
2365
2366 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) /* Magn only enabled */
2367 /* I2C-1 configuration */
2368 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */
2369 buffer[1] = LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */
2370 buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV1_CONFIG */
2371 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3);
2372
2373 #ifdef LSM6DSM_I2C_MASTER_AK09916
2374 /* I2C-2 configuration */
2375 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV2_ADDR */
2376 buffer[1] = AK09916_STATUS_DATA_ADDR; /* LSM6DSM_EMBEDDED_SLV2_SUBADDR */
2377 buffer[2] = 1; /* LSM6DSM_EMBEDDED_SLV2_CONFIG */
2378 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR, buffer, 3);
2379 #endif /* LSM6DSM_I2C_MASTER_AK09916 */
2380 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
2381
2382 #if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) /* Baro only enabled */
2383 /* I2C-1 configuration */
2384 buffer[0] = (LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */
2385 buffer[1] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */
2386 buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV1_CONFIG */
2387 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3);
2388 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */
2389
2390 /* Disable access for embedded registers */
2391 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
2392
2393 T(initState) = INIT_I2C_MASTER_SENSOR_RESET;
2394
2395 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2396 break;
2397
2398 case INIT_I2C_MASTER_SENSOR_RESET:
2399 INFO_PRINT("Performing soft-reset slave sensors\n");
2400 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
2401 T(initState) = INIT_I2C_MASTER_MAGN_SENSOR;
2402 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
2403 T(initState) = INIT_I2C_MASTER_BARO_SENSOR;
2404 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
2405
2406 /* Enable accelerometer and sensor-hub to initialize slave sensor */
2407 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE);
2408 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, LSM6DSM_CTRL10_C_BASE | LSM6DSM_ENABLE_DIGITAL_FUNC);
2409 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE | LSM6DSM_MASTER_CONFIG_MASTER_ON);
2410
2411 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
2412 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR, LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE, SENSOR_HZ(104.0f), MAGN, 20000);
2413 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
2414 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
2415 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR, LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE, SENSOR_HZ(104.0f), PRESS, 20000);
2416 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
2417
2418 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2419 break;
2420
2421 case INIT_I2C_MASTER_MAGN_SENSOR:
2422 INFO_PRINT("Initial slave magnetometer sensor registers configuration\n");
2423 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
2424 T(initState) = INIT_I2C_MASTER_BARO_SENSOR;
2425 #else /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
2426 T(initState) = INIT_I2C_MASTER_SENSOR_END;
2427 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
2428
2429 #ifdef LSM6DSM_I2C_MASTER_LIS3MDL
2430 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL1_ADDR, LIS3MDL_CTRL1_BASE, SENSOR_HZ(104.0f), MAGN);
2431 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL2_ADDR, LIS3MDL_CTRL2_BASE, SENSOR_HZ(104.0f), MAGN);
2432 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL3_ADDR, LIS3MDL_CTRL3_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN);
2433 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL4_ADDR, LIS3MDL_CTRL4_BASE, SENSOR_HZ(104.0f), MAGN);
2434 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL5_ADDR, LIS3MDL_CTRL5_BASE, SENSOR_HZ(104.0f), MAGN);
2435 #endif /* LSM6DSM_I2C_MASTER_LIS3MDL */
2436
2437 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR
2438 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_A_M_ADDR, LSM303AGR_CFG_REG_A_M_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN);
2439 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_C_M_ADDR, LSM303AGR_CFG_REG_C_M_BASE, SENSOR_HZ(104.0f), MAGN);
2440 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */
2441
2442 #ifdef LSM6DSM_I2C_MASTER_AK09916
2443 SPI_WRITE_SLAVE_SENSOR_REGISTER(AK09916_CNTL2_ADDR, AK09916_CNTL2_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN);
2444 #endif /* LSM6DSM_I2C_MASTER_AK09916 */
2445
2446 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2447 break;
2448
2449 case INIT_I2C_MASTER_BARO_SENSOR:
2450 INFO_PRINT("Initial slave barometer sensor registers configuration\n");
2451 T(initState) = INIT_I2C_MASTER_SENSOR_END;
2452
2453 #ifdef LSM6DSM_I2C_MASTER_LPS22HB
2454 SPI_WRITE_SLAVE_SENSOR_REGISTER(LPS22HB_CTRL1_ADDR, LPS22HB_CTRL1_BASE | LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE, SENSOR_HZ(104.0f), PRESS);
2455 SPI_WRITE_SLAVE_SENSOR_REGISTER(LPS22HB_CTRL2_ADDR, LPS22HB_CTRL2_BASE, SENSOR_HZ(104.0f), PRESS);
2456 #endif /* LSM6DSM_I2C_MASTER_LPS22HB */
2457
2458 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2459 break;
2460
2461 case INIT_I2C_MASTER_SENSOR_END:
2462 INFO_PRINT("Initialization completed successfully!\n");
2463 T(initState) = INIT_DONE;
2464
2465 /* Disable accelerometer and sensor-hub */
2466 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
2467 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, LSM6DSM_CTRL10_C_BASE);
2468 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
2469
2470 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2471 break;
2472 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
2473
2474 default:
2475 break;
2476 }
2477 }
2478
2479 /*
2480 * lsm6dsm_processPendingEvt: process pending events
2481 */
lsm6dsm_processPendingEvt(void)2482 static void lsm6dsm_processPendingEvt(void)
2483 {
2484 TDECL();
2485 enum SensorIndex i;
2486
2487 if (T(pendingInt[LSM6DSM_INT1_INDEX])) {
2488 T(pendingInt[LSM6DSM_INT1_INDEX]) = false;
2489 lsm6dsm_readStatusReg(false);
2490 return;
2491 }
2492
2493 for (i = ACCEL; i < NUM_SENSORS; i++) {
2494 if (T(pendingEnableConfig[i])) {
2495 T(pendingEnableConfig[i]) = false;
2496 LSM6DSMSensorOps[i].sensorPower(T(sensors[i]).pConfig.enable, (void *)i);
2497 return;
2498 }
2499
2500 if (T(pendingRateConfig[i])) {
2501 T(pendingRateConfig[i]) = false;
2502 LSM6DSMSensorOps[i].sensorSetRate(T(sensors[i]).pConfig.rate, T(sensors[i]).pConfig.latency, (void *)i);
2503 return;
2504 }
2505 }
2506 }
2507
2508 /*
2509 * lsm6dsm_processSensorThreeAxisData: elaborate three axis sensors data
2510 */
lsm6dsm_processSensorThreeAxisData(struct LSM6DSMSensor * mSensor,uint8_t * data)2511 static bool lsm6dsm_processSensorThreeAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data)
2512 {
2513 TDECL();
2514 float kScale = 1.0f, x, y, z;
2515
2516 switch (mSensor->idx) {
2517 case ACCEL:
2518 kScale = LSM6DSM_ACCEL_KSCALE;
2519 break;
2520 case GYRO:
2521 kScale = LSM6DSM_GYRO_KSCALE;
2522 break;
2523 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
2524 case MAGN:
2525 kScale = LSM6DSM_MAGN_KSCALE;
2526 break;
2527 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
2528 default:
2529 return false;
2530 }
2531
2532 x = ((int16_t)(data[1] << 8) | data[0]) * kScale;
2533 y = ((int16_t)(data[3] << 8) | data[2]) * kScale;
2534 z = ((int16_t)(data[5] << 8) | data[4]) * kScale;
2535
2536 mSensor->tADataEvt->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
2537 mSensor->tADataEvt->samples[0].firstSample.numSamples = 1;
2538
2539 switch (mSensor->idx) {
2540 case ACCEL:
2541 case GYRO:
2542 mSensor->tADataEvt->samples[0].x = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ROT_MATRIX);
2543 mSensor->tADataEvt->samples[0].y = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ROT_MATRIX);
2544 mSensor->tADataEvt->samples[0].z = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ROT_MATRIX);
2545 break;
2546
2547 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
2548 case MAGN:
2549 mSensor->tADataEvt->samples[0].x = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX);
2550 mSensor->tADataEvt->samples[0].y = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX);
2551 mSensor->tADataEvt->samples[0].z = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX);
2552 break;
2553 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
2554
2555 default:
2556 break;
2557 }
2558
2559 if (mSensor->samplesToDiscard == 0) {
2560 mSensor->samplesCounter++;
2561
2562 if (mSensor->samplesCounter >= mSensor->samplesDecimator) {
2563 switch (mSensor->idx) {
2564 case ACCEL:
2565 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
2566 accelCalRun(&T(accelCal), T(timestampInt[LSM6DSM_INT1_INDEX]),
2567 mSensor->tADataEvt->samples[0].x,
2568 mSensor->tADataEvt->samples[0].y,
2569 mSensor->tADataEvt->samples[0].z, T(currentTemperature));
2570
2571 accelCalBiasRemove(&T(accelCal),
2572 &mSensor->tADataEvt->samples[0].x,
2573 &mSensor->tADataEvt->samples[0].y,
2574 &mSensor->tADataEvt->samples[0].z);
2575 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
2576
2577 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
2578 if (T(sensors[GYRO].enabled)) {
2579 gyroCalUpdateAccel(&T(gyroCal), T(timestampInt[LSM6DSM_INT1_INDEX]),
2580 mSensor->tADataEvt->samples[0].x,
2581 mSensor->tADataEvt->samples[0].y,
2582 mSensor->tADataEvt->samples[0].z);
2583 }
2584 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
2585 break;
2586
2587 case GYRO:
2588 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
2589 gyroCalUpdateGyro(&T(gyroCal), T(timestampInt[LSM6DSM_INT1_INDEX]),
2590 mSensor->tADataEvt->samples[0].x,
2591 mSensor->tADataEvt->samples[0].y,
2592 mSensor->tADataEvt->samples[0].z, T(currentTemperature));
2593
2594 gyroCalRemoveBias(&T(gyroCal),
2595 mSensor->tADataEvt->samples[0].x,
2596 mSensor->tADataEvt->samples[0].y,
2597 mSensor->tADataEvt->samples[0].z,
2598 &mSensor->tADataEvt->samples[0].x,
2599 &mSensor->tADataEvt->samples[0].y,
2600 &mSensor->tADataEvt->samples[0].z);
2601 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
2602 break;
2603
2604 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
2605 case MAGN: ;
2606 float magnOffX, magnOffY, magnOffZ;
2607
2608 magCalRemoveSoftiron(&T(magnCal),
2609 mSensor->tADataEvt->samples[0].x,
2610 mSensor->tADataEvt->samples[0].y,
2611 mSensor->tADataEvt->samples[0].z,
2612 &magnOffX, &magnOffY, &magnOffZ);
2613
2614 T(newMagnCalibData) = magCalUpdate(&T(magnCal), NS_TO_US(T(timestampInt[LSM6DSM_INT1_INDEX])), magnOffX, magnOffY, magnOffZ);
2615
2616 magCalRemoveBias(&T(magnCal), magnOffX, magnOffY, magnOffZ,
2617 &mSensor->tADataEvt->samples[0].x,
2618 &mSensor->tADataEvt->samples[0].y,
2619 &mSensor->tADataEvt->samples[0].z);
2620 break;
2621 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
2622
2623 default:
2624 break;
2625 }
2626
2627 mSensor->samplesCounter = 0;
2628 return true;
2629 }
2630 } else
2631 mSensor->samplesToDiscard--;
2632
2633 return false;
2634 }
2635
2636 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
2637 /*
2638 * lsm6dsm_processSensorOneAxisData: elaborate single axis sensors data
2639 */
lsm6dsm_processSensorOneAxisData(struct LSM6DSMSensor * mSensor,uint8_t * data,void * store)2640 static bool lsm6dsm_processSensorOneAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, void *store)
2641 {
2642 TDECL();
2643 struct SingleAxisDataEvent *pressData = (struct SingleAxisDataEvent *)store;
2644 union EmbeddedDataPoint *tempData = (union EmbeddedDataPoint *)store;
2645
2646 if (mSensor->samplesToDiscard == 0) {
2647 mSensor->samplesCounter++;
2648
2649 if (mSensor->samplesCounter >= mSensor->samplesDecimator) {
2650 switch (mSensor->idx) {
2651 case PRESS:
2652 pressData->samples[0].fdata = ((data[2] << 16) | (data[1] << 8) | data[0]) * LSM6DSM_PRESS_KSCALE;
2653 pressData->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
2654 pressData->samples[0].firstSample.numSamples = 1;
2655 break;
2656 case TEMP:
2657 tempData->fdata = ((int16_t)(data[1] << 8) | data[0]) * LSM6DSM_TEMP_KSCALE;
2658 break;
2659 default:
2660 return false;
2661 }
2662
2663 mSensor->samplesCounter = 0;
2664 return true;
2665 }
2666 } else
2667 mSensor->samplesToDiscard--;
2668
2669 return false;
2670 }
2671 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
2672
2673 /*
2674 * lsm6dsm_handleSpiDoneEvt: all SPI operation fall back here
2675 */
lsm6dsm_handleSpiDoneEvt(const void * evtData)2676 static void lsm6dsm_handleSpiDoneEvt(const void *evtData)
2677 {
2678 TDECL();
2679 bool returnIdle = false, validData;
2680 struct LSM6DSMSensor *mSensor;
2681 int i;
2682
2683 switch (GET_STATE()) {
2684 case SENSOR_BOOT:
2685 SET_STATE(SENSOR_VERIFY_WAI);
2686
2687 SPI_READ(LSM6DSM_WAI_ADDR, 1, &T_SLAVE_INTERFACE(tmpDataBuffer));
2688 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2689 break;
2690
2691 case SENSOR_VERIFY_WAI:
2692 if (T_SLAVE_INTERFACE(tmpDataBuffer[1]) != LSM6DSM_WAI_VALUE) {
2693 T(mRetryLeft)--;
2694 if (T(mRetryLeft) == 0)
2695 break;
2696
2697 ERROR_PRINT("`Who-Am-I` register value not valid: %x\n", T_SLAVE_INTERFACE(tmpDataBuffer[1]));
2698 SET_STATE(SENSOR_BOOT);
2699 timTimerSet(100000000, 100, 100, lsm6dsm_timerCallback, NULL, true);
2700 } else {
2701 SET_STATE(SENSOR_INITIALIZATION);
2702 T(initState) = RESET_LSM6DSM;
2703 lsm6dsm_sensorInit();
2704 }
2705
2706 break;
2707
2708 case SENSOR_INITIALIZATION:
2709 if (T(initState) == INIT_DONE) {
2710 for (i = 0; i < NUM_SENSORS; i++)
2711 sensorRegisterInitComplete(T(sensors[i]).handle);
2712
2713 returnIdle = true;
2714 } else
2715 lsm6dsm_sensorInit();
2716
2717 break;
2718
2719 case SENSOR_POWERING_UP:
2720 mSensor = (struct LSM6DSMSensor *)evtData;
2721
2722 mSensor->enabled = true;
2723 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
2724 returnIdle = true;
2725 break;
2726
2727 case SENSOR_POWERING_DOWN:
2728 mSensor = (struct LSM6DSMSensor *)evtData;
2729
2730 mSensor->enabled = false;
2731 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
2732 returnIdle = true;
2733 break;
2734
2735 case SENSOR_CONFIG_CHANGING:
2736 mSensor = (struct LSM6DSMSensor *)evtData;
2737
2738 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency);
2739 returnIdle = true;
2740 break;
2741
2742 case SENSOR_INT1_STATUS_REG_HANDLING:
2743 if (T(sensors[STEP_DETECTOR].enabled)) {
2744 if (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_DETECTED) {
2745 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), NULL, NULL);
2746 DEBUG_PRINT("Step Detected!\n");
2747 }
2748 }
2749
2750 if (T(sensors[STEP_COUNTER].enabled)) {
2751 if (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA) {
2752 T(readSteps) = true;
2753 SPI_READ(LSM6DSM_STEP_COUNTER_L_ADDR, 2, &T_SLAVE_INTERFACE(stepCounterDataBuffer));
2754 }
2755 }
2756
2757 if (T(sensors[SIGN_MOTION].enabled)) {
2758 if (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_SIGN_MOTION) {
2759 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), NULL, NULL);
2760 DEBUG_PRINT("Significant Motion event!\n");
2761 }
2762 }
2763
2764 #ifdef LSM6DSM_I2C_MASTER_ENABLED
2765 if (T(masterConfigRegister) & LSM6DSM_MASTER_CONFIG_MASTER_ON) {
2766 T(statusRegisterSH) = T_SLAVE_INTERFACE(statusRegBuffer[1]) & LSM6DSM_FUNC_SRC_SENSOR_HUB_END_OP;
2767 if (T(statusRegisterSH))
2768 SPI_READ(LSM6DSM_SENSORHUB1_REG_ADDR, LSM6DSM_SH_READ_BYTE_NUM, &T_SLAVE_INTERFACE(SHDataBuffer));
2769 }
2770 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
2771
2772 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
2773 T(statusRegisterTDA) = T_SLAVE_INTERFACE(statusRegBuffer[1]) & LSM6DSM_STATUS_REG_TDA;
2774 if (T(statusRegisterTDA))
2775 SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer));
2776 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
2777
2778 T(statusRegisterDA) = T_SLAVE_INTERFACE(statusRegBuffer[1]) & (LSM6DSM_STATUS_REG_XLDA | LSM6DSM_STATUS_REG_GDA);
2779
2780 switch(T(statusRegisterDA)) {
2781 case LSM6DSM_STATUS_REG_XLDA:
2782 SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(accelDataBuffer));
2783 break;
2784
2785 case LSM6DSM_STATUS_REG_GDA:
2786 SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(gyroDataBuffer));
2787 break;
2788
2789 case (LSM6DSM_STATUS_REG_XLDA | LSM6DSM_STATUS_REG_GDA):
2790 SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(accelDataBuffer));
2791 SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(gyroDataBuffer));
2792 break;
2793
2794 default:
2795 if (!T(readSteps)) {
2796 SET_STATE(SENSOR_IDLE);
2797 lsm6dsm_processPendingEvt();
2798 return;
2799 }
2800 break;
2801 }
2802
2803 SET_STATE(SENSOR_INT1_OUTPUT_DATA_HANDLING);
2804 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
2805 break;
2806
2807 case SENSOR_INT1_OUTPUT_DATA_HANDLING:
2808 if (T(readSteps)) {
2809 union EmbeddedDataPoint step_cnt;
2810
2811 step_cnt.idata = T_SLAVE_INTERFACE(stepCounterDataBuffer[1]) | (T_SLAVE_INTERFACE(stepCounterDataBuffer[2]) << 8);
2812 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), step_cnt.vptr, NULL);
2813 DEBUG_PRINT("Step Counter update: %ld steps\n", step_cnt.idata);
2814 T(totalNumSteps) = step_cnt.idata;
2815 T(readSteps) = false;
2816 }
2817
2818 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
2819 if (T(statusRegisterTDA)) {
2820 T(currentTemperature) = LSM6DSM_TEMP_OFFSET +
2821 (float)((int16_t)((T_SLAVE_INTERFACE(tempDataBuffer[2]) << 8) | T_SLAVE_INTERFACE(tempDataBuffer[1]))) / 256.0f;
2822 }
2823 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
2824
2825 #ifdef LSM6DSM_I2C_MASTER_ENABLED
2826 if (T(statusRegisterSH)) {
2827 T(statusRegisterSH) = 0;
2828 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
2829 if (T(sensors[MAGN].enabled)) {
2830 validData = lsm6dsm_processSensorThreeAxisData(&T(sensors[MAGN]), &T_SLAVE_INTERFACE(SHDataBuffer[1]));
2831 if (validData) {
2832 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), T(sensors[MAGN]).tADataEvt, NULL);
2833
2834 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
2835 if T(newMagnCalibData) {
2836 magCalGetBias(&T(magnCal), &T(magnCalDataEvt)->samples[0].x,
2837 &T(magnCalDataEvt)->samples[0].y, &T(magnCalDataEvt)->samples[0].z);
2838
2839 T(newMagnCalibData) = false;
2840 T(magnCalDataEvt)->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
2841 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG_BIAS), T(magnCalDataEvt), NULL);
2842 }
2843 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
2844 }
2845 }
2846 #endif /* LSM6DSM_MAGN_CALIB_MAGNETOMETER_ENABLED */
2847
2848 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
2849 if (T(sensors[PRESS].enabled)) {
2850 validData = lsm6dsm_processSensorOneAxisData(&T(sensors[PRESS]),
2851 &T_SLAVE_INTERFACE(SHDataBuffer[LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN + 1]), T(sensors[PRESS]).sADataEvt);
2852 if (validData)
2853 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), T(sensors[PRESS]).sADataEvt, NULL);
2854 }
2855
2856 if (T(sensors[TEMP].enabled)) {
2857 union EmbeddedDataPoint tempData;
2858
2859 validData = lsm6dsm_processSensorOneAxisData(&T(sensors[TEMP]),
2860 &T_SLAVE_INTERFACE(SHDataBuffer[LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN + LSM6DSM_PRESS_OUTDATA_LEN + 1]), &tempData);
2861 if (validData)
2862 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL);
2863 }
2864 #endif /* LSM6DSM_MAGN_CALIB_BAROMETER_ENABLED */
2865 }
2866 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
2867
2868 if (T(statusRegisterDA) & LSM6DSM_STATUS_REG_XLDA) {
2869 validData = lsm6dsm_processSensorThreeAxisData(&T(sensors[ACCEL]), &T_SLAVE_INTERFACE(accelDataBuffer[1]));
2870 if (validData) {
2871 if (T(sensors[ACCEL].enabled)) {
2872 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), T(sensors[ACCEL]).tADataEvt, NULL);
2873 } else {
2874 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
2875 if (accelCalUpdateBias(&T(accelCal), &T(accelBiasDataEvt)->samples[0].x,
2876 &T(accelBiasDataEvt)->samples[0].y, &T(accelBiasDataEvt)->samples[0].z)) {
2877 T(accelBiasDataEvt)->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
2878 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL_BIAS), T(accelBiasDataEvt), NULL);
2879 }
2880 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
2881 }
2882 }
2883 }
2884
2885 if (T(statusRegisterDA) & LSM6DSM_STATUS_REG_GDA) {
2886 validData = lsm6dsm_processSensorThreeAxisData(&T(sensors[GYRO]), &T_SLAVE_INTERFACE(gyroDataBuffer[1]));
2887 if (validData) {
2888 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO), T(sensors[GYRO]).tADataEvt, NULL);
2889
2890 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
2891 if (gyroCalNewBiasAvailable(&T(gyroCal))) {
2892 gyroCalGetBias(&T(gyroCal), &T(gyroBiasDataEvt)->samples[0].x,
2893 &T(gyroBiasDataEvt)->samples[0].y, &T(gyroBiasDataEvt)->samples[0].z);
2894
2895 T(gyroBiasDataEvt)->referenceTime = T(timestampInt[LSM6DSM_INT1_INDEX]);
2896 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO_BIAS), T(gyroBiasDataEvt), NULL);
2897 }
2898 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
2899 }
2900 }
2901
2902 returnIdle = true;
2903 break;
2904
2905 default:
2906 break;
2907 }
2908
2909 if (returnIdle) {
2910 SET_STATE(SENSOR_IDLE);
2911 lsm6dsm_processPendingEvt();
2912 }
2913 }
2914
2915 /*
2916 * lsm6dsm_handleEvent: handle driver events
2917 */
lsm6dsm_handleEvent(uint32_t evtType,const void * evtData)2918 static void lsm6dsm_handleEvent(uint32_t evtType, const void *evtData)
2919 {
2920 TDECL();
2921 uint64_t currTime;
2922
2923 switch (evtType) {
2924 case EVT_APP_START:
2925 T(mRetryLeft) = LSM6DSM_RETRY_CNT_WAI;
2926 SET_STATE(SENSOR_BOOT);
2927 osEventUnsubscribe(T(tid), EVT_APP_START);
2928
2929 /* Sensor need 100ms to boot, use a timer callback to continue */
2930 currTime = timGetTime();
2931 if (currTime < 100000000ULL) {
2932 timTimerSet(100000000 - currTime, 100, 100, lsm6dsm_timerCallback, NULL, true);
2933 break;
2934 }
2935
2936 /* If 100ms already passed, just fall through next step */
2937 case EVT_SPI_DONE:
2938 lsm6dsm_handleSpiDoneEvt(evtData);
2939 break;
2940
2941 case EVT_SENSOR_INTERRUPT_1:
2942 lsm6dsm_readStatusReg(false);
2943 break;
2944
2945 case EVT_APP_FROM_HOST:
2946 break;
2947
2948 default:
2949 break;
2950 }
2951 }
2952
2953 /*
2954 * lsm6dsm_initSensorStruct: initialize sensor struct variable
2955 */
lsm6dsm_initSensorStruct(struct LSM6DSMSensor * sensor,enum SensorIndex idx)2956 static void lsm6dsm_initSensorStruct(struct LSM6DSMSensor *sensor, enum SensorIndex idx)
2957 {
2958 sensor->idx = idx;
2959 sensor->rate = 0;
2960 sensor->hwRate = 0;
2961 sensor->latency = 0;
2962 sensor->enabled = false;
2963 sensor->samplesToDiscard = 0;
2964 sensor->samplesDecimator = 1;
2965 sensor->samplesCounter = 0;
2966 sensor->tADataEvt = NULL;
2967 sensor->sADataEvt = NULL;
2968 }
2969
2970 /*
2971 * lsm6dsm_calculateSlabNumItems: calculate number of items needed to allocate memory
2972 */
lsm6dsm_calculateSlabNumItems()2973 static uint8_t lsm6dsm_calculateSlabNumItems()
2974 {
2975 uint8_t slabElements = 2;
2976
2977 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
2978 slabElements++;
2979 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
2980 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
2981 slabElements++;
2982 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
2983 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
2984 slabElements++;
2985 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
2986 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
2987 slabElements++;
2988 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
2989
2990 return slabElements;
2991 }
2992
2993 /*
2994 * lsm6dsm_startTask: first function executed when App start
2995 */
lsm6dsm_startTask(uint32_t task_id)2996 static bool lsm6dsm_startTask(uint32_t task_id)
2997 {
2998 TDECL();
2999 enum SensorIndex i;
3000 int err;
3001
3002 DEBUG_PRINT("IMU: %lu\n", task_id);
3003
3004 T(tid) = task_id;
3005 T(int1) = gpioRequest(LSM6DSM_INT1_GPIO);
3006 T(isr1).func = lsm6dsm_isr1;
3007
3008 T_SLAVE_INTERFACE(mode).speed = LSM6DSM_SPI_SLAVE_FREQUENCY_HZ;
3009 T_SLAVE_INTERFACE(mode).bitsPerWord = 8;
3010 T_SLAVE_INTERFACE(mode).cpol = SPI_CPOL_IDLE_HI;
3011 T_SLAVE_INTERFACE(mode).cpha = SPI_CPHA_TRAILING_EDGE;
3012 T_SLAVE_INTERFACE(mode).nssChange = true;
3013 T_SLAVE_INTERFACE(mode).format = SPI_FORMAT_MSB_FIRST;
3014 T_SLAVE_INTERFACE(cs) = LSM6DSM_SPI_SLAVE_CS_GPIO;
3015
3016 DEBUG_PRINT("Requested SPI on bus #%d @ %dHz, int1 on gpio#%d\n",
3017 LSM6DSM_SPI_SLAVE_BUS_ID, LSM6DSM_SPI_SLAVE_FREQUENCY_HZ, LSM6DSM_INT1_GPIO);
3018
3019 err = spiMasterRequest(LSM6DSM_SPI_SLAVE_BUS_ID, &T_SLAVE_INTERFACE(spiDev));
3020 if (err < 0) {
3021 ERROR_PRINT("Failed to request SPI on this bus: #%d\n", LSM6DSM_SPI_SLAVE_BUS_ID);
3022 return false;
3023 }
3024
3025 T(int1Register) = LSM6DSM_INT1_CTRL_BASE;
3026 T(int2Register) = LSM6DSM_INT2_CTRL_BASE;
3027 T(embeddedFunctionsRegister) = LSM6DSM_CTRL10_C_BASE;
3028 T(accelSensorDependencies) = 0;
3029 T(embeddedFunctionsDependencies) = 0;
3030 T(pendingInt[LSM6DSM_INT1_INDEX]) = false;
3031 T(pendingInt[LSM6DSM_INT2_INDEX]) = false;
3032 T(timestampInt[LSM6DSM_INT1_INDEX]) = 0;
3033 T(timestampInt[LSM6DSM_INT2_INDEX]) = 0;
3034 T(totalNumSteps) = 0;
3035 T(triggerRate) = SENSOR_HZ_RATE_TO_US(SENSOR_HZ(26.0f / 2.0f));
3036 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
3037 T(currentTemperature) = 0;
3038 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
3039 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
3040 T(newMagnCalibData) = false;
3041 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
3042 #ifdef LSM6DSM_I2C_MASTER_ENABLED
3043 T(masterConfigRegister) = LSM6DSM_MASTER_CONFIG_BASE;
3044 #endif /* LSM6DSM_I2C_MASTER_ENABLED */
3045
3046 T(mDataSlabThreeAxis) = slabAllocatorNew(sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint), 4, lsm6dsm_calculateSlabNumItems());
3047 if (!T(mDataSlabThreeAxis)) {
3048 ERROR_PRINT("Failed to allocate mDataSlabThreeAxis memory\n");
3049 spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
3050 return false;
3051 }
3052
3053 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
3054 T(mDataSlabOneAxis) = slabAllocatorNew(sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint), 4, 10);
3055 if (!T(mDataSlabOneAxis)) {
3056 ERROR_PRINT("Failed to allocate mDataSlabOneAxis memory\n");
3057 slabAllocatorDestroy(T(mDataSlabThreeAxis));
3058 spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
3059 return false;
3060 }
3061 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
3062
3063 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
3064 T(accelBiasDataEvt) = slabAllocatorAlloc(T(mDataSlabThreeAxis));
3065 if (!T(accelBiasDataEvt)) {
3066 ERROR_PRINT("Failed to allocate accelBiasDataEvt memory");
3067 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
3068 slabAllocatorDestroy(T(mDataSlabOneAxis));
3069 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
3070 slabAllocatorDestroy(T(mDataSlabThreeAxis));
3071 spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
3072 return false;
3073 }
3074
3075 memset(&T(accelBiasDataEvt)->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
3076 T(accelBiasDataEvt)->samples[0].firstSample.biasCurrent = true;
3077 T(accelBiasDataEvt)->samples[0].firstSample.biasPresent = 1;
3078 T(accelBiasDataEvt)->samples[0].firstSample.biasSample = 0;
3079 T(accelBiasDataEvt)->samples[0].firstSample.numSamples = 1;
3080 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
3081
3082 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
3083 T(gyroBiasDataEvt) = slabAllocatorAlloc(T(mDataSlabThreeAxis));
3084 if (!T(gyroBiasDataEvt)) {
3085 ERROR_PRINT("Failed to allocate gyroBiasDataEvt memory");
3086 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
3087 slabAllocatorDestroy(T(mDataSlabOneAxis));
3088 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
3089 slabAllocatorDestroy(T(mDataSlabThreeAxis));
3090 spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
3091 return false;
3092 }
3093
3094 memset(&T(gyroBiasDataEvt)->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
3095 T(gyroBiasDataEvt)->samples[0].firstSample.biasCurrent = true;
3096 T(gyroBiasDataEvt)->samples[0].firstSample.biasPresent = 1;
3097 T(gyroBiasDataEvt)->samples[0].firstSample.biasSample = 0;
3098 T(gyroBiasDataEvt)->samples[0].firstSample.numSamples = 1;
3099 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
3100
3101 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
3102 T(magnCalDataEvt) = slabAllocatorAlloc(T(mDataSlabThreeAxis));
3103 if (!T(magnCalDataEvt)) {
3104 ERROR_PRINT("Failed to allocate magnCalDataEvt memory");
3105 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
3106 slabAllocatorDestroy(T(mDataSlabOneAxis));
3107 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
3108 slabAllocatorDestroy(T(mDataSlabThreeAxis));
3109 spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
3110 return false;
3111 }
3112
3113 memset(&T(magnCalDataEvt)->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
3114 T(magnCalDataEvt)->samples[0].firstSample.biasCurrent = true;
3115 T(magnCalDataEvt)->samples[0].firstSample.biasPresent = 1;
3116 T(magnCalDataEvt)->samples[0].firstSample.biasSample = 0;
3117 T(magnCalDataEvt)->samples[0].firstSample.numSamples = 1;
3118 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
3119
3120 for (i = 0; i < NUM_SENSORS; i++) {
3121 T(pendingEnableConfig[i]) = false;
3122 T(pendingRateConfig[i]) = false;
3123 lsm6dsm_initSensorStruct(&T(sensors[i]), i);
3124
3125 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
3126 if ((i == ACCEL) || (i == GYRO) || (i == MAGN)) {
3127 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
3128 if ((i == ACCEL) || (i == GYRO)) {
3129 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
3130 T(sensors[i]).tADataEvt = slabAllocatorAlloc(T(mDataSlabThreeAxis));
3131 if (!T(sensors[i]).tADataEvt) {
3132 ERROR_PRINT("Failed to allocate tADataEvt memory");
3133 goto unregister_sensors;
3134 }
3135
3136 memset(&T(sensors[i]).tADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
3137 }
3138 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
3139 if (i == PRESS) {
3140 T(sensors[i]).sADataEvt = slabAllocatorAlloc(T(mDataSlabOneAxis));
3141 if (!T(sensors[i]).sADataEvt) {
3142 ERROR_PRINT("Failed to allocate sADataEvt memory");
3143 goto unregister_sensors;
3144 }
3145
3146 memset(&T(sensors[i]).sADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
3147 }
3148 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
3149
3150 T(sensors[i]).handle = sensorRegister(&LSM6DSMSensorInfo[i], &LSM6DSMSensorOps[i], NULL, false);
3151 }
3152
3153 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
3154 accelCalInit(&T(accelCal),
3155 800000000, /* stillness period = 0.8 seconds [ns] */
3156 5, /* minimum sample number */
3157 0.00025, /* threshold */
3158 15, /* nx bucket count */
3159 15, /* nxb bucket count */
3160 15, /* ny bucket count */
3161 15, /* nyb bucket count */
3162 15, /* nz bucket count */
3163 15, /* nzb bucket count */
3164 15); /* nle bucket count */
3165 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
3166
3167 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
3168 gyroCalInit(&T(gyroCal),
3169 5e9, /* min stillness period = 5 seconds [ns] */
3170 6e9, /* max stillness period = 6 seconds [ns] */
3171 0, 0, 0, /* initial bias offset calibration values [rad/sec] */
3172 0, /* timestamp of initial bias calibration [ns] */
3173 1.5e9, /* analysis window length = 1.5 seconds [ns] */
3174 5e-5f, /* gyroscope variance threshold [rad/sec]^2 */
3175 1e-5f, /* gyroscope confidence delta [rad/sec]^2 */
3176 8e-3f, /* accelerometer variance threshold [m/sec^2]^2 */
3177 1.6e-3f, /* accelerometer confidence delta [m/sec^2]^2 */
3178 1.4f, /* magnetometer variance threshold [uT]^2 */
3179 0.25, /* magnetometer confidence delta [uT]^2 */
3180 0.95f, /* stillness threshold [0, 1] */
3181 1); /* 1 : gyro calibrations will be applied */
3182 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
3183
3184 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
3185 initMagCal(&T(magnCal),
3186 0.0f, 0.0f, 0.0f, /* magn offset x - y - z */
3187 1.0f, 0.0f, 0.0f, /* magn scale matrix c00 - c01 - c02 */
3188 0.0f, 1.0f, 0.0f, /* magn scale matrix c10 - c11 - c12 */
3189 0.0f, 0.0f, 1.0f); /* magn scale matrix c20 - c21 - c22 */
3190 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
3191
3192 /* Initialize index used to fill/get data from buffer */
3193 T_SLAVE_INTERFACE(mWbufCnt) = 0;
3194 T_SLAVE_INTERFACE(mRegCnt) = 0;
3195
3196 osEventSubscribe(T(tid), EVT_APP_START);
3197
3198 DEBUG_PRINT("Enabling gpio#%d connected to int1\n", LSM6DSM_INT1_GPIO);
3199 lsm6dsm_enableInterrupt(T(int1), &T(isr1));
3200
3201 return true;
3202
3203 unregister_sensors:
3204 for (i--; i >= ACCEL; i--)
3205 sensorUnregister(T(sensors[i]).handle);
3206
3207 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
3208 accelCalDestroy(&T(accelCal));
3209 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
3210 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
3211 destroy_mag_cal(&T(magnCal));
3212 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
3213 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
3214 gyroCalDestroy(&T(gyroCal));
3215 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
3216 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
3217 slabAllocatorDestroy(T(mDataSlabOneAxis));
3218 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
3219 slabAllocatorDestroy(T(mDataSlabThreeAxis));
3220 spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
3221 return false;
3222 }
3223
3224 /*
3225 * lsm6dsm_endTask: last function executed when App end
3226 */
3227 static void lsm6dsm_endTask(void)
3228 {
3229 TDECL();
3230 enum SensorIndex i;
3231
3232 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED
3233 accelCalDestroy(&T(accelCal));
3234 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
3235 #ifdef LSM6DSM_MAGN_CALIB_ENABLED
3236 destroy_mag_cal(&T(magnCal));
3237 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */
3238 #ifdef LSM6DSM_GYRO_CALIB_ENABLED
3239 gyroCalDestroy(&T(gyroCal));
3240 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */
3241
3242 lsm6dsm_disableInterrupt(T(int1), &T(isr1));
3243 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
3244 slabAllocatorDestroy(T(mDataSlabOneAxis));
3245 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
3246 slabAllocatorDestroy(T(mDataSlabThreeAxis));
3247 spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
3248
3249 for (i = 0; i < NUM_SENSORS; i++)
3250 sensorUnregister(T(sensors[i]).handle);
3251
3252 gpioRelease(T(int1));
3253 }
3254
3255 INTERNAL_APP_INIT(LSM6DSM_APP_ID, 0, lsm6dsm_startTask, lsm6dsm_endTask, lsm6dsm_handleEvent);
3256