1 /* 2 * Copyright (C) 2016-2017 STMicroelectronics 3 * 4 * Author: Denis Ciocca <denis.ciocca (at) 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 <heap.h> 24 #include <halIntf.h> 25 #include <spi.h> 26 #include <gpio.h> 27 #include <atomic.h> 28 #include <timer.h> 29 #include <printf.h> 30 #include <isr.h> 31 #include <hostIntf.h> 32 #include <nanohubPacket.h> 33 #include <cpu/cpuMath.h> 34 #include <variant/sensType.h> 35 #include <plat/gpio.h> 36 #include <plat/syscfg.h> 37 #include <plat/exti.h> 38 #include <plat/rtc.h> 39 #include <calibration/accelerometer/accel_cal.h> 40 #include <calibration/gyroscope/gyro_cal.h> 41 #include <calibration/magnetometer/mag_cal.h> 42 #include <calibration/over_temp/over_temp_cal.h> 43 #include <algos/time_sync.h> 44 45 #include "st_lsm6dsm_lis3mdl_slave.h" 46 #include "st_lsm6dsm_lsm303agr_slave.h" 47 #include "st_lsm6dsm_ak09916_slave.h" 48 #include "st_lsm6dsm_lps22hb_slave.h" 49 50 #define LSM6DSM_APP_VERSION 1 51 52 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) || defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 53 #define LSM6DSM_I2C_MASTER_ENABLED 1 54 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 55 56 #if defined(LSM6DSM_MAGN_CALIB_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 57 #pragma message("LSM6DSM_MAGN_CALIB_ENABLED can not be used if no magnetometer sensors are enabled on I2C master. Disabling it!") 58 #undef LSM6DSM_MAGN_CALIB_ENABLED 59 #endif /* LSM6DSM_MAGN_CALIB_ENABLED, LSM6DSM_I2C_MASTER_ENABLED */ 60 61 #if defined(LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP) && !defined(LSM6DSM_I2C_MASTER_ENABLED) 62 #pragma message("LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP has no meaning if no sensors are enabled on I2C master. Discarding it!") 63 #endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP, LSM6DSM_I2C_MASTER_ENABLED */ 64 65 #if defined(LSM6DSM_OVERTEMP_CALIB_ENABLED) && !defined(LSM6DSM_GYRO_CALIB_ENABLED) 66 #pragma message("LSM6DSM_OVERTEMP_CALIB_ENABLED has no meaning if gyro calibration is not enabled. Discarding it!") 67 #undef LSM6DSM_OVERTEMP_CALIB_ENABLED 68 #endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED, LSM6DSM_GYRO_CALIB_ENABLED */ 69 70 #if !defined(LSM6DSM_SPI_SLAVE_BUS_ID) || !defined(LSM6DSM_SPI_SLAVE_FREQUENCY_HZ) || !defined(LSM6DSM_SPI_SLAVE_CS_GPIO) 71 #error "SPI macros not fully defined. Please check README file" 72 #endif /* LSM6DSM_SPI_SLAVE_BUS_ID, LSM6DSM_SPI_SLAVE_FREQUENCY_HZ, LSM6DSM_SPI_SLAVE_CS_GPIO */ 73 74 #if !defined(LSM6DSM_INT_IRQ) || !defined(LSM6DSM_INT1_GPIO) 75 #error "Interrupts macros not fully defined. Please check README file" 76 #endif /* LSM6DSM_INT_IRQ, LSM6DSM_INT1_GPIO */ 77 78 #if !defined(LSM6DSM_ACCEL_GYRO_ROT_MATRIX) 79 #error "Accel/gyro rotation matrix macro not defined. Please check README file" 80 #endif /* LSM6DSM_ACCEL_GYRO_ROT_MATRIX */ 81 82 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 83 #if !defined(LSM6DSM_MAGN_ROT_MATRIX) 84 #error "Magn rotation matrix macro not defined. Please check README file" 85 #endif /* LSM6DSM_MAGN_ROT_MATRIX */ 86 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 87 88 #define LSM6DSM_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 0) 89 90 #define LSM6DSM_WAI_VALUE (0x6a) 91 #define LSM6DSM_RETRY_CNT_WAI 5 /* Retry #n times if WAI value is wrong. Maybe HW is not ready after power on */ 92 #define LSM6DSM_ACCEL_KSCALE 0.00239364f /* Accel scale @8g in (m/s^2)/LSB */ 93 #define LSM6DSM_GYRO_KSCALE 0.00122173f /* Gyro scale @2000dps in (rad/sec)/LSB */ 94 #define LSM6DSM_ONE_SAMPLE_BYTE 6 /* One sample of triaxial sensor is expressed on 6 byte */ 95 #define LSM6DSM_TEMP_SAMPLE_BYTE 2 /* One sample of temperature sensor is expressed on 2 byte */ 96 #define LSM6DSM_TIMESTAMP_SAMPLE_BYTE 3 /* One sample of timestamp is expressed on 3 byte */ 97 #define LSM6DSM_TEMP_OFFSET (25.0f) 98 #define LSM6DSM_SC_DELTA_TIME_PERIOD_SEC (1.6384f) /* Step counter deltatime resolution */ 99 #define LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE 15 100 #define LSM6DSM_MAX_WATERMARK_VALUE 600 /* 4096byte = 682 samples, use 600 to avoid overflow */ 101 #define LSM6DSM_TIME_RESOLUTION 25000UL /* 25us [ns] */ 102 #define LSM6DSM_MASK_24BIT_TIMESTAMP 0x00ffffff /* mask to select 24bit data from 32bit storage data type */ 103 #define LSM6DSM_TIMEDIFF_OVERFLOW_LSB 8388608LL /* If deltatime is bigger than 2^23 it means timer is overflowed */ 104 #define LSM6DSM_SYNC_DELTA_INTERVAL 100000000ULL /* Sensor timestamp is synced with MCU every #n deltatime [ns] */ 105 #define LSM6DSM_TRIAXIAL_NUM_AXIS 3 106 107 /* SPI buffers */ 108 #define LSM6DSM_SPI_PACKET_SIZE 75 109 #define LSM6DSM_SPI_FIFO_SIZE 1024 110 #define LSM6DSM_BUF_MARGIN 100 111 #define SPI_BUF_SIZE (LSM6DSM_SPI_FIFO_SIZE + LSM6DSM_BUF_MARGIN) 112 113 /* LSM6DSM status check registers */ 114 #define LSM6DSM_FUNC_SRC_STEP_DETECTED (0x10) 115 #define LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA (0x80) 116 #define LSM6DSM_FUNC_SRC_SIGN_MOTION (0x40) 117 #define LSM6DSM_FIFO_STATUS2_FIFO_EMPTY (0x10) 118 #define LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART (0x20) 119 #define LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN (0x40) 120 #define LSM6DSM_FIFO_STATUS2_FIFO_ERROR (LSM6DSM_FIFO_STATUS2_FIFO_EMPTY | \ 121 LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART | \ 122 LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN) 123 124 /* LSM6DSM ODR related */ 125 #define LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON 80000 126 #define LSM6DSM_ODR_12HZ_ACCEL_STD 1 127 #define LSM6DSM_ODR_26HZ_ACCEL_STD 1 128 #define LSM6DSM_ODR_52HZ_ACCEL_STD 1 129 #define LSM6DSM_ODR_104HZ_ACCEL_STD 1 130 #define LSM6DSM_ODR_208HZ_ACCEL_STD 1 131 #define LSM6DSM_ODR_416HZ_ACCEL_STD 1 132 #define LSM6DSM_ODR_12HZ_GYRO_STD 2 133 #define LSM6DSM_ODR_26HZ_GYRO_STD 3 134 #define LSM6DSM_ODR_52HZ_GYRO_STD 3 135 #define LSM6DSM_ODR_104HZ_GYRO_STD 3 136 #define LSM6DSM_ODR_208HZ_GYRO_STD 3 137 #define LSM6DSM_ODR_416HZ_GYRO_STD 3 138 139 #define LSM6DSM_ODR_12HZ_REG_VALUE (0x10) 140 #define LSM6DSM_ODR_26HZ_REG_VALUE (0x20) 141 #define LSM6DSM_ODR_52HZ_REG_VALUE (0x30) 142 #define LSM6DSM_ODR_104HZ_REG_VALUE (0x40) 143 #define LSM6DSM_ODR_208HZ_REG_VALUE (0x50) 144 #define LSM6DSM_ODR_416HZ_REG_VALUE (0x60) 145 146 #define LSM6DSM_INT_FIFO_FTH_ENABLE_REG_VALUE (0x08) 147 #define LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE (0x80) 148 #define LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE (0x80) 149 #define LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE (0x40) 150 151 /* LSM6DSM registers */ 152 #define LSM6DSM_FUNC_CFG_ACCESS_ADDR (0x01) 153 #define LSM6DSM_FIFO_CTRL1_ADDR (0x06) 154 #define LSM6DSM_FIFO_CTRL5_ADDR (0x0a) 155 #define LSM6DSM_DRDY_PULSE_CFG_ADDR (0x0b) 156 #define LSM6DSM_INT1_CTRL_ADDR (0x0d) 157 #define LSM6DSM_INT2_CTRL_ADDR (0x0e) 158 #define LSM6DSM_WAI_ADDR (0x0f) 159 #define LSM6DSM_CTRL1_XL_ADDR (0x10) 160 #define LSM6DSM_CTRL2_G_ADDR (0x11) 161 #define LSM6DSM_CTRL3_C_ADDR (0x12) 162 #define LSM6DSM_CTRL4_C_ADDR (0x13) 163 #define LSM6DSM_CTRL5_C_ADDR (0x14) 164 #define LSM6DSM_EBD_STEP_COUNT_DELTA_ADDR (0x15) 165 #define LSM6DSM_CTRL10_C_ADDR (0x19) 166 #define LSM6DSM_MASTER_CONFIG_ADDR (0x1a) 167 #define LSM6DSM_STATUS_REG_ADDR (0x1e) 168 #define LSM6DSM_OUT_TEMP_L_ADDR (0x20) 169 #define LSM6DSM_OUTX_L_G_ADDR (0x22) 170 #define LSM6DSM_OUTX_L_XL_ADDR (0x28) 171 #define LSM6DSM_OUT_SENSORHUB1_ADDR (0x2e) 172 #define LSM6DSM_FIFO_STATUS1_ADDR (0x3a) 173 #define LSM6DSM_FIFO_DATA_OUT_L_ADDR (0x3e) 174 #define LSM6DSM_TIMESTAMP0_REG_ADDR (0x40) 175 #define LSM6DSM_TIMESTAMP2_REG_ADDR (0x42) 176 #define LSM6DSM_STEP_COUNTER_L_ADDR (0x4b) 177 #define LSM6DSM_FUNC_SRC_ADDR (0x53) 178 #define LSM6DSM_WAKE_UP_DUR_ADDR (0x5c) 179 #define LSM6DSM_X_OFS_USR_ADDR (0x73) 180 181 #define LSM6DSM_SW_RESET (0x01) 182 #define LSM6DSM_RESET_PEDOMETER (0x02) 183 #define LSM6DSM_ENABLE_FUNC_CFG_ACCESS (0x80) 184 #define LSM6DSM_ENABLE_DIGITAL_FUNC (0x04) 185 #define LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC (0x10) 186 #define LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC (0x01) 187 #define LSM6DSM_MASTER_CONFIG_PULL_UP_EN (0x08) 188 #define LSM6DSM_MASTER_CONFIG_MASTER_ON (0x01) 189 #define LSM6DSM_ENABLE_FIFO_TIMESTAMP (0x80) 190 #define LSM6DSM_TIMESTAMP2_REG_RESET_TIMESTAMP (0xaa) 191 192 /* LSM6DSM fifo modes */ 193 #define LSM6DSM_FIFO_BYPASS_MODE (0x00) 194 #define LSM6DSM_FIFO_CONTINUOS_MODE (0x36) 195 #define LSM6DSM_FIFO_CTRL2_FTH_MASK (0x07) 196 197 /* LSM6DSM fifo decimators */ 198 #define LSM6DSM_FIFO_SAMPLE_NOT_IN_FIFO (0x00) 199 #define LSM6DSM_FIFO_NO_DECIMATION (0x01) 200 #define LSM6DSM_FIFO_DECIMATION_FACTOR_2 (0x02) 201 #define LSM6DSM_FIFO_DECIMATION_FACTOR_3 (0x03) 202 #define LSM6DSM_FIFO_DECIMATION_FACTOR_4 (0x04) 203 #define LSM6DSM_FIFO_DECIMATION_FACTOR_8 (0x05) 204 #define LSM6DSM_FIFO_DECIMATION_FACTOR_16 (0x06) 205 #define LSM6DSM_FIFO_DECIMATION_FACTOR_32 (0x07) 206 207 /* LSM6DSM selftest related */ 208 #define LSM6DSM_NUM_AVERAGE_SELFTEST 5 209 #define LSM6DSM_NUM_AVERAGE_SELFTEST_SLOW 30 210 #define LSM6DSM_ACCEL_SELFTEST_PS (0x01) 211 #define LSM6DSM_GYRO_SELFTEST_PS (0x04) 212 #define LSM6DSM_ACCEL_SELFTEST_NS (0x02) 213 #define LSM6DSM_GYRO_SELFTEST_NS (0x0c) 214 #define LSM6DSM_ACCEL_SELFTEST_HIGH_THR_LSB 6967 /* 1700mg @8g in LSB */ 215 #define LSM6DSM_ACCEL_SELFTEST_LOW_THR_LSB 368 /* 90mg @8g in LSB */ 216 #define LSM6DSM_GYRO_SELFTEST_HIGH_THR_LSB 10000 /* 700dps @2000dps in LSB */ 217 #define LSM6DSM_GYRO_SELFTEST_LOW_THR_LSB 2142 /* 150dps @2000dps in LSB */ 218 219 /* LSM6DSM calibration related */ 220 #define LSM6DSM_NUM_AVERAGE_CALIBRATION 10 221 #define LSM6DSM_1G_IN_LSB_CALIBRATION 4098 /* 1000mg @8g in LSB */ 222 #define LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB 127 /* 8-bit available */ 223 #define LSM6DSM_ACCEL_LSB_TO_OFFSET_DIGIT_SCALE 0.2501f /* @8g */ 224 225 /* LSM6DSM embedded registers */ 226 #define LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR (0x02) 227 #define LSM6DSM_EMBEDDED_SLV0_SUBADDR_ADDR (0x03) 228 #define LSM6DSM_EMBEDDED_SLV0_CONFIG_ADDR (0x04) 229 #define LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR (0x05) 230 #define LSM6DSM_EMBEDDED_SLV1_SUBADDR_ADDR (0x06) 231 #define LSM6DSM_EMBEDDED_SLV1_CONFIG_ADDR (0x07) 232 #define LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR (0x08) 233 #define LSM6DSM_EMBEDDED_SLV2_SUBADDR_ADDR (0x09) 234 #define LSM6DSM_EMBEDDED_SLV2_CONFIG_ADDR (0x0a) 235 #define LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR (0x0b) 236 #define LSM6DSM_EMBEDDED_SLV3_SUBADDR_ADDR (0x0c) 237 #define LSM6DSM_EMBEDDED_SLV3_CONFIG_ADDR (0x0d) 238 #define LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR (0x0e) 239 #define LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR (0x15) 240 241 #define LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB (0x01) 242 #define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR (0x10) 243 #define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR (0x20) 244 #define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR (0x30) 245 #define LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE (0x20) 246 #define LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP (0x07) 247 248 /* LSM6DSM I2C master - slave devices */ 249 #ifdef LSM6DSM_I2C_MASTER_LIS3MDL 250 #define LSM6DSM_MAGN_KSCALE LIS3MDL_KSCALE 251 #define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LIS3MDL_I2C_ADDRESS 252 #define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LIS3MDL_WAI_ADDR 253 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LIS3MDL_CTRL2_ADDR 254 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LIS3MDL_SW_RESET 255 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LIS3MDL_CTRL3_ADDR 256 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LIS3MDL_CTRL3_BASE 257 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LIS3MDL_POWER_ON_VALUE 258 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LIS3MDL_POWER_OFF_VALUE 259 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LIS3MDL_CTRL1_ADDR 260 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LIS3MDL_CTRL1_BASE 261 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LIS3MDL_OUTDATA_ADDR 262 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LIS3MDL_OUTDATA_LEN 263 #define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LIS3MDLMagnRatesRegValue[i] 264 #endif /* LSM6DSM_I2C_MASTER_LIS3MDL */ 265 266 #ifdef LSM6DSM_I2C_MASTER_AK09916 267 #define LSM6DSM_MAGN_KSCALE AK09916_KSCALE 268 #define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT AK09916_I2C_ADDRESS 269 #define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR AK09916_WAI_ADDR 270 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR AK09916_CNTL3_ADDR 271 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE AK09916_SW_RESET 272 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR AK09916_CNTL2_ADDR 273 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE AK09916_CNTL2_BASE 274 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE AK09916_POWER_ON_VALUE 275 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE AK09916_POWER_OFF_VALUE 276 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR AK09916_CNTL2_ADDR 277 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE AK09916_CNTL2_BASE 278 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR AK09916_OUTDATA_ADDR 279 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN AK09916_OUTDATA_LEN 280 #define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) AK09916MagnRatesRegValue[i] 281 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 282 283 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR 284 #define LSM6DSM_MAGN_KSCALE LSM303AGR_KSCALE 285 #define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT LSM303AGR_I2C_ADDRESS 286 #define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR LSM303AGR_WAI_ADDR 287 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR LSM303AGR_CFG_REG_A_M_ADDR 288 #define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE LSM303AGR_SW_RESET 289 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR LSM303AGR_CFG_REG_A_M_ADDR 290 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE LSM303AGR_CFG_REG_A_M_BASE 291 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE LSM303AGR_POWER_ON_VALUE 292 #define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE LSM303AGR_POWER_OFF_VALUE 293 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR LSM303AGR_CFG_REG_A_M_ADDR 294 #define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE LSM303AGR_CFG_REG_A_M_BASE 295 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR LSM303AGR_OUTDATA_ADDR 296 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN LSM303AGR_OUTDATA_LEN 297 #define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i) LSM303AGRMagnRatesRegValue[i] 298 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */ 299 300 #ifdef LSM6DSM_I2C_MASTER_LPS22HB 301 #define LSM6DSM_PRESS_KSCALE LPS22HB_PRESS_KSCALE 302 #define LSM6DSM_TEMP_KSCALE LPS22HB_TEMP_KSCALE 303 #define LSM6DSM_PRESS_OUTDATA_LEN LPS22HB_OUTDATA_PRESS_BYTE 304 #define LSM6DSM_TEMP_OUTDATA_LEN LPS22HB_OUTDATA_TEMP_BYTE 305 #define LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT LPS22HB_I2C_ADDRESS 306 #define LSM6DSM_SENSOR_SLAVE_BARO_DUMMY_REG_ADDR LPS22HB_WAI_ADDR 307 #define LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR LPS22HB_CTRL2_ADDR 308 #define LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE LPS22HB_SW_RESET 309 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR LPS22HB_CTRL1_ADDR 310 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE LPS22HB_CTRL1_BASE 311 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ON_VALUE LPS22HB_POWER_ON_VALUE 312 #define LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE LPS22HB_POWER_OFF_VALUE 313 #define LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR LPS22HB_CTRL1_ADDR 314 #define LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE LPS22HB_CTRL1_BASE 315 #define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR LPS22HB_OUTDATA_ADDR 316 #define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN LPS22HB_OUTDATA_LEN 317 #define LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i) LPS22HBBaroRatesRegValue[i] 318 #endif /* LSM6DSM_I2C_MASTER_LPS22HB */ 319 320 #ifndef LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN 321 #define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN 0 322 #endif /* LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN */ 323 #ifndef LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN 324 #define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN 0 325 #endif /* LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN */ 326 327 /* Magn only enabled */ 328 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 329 #ifdef LSM6DSM_I2C_MASTER_AK09916 330 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR 331 #else /* LSM6DSM_I2C_MASTER_AK09916 */ 332 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR 333 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 334 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */ 335 336 /* Baro only enabled */ 337 #if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 338 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR 339 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */ 340 341 /* Magn & Baro both enabled */ 342 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 343 #ifdef LSM6DSM_I2C_MASTER_AK09916 344 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR 345 #else /* LSM6DSM_I2C_MASTER_AK09916 */ 346 #define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR 347 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 348 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */ 349 350 351 /* LSM6DSM default base registers status */ 352 /* LSM6DSM_FUNC_CFG_ACCESS_BASE: enable embedded functions register */ 353 #define LSM6DSM_FUNC_CFG_ACCESS_BASE (0x00) 354 355 /* LSM6DSM_DRDY_PULSE_CFG_BASE: enable pulsed interrupt register */ 356 #define LSM6DSM_DRDY_PULSE_CFG_BASE (0x00) 357 358 /* LSM6DSM_INT1_CTRL_BASE: interrupt 1 control register default settings */ 359 #define LSM6DSM_INT1_CTRL_BASE ((0 << 7) | /* INT1_STEP_DETECTOR */ \ 360 (0 << 6) | /* INT1_SIGN_MOT */ \ 361 (1 << 5) | /* INT1_FULL_FLAG */ \ 362 (1 << 4) | /* INT1_FIFO_OVR */ \ 363 (1 << 3) | /* INT1_FTH */ \ 364 (0 << 2) | /* INT1_BOOT */ \ 365 (0 << 1) | /* INT1_DRDY_G */ \ 366 (0 << 0)) /* INT1_DRDY_XL */ 367 368 /* LSM6DSM_INT2_CTRL_BASE: interrupt 2 control register default settings */ 369 #define LSM6DSM_INT2_CTRL_BASE ((0 << 7) | /* INT2_STEP_DELTA */ \ 370 (0 << 6) | /* INT2_STEP_OV */ \ 371 (0 << 5) | /* INT2_FULL_FLAG */ \ 372 (0 << 4) | /* INT2_FIFO_OVR */ \ 373 (0 << 3) | /* INT2_FTH */ \ 374 (0 << 2) | /* INT2_DRDY_TEMP */ \ 375 (0 << 1) | /* INT2_DRDY_G */ \ 376 (0 << 0)) /* INT2_DRDY_XL */ 377 378 /* LSM6DSM_CTRL1_XL_BASE: accelerometer sensor register default settings */ 379 #define LSM6DSM_CTRL1_XL_BASE ((0 << 7) | /* ODR_XL3 */ \ 380 (0 << 6) | /* ODR_XL2 */ \ 381 (0 << 5) | /* ODR_XL1 */ \ 382 (0 << 4) | /* ODR_XL0 */ \ 383 (1 << 3) | /* FS_XL1 */ \ 384 (1 << 2) | /* FS_XL0 */ \ 385 (0 << 1) | /* LPF1_BW_SEL */ \ 386 (0 << 0)) /* (0) */ 387 388 /* LSM6DSM_CTRL2_G_BASE: gyroscope sensor register default settings */ 389 #define LSM6DSM_CTRL2_G_BASE ((0 << 7) | /* ODR_G3 */ \ 390 (0 << 6) | /* ODR_G2 */ \ 391 (0 << 5) | /* ODR_G1 */ \ 392 (0 << 4) | /* ODR_G0 */ \ 393 (1 << 3) | /* FS_G1 */ \ 394 (1 << 2) | /* FS_G0 */ \ 395 (0 << 1) | /* FS_125 */ \ 396 (0 << 0)) /* (0) */ 397 398 /* LSM6DSM_CTRL3_C_BASE: control register 3 default settings */ 399 #define LSM6DSM_CTRL3_C_BASE ((0 << 7) | /* BOOT */ \ 400 (1 << 6) | /* BDU */ \ 401 (0 << 5) | /* H_LACTIVE */ \ 402 (0 << 4) | /* PP_OD */ \ 403 (0 << 3) | /* SIM */ \ 404 (1 << 2) | /* IF_INC */ \ 405 (0 << 1) | /* BLE */ \ 406 (0 << 0)) /* SW_RESET */ 407 408 /* LSM6DSM_CTRL4_C_BASE: control register 4 default settings */ 409 #define LSM6DSM_CTRL4_C_BASE ((0 << 7) | /* DEN_XL_EN */ \ 410 (0 << 6) | /* SLEEP */ \ 411 (1 << 5) | /* INT2_on_INT1 */ \ 412 (0 << 4) | /* DEN_DRDY_MASK */ \ 413 (0 << 3) | /* DRDY_MASK */ \ 414 (1 << 2) | /* I2C_disable */ \ 415 (0 << 1) | /* LPF1_SEL_G */ \ 416 (0 << 0)) /* (0) */ 417 418 /* LSM6DSM_CTRL5_C_BASE: control register 5 default settings */ 419 #define LSM6DSM_CTRL5_C_BASE (0x00) 420 421 /* LSM6DSM_CTRL10_C_BASE: control register 10 default settings */ 422 #define LSM6DSM_CTRL10_C_BASE ((0 << 7) | /* (WRIST_TILT_EN) */ \ 423 (0 << 6) | /* (0) */ \ 424 (1 << 5) | /* TIMER_EN */ \ 425 (0 << 4) | /* PEDO_EN */ \ 426 (0 << 3) | /* TILT_EN */ \ 427 (1 << 2) | /* FUNC_EN */ \ 428 (0 << 1) | /* PEDO_RST_STEP */ \ 429 (0 << 0)) /* SIGN_MOTION_EN */ 430 431 /* LSM6DSM_MASTER_CONFIG_BASE: I2C master configuration register default value */ 432 #ifdef LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP 433 #define LSM6DSM_MASTER_CONFIG_BASE (LSM6DSM_MASTER_CONFIG_PULL_UP_EN) 434 #else /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */ 435 #define LSM6DSM_MASTER_CONFIG_BASE (0x00) 436 #endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */ 437 438 /* LSM6DSM_WAKE_UP_DUR_BASE: control register WK default settings */ 439 #define LSM6DSM_WAKE_UP_DUR_BASE (0x10) /* TIMER_HR */ 440 441 #define LSM6DSM_X_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \ 442 ((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \ 443 (r21 == 1 ? y : (r21 == -1 ? -y : 0)) + \ 444 (r31 == 1 ? z : (r31 == -1 ? -z : 0))) 445 446 #define LSM6DSM_Y_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \ 447 ((r12 == 1 ? x : (r12 == -1 ? -x : 0)) + \ 448 (r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \ 449 (r32 == 1 ? z : (r32 == -1 ? -z : 0))) 450 451 #define LSM6DSM_Z_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \ 452 ((r13 == 1 ? x : (r13 == -1 ? -x : 0)) + \ 453 (r23 == 1 ? y : (r23 == -1 ? -y : 0)) + \ 454 (r33 == 1 ? z : (r33 == -1 ? -z : 0))) 455 456 #define LSM6DSM_REMAP_X_DATA(...) LSM6DSM_X_MAP(__VA_ARGS__) 457 #define LSM6DSM_REMAP_Y_DATA(...) LSM6DSM_Y_MAP(__VA_ARGS__) 458 #define LSM6DSM_REMAP_Z_DATA(...) LSM6DSM_Z_MAP(__VA_ARGS__) 459 460 enum SensorIndex { 461 GYRO = 0, 462 ACCEL, 463 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 464 MAGN, 465 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 466 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 467 PRESS, 468 TEMP, 469 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 470 STEP_DETECTOR, 471 STEP_COUNTER, 472 SIGN_MOTION, 473 NUM_SENSORS, 474 EMBEDDED_TIMESTAMP 475 }; 476 477 enum SensorFifoIndex { 478 FIFO_GYRO, 479 FIFO_ACCEL, 480 FIFO_DS3, 481 FIFO_DS4, 482 FIFO_NUM 483 }; 484 485 enum InitState { 486 RESET_LSM6DSM = 0, 487 INIT_LSM6DSM, 488 #ifdef LSM6DSM_I2C_MASTER_ENABLED 489 INIT_I2C_MASTER_REGS_CONF, 490 INIT_I2C_MASTER_SENSOR_RESET, 491 INIT_I2C_MASTER_MAGN_SENSOR, 492 INIT_I2C_MASTER_BARO_SENSOR, 493 INIT_I2C_MASTER_SENSOR_END, 494 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 495 INIT_DONE, 496 }; 497 498 enum SelfTestState { 499 SELFTEST_INITIALIZATION = 0, 500 SELFTEST_READ_EST_DATA, 501 SELFTEST_SECOND_STEP_INITIALIZATION, 502 SELFTEST_READ_NST_DATA, 503 SELFTEST_VERIFICATION, 504 SELFTEST_COMPLETED 505 }; 506 507 enum CalibrationState { 508 CALIBRATION_INITIALIZATION = 0, 509 CALIBRATION_READ_DATA, 510 CALIBRATION_VERIFICATION, 511 CALIBRATION_COMPLETED 512 }; 513 514 enum SensorEvents { 515 NO_EVT = -1, 516 EVT_SPI_DONE = EVT_APP_START + 1, 517 EVT_START_ACCEL_TIME_CALIB, 518 EVT_SENSOR_INTERRUPT_1, 519 EVT_SENSOR_POWERING_UP, 520 EVT_SENSOR_POWERING_DOWN, 521 EVT_SENSOR_CONFIG_CHANGING, 522 EVT_SENSOR_RESTORE_IDLE, 523 EVT_TIME_SYNC 524 }; 525 526 enum SensorState { 527 SENSOR_BOOT = 0, 528 SENSOR_VERIFY_WAI, 529 SENSOR_INITIALIZATION, 530 SENSOR_IDLE, 531 SENSOR_POWERING_UP, 532 SENSOR_POWERING_DOWN, 533 SENSOR_CONFIG_CHANGING, 534 SENSOR_CONFIG_WATERMARK_CHANGING, 535 SENSOR_CALIBRATION, 536 SENSOR_STORE_CALIBRATION_DATA, 537 SENSOR_SELFTEST, 538 SENSOR_INT1_STATUS_REG_HANDLING, 539 SENSOR_INT1_OUTPUT_DATA_HANDLING, 540 SENSOR_TIME_SYNC, 541 SENSOR_BARO_READ_DATA, 542 SENSOR_INVALID_STATE 543 }; 544 545 static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay); 546 static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay); 547 static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay); 548 549 #define SPI_MULTIWRITE_0(addr, data, size) lsm6dsm_spiQueueMultiwrite(addr, data, size, 2) 550 #define SPI_MULTIWRITE_1(addr, data, size, delay) lsm6dsm_spiQueueMultiwrite(addr, data, size, delay) 551 #define GET_SPI_MULTIWRITE_MACRO(_1, _2, _3, _4, NAME, ...) NAME 552 #define SPI_MULTIWRITE(...) GET_SPI_MULTIWRITE_MACRO(__VA_ARGS__, SPI_MULTIWRITE_1, SPI_MULTIWRITE_0)(__VA_ARGS__) 553 554 #define SPI_WRITE_0(addr, data) lsm6dsm_spiQueueWrite(addr, data, 2) 555 #define SPI_WRITE_1(addr, data, delay) lsm6dsm_spiQueueWrite(addr, data, delay) 556 #define GET_SPI_WRITE_MACRO(_1, _2, _3, NAME, ...) NAME 557 #define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__) 558 559 #define SPI_READ_0(addr, size, buf) lsm6dsm_spiQueueRead(addr, size, buf, 0) 560 #define SPI_READ_1(addr, size, buf, delay) lsm6dsm_spiQueueRead(addr, size, buf, delay) 561 #define GET_SPI_READ_MACRO(_1, _2, _3, _4, NAME, ...) NAME 562 #define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__) 563 564 #ifdef LSM6DSM_I2C_MASTER_ENABLED 565 static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si); 566 567 #define SPI_WRITE_SS_REGISTER_0(addr, value, accelRate, si) lsm6dsm_writeSlaveRegister(addr, value, accelRate, 0, si) 568 #define SPI_WRITE_SS_REGISTER_1(addr, value, accelRate, si, delay) lsm6dsm_writeSlaveRegister(addr, value, accelRate, delay, si) 569 #define GET_SPI_WRITE_SS_MACRO(_1, _2, _3, _4, _5, NAME, ...) NAME 570 #define SPI_WRITE_SLAVE_SENSOR_REGISTER(...) GET_SPI_WRITE_SS_MACRO(__VA_ARGS__, SPI_WRITE_SS_REGISTER_1, \ 571 SPI_WRITE_SS_REGISTER_0)(__VA_ARGS__) 572 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 573 574 #define INFO_PRINT(fmt, ...) \ 575 do { \ 576 osLog(LOG_INFO, "%s " fmt, "[LSM6DSM]", ##__VA_ARGS__); \ 577 } while (0); 578 579 #define DEBUG_PRINT(fmt, ...) \ 580 do { \ 581 if (LSM6DSM_DBG_ENABLED) { \ 582 osLog(LOG_DEBUG, "%s " fmt, "[LSM6DSM]", ##__VA_ARGS__); \ 583 } \ 584 } while (0); 585 586 #define ERROR_PRINT(fmt, ...) \ 587 do { \ 588 osLog(LOG_ERROR, "%s " fmt, "[LSM6DSM]", ##__VA_ARGS__); \ 589 } while (0); 590 591 /* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */ 592 #ifndef LSM6DSM_DBG_ENABLED 593 #define LSM6DSM_DBG_ENABLED 0 594 #endif /* LSM6DSM_DBG_ENABLED */ 595 596 597 /* 598 * struct LSM6DSMSPISlaveInterface: SPI slave data interface 599 * @packets: spi packets needed to perform read/write operations. 600 * @txrxBuffer: spi data buffer. 601 * @spiDev: spi device info. 602 * @mode: spi mode info (frequency, polarity, etc). 603 * @mWbufCnt: counter of total data in spi buffer. 604 * @cs: chip select used by SPI slave. 605 * @funcSrcBuffer: pointer of txrxBuffer to access func source register data. 606 * @tmpDataBuffer: pointer of txrxBuffer to access sporadic temp read. 607 * @fifoDataBuffer: pointer of txrxBuffer to access fifo data. 608 * @fifoStatusRegBuffer: pointer of txrxBuffer to access fifo status registers. 609 * @stepCounterDataBuffer: pointer of txrxBuffer to access step counter data. 610 * @tempDataBuffer: pointer of txrxBuffer to access sensor temperature data needed by calibration algos. 611 * @timestampDataBuffer: pointer of txrxBuffer to access sensor timestamp data in order to syncronize time. 612 * @timestampDataBufferBaro: pointer of txrxBuffer to access sensor timestamp data for barometer when not in FIFO. 613 * @baroDataBuffer: pointer of txrx to access barometer data from DSM when not in FIFO. 614 * @mRegCnt: spi packet num counter. 615 * @spiInUse: flag used to check if SPI is currently busy. 616 */ 617 struct LSM6DSMSPISlaveInterface { 618 struct SpiPacket packets[LSM6DSM_SPI_PACKET_SIZE]; 619 uint8_t txrxBuffer[SPI_BUF_SIZE]; 620 struct SpiDevice *spiDev; 621 struct SpiMode mode; 622 623 uint16_t mWbufCnt; 624 625 spi_cs_t cs; 626 627 uint8_t *funcSrcBuffer; 628 uint8_t *tmpDataBuffer; 629 uint8_t *fifoDataBuffer; 630 uint8_t *fifoStatusRegBuffer; 631 uint8_t *stepCounterDataBuffer; 632 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED) 633 uint8_t *tempDataBuffer; 634 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */ 635 uint8_t *timestampDataBuffer; 636 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 637 uint8_t *timestampDataBufferBaro; 638 uint8_t *baroDataBuffer; 639 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 640 uint8_t mRegCnt; 641 642 bool spiInUse; 643 }; 644 645 /* 646 * struct LSM6DSMConfigStatus: temporary data of pending events 647 * @latency: value to be used in next setRate operation [ns]. 648 * @rate: value to be used in next setRate operation [Hz * 1024]. 649 * @enable: value to be used in next setEnable. 650 */ 651 struct LSM6DSMConfigStatus { 652 uint64_t latency; 653 uint32_t rate; 654 bool enable; 655 }; 656 657 /* 658 * struct LSM6DSMSensor: sensor status data 659 * @pConfig: temporary data of pending events. 660 * @tADataEvt: three axis sensor data to send to nanohub. 661 * @sADataEvt: one axis sensor data to send to nanohub. 662 * @latency: current value of latency [n]. 663 * @pushedTimestamp: latest sample timestamp pusshed to nanohub. 664 * @handle: sensor handle obtained by sensorRegister. 665 * @rate: current value of rates based on dependecies [Hz * 1024]. 666 * @hwRate: current value of physical rate [Hz * 1024]. 667 * @idx: enum SensorIndex. 668 * @samplesToDiscard: samples to discard after enable or odr switch. 669 * @samplesDecimator: sw decimator factor to achieve lower odr that cannot be achieved only by FIFO decimator. For example accel is used by dependecies. 670 * @samplesDecimatorCounter: samples counter working together with samplesDecimator. 671 * @samplesFifoDecimator: sw decimator factor to achieve lower odr that cannot be achived by FIFO decimator. 672 * @samplesFifoDecimatorCounter: samples counter working together with sampleFifoDecimator. 673 * @dependenciesRequireData: mask used to verify if dependencies needs data or not. For example accel is used for internal algos. 674 * enabled: current status of sensor. 675 */ 676 struct LSM6DSMSensor { 677 struct LSM6DSMConfigStatus pConfig; 678 679 union { 680 struct TripleAxisDataEvent *tADataEvt; 681 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 682 struct SingleAxisDataEvent *sADataEvt; 683 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 684 }; 685 686 uint64_t latency; 687 uint64_t pushedTimestamp; 688 uint32_t handle; 689 uint32_t rate[NUM_SENSORS]; 690 uint32_t hwRate; 691 enum SensorIndex idx; 692 uint8_t samplesToDiscard; 693 uint8_t samplesDecimator; 694 uint8_t samplesDecimatorCounter; 695 uint8_t samplesFifoDecimator; 696 uint8_t samplesFifoDecimatorCounter; 697 bool dependenciesRequireData[NUM_SENSORS]; 698 bool enabled; 699 }; 700 701 /* 702 * struct LSM6DSMFifoCntl: fifo control data 703 * @decimatorsIdx: give who is the sensor that store data in that FIFO slot. 704 * @triggerRate: frequency of FIFO [Hz * 1024]. 705 * @watermark: watermark value in #num of samples. 706 * @decimators: fifo decimators value. 707 * @minDecimator: min value of decimators. 708 * @maxDecimator: max value of decimators. 709 * @maxMinDecimator: maxDecimator devided by minDecimator. 710 * @totalSip: total number of samples in one pattern. 711 * @timestampPosition: since timestamp in FIFO is the latest sensor, we need to know where is located during FIFO parsing. 712 */ 713 struct LSM6DSMFifoCntl { 714 enum SensorIndex decimatorsIdx[FIFO_NUM]; 715 uint32_t triggerRate; 716 uint16_t watermark; 717 uint8_t decimators[FIFO_NUM]; 718 uint8_t minDecimator; 719 uint8_t maxDecimator; 720 uint8_t maxMinDecimator; 721 uint8_t totalSip; 722 uint8_t timestampPosition[32]; 723 }; 724 725 /* 726 * struct LSM6DSMTimeCalibrationWithoutTimer: data used when time calibration is performed during FIFO read. 727 * If latency is smaller than LSM6DSM_SYNC_DELTA_INTERVAL no need to use a timer but we can read timestamp before read FIFO data. 728 * @lastTimestampDataAvlRtcTime: last time we perform a timestamp read from LSM6DSM based on RTC time. 729 * @newTimestampDataAvl: when deltatime is enough we can read again timestamp from LSM6DSM. 730 */ 731 struct LSM6DSMTimeCalibrationWithoutTimer { 732 uint64_t lastTimestampDataAvlRtcTime; 733 bool newTimestampDataAvl; 734 }; 735 736 enum LSM6DSMTimeCalibrationStatus { 737 TIME_SYNC_DISABLED, 738 TIME_SYNC_TIMER, 739 TIME_SYNC_DURING_FIFO_READ 740 }; 741 742 /* 743 * struct LSM6DSMTimeCalibration: time calibration task data 744 * @sensorTimeToRtcData: timeSync algo data. 745 * @noTimer: if timer is not used to perform time sync, those data will be used. 746 * @lastSampleTimestamp: last sample timestamp from FIFO. Already coverted to RTC time. 747 * @timeSyncRtcTime: Rtc time while performing timestamp read from LSM6DSM. 748 * @sampleTimestampFromFifoLSB: current timestamp from FIFO in LSB. Needs to be stored becasue of overflow. 749 * @timestampSyncTaskLSB: when timer is used to sync time, this is the last timestamp read from LSM6DSM in LSB. Needs to be stored becasue of overflow. 750 * @deltaTimeMarginLSB: is it used to verify if timestamp from FIFO is valid, this is max jitter that timestamp can have from FIFO. 751 * @timestampBaroLSB: if magn and baro are both enabled, barometer data are read with a timer because no slots are available in FIFO. This is the timestamp of baro data. 752 * @theoreticalDeltaTimeLSB: theoretical value of timestamp based on sensor frequency. 753 * @timestampIsValid: flag that indicate if current timestamp parsing FIFO is valid. 754 */ 755 struct LSM6DSMTimeCalibration { 756 time_sync_t sensorTimeToRtcData; 757 struct LSM6DSMTimeCalibrationWithoutTimer noTimer; 758 uint64_t lastSampleTimestamp; 759 uint64_t timeSyncRtcTime; 760 enum LSM6DSMTimeCalibrationStatus status; 761 uint32_t sampleTimestampFromFifoLSB; 762 uint32_t timestampSyncTaskLSB; 763 uint32_t deltaTimeMarginLSB; 764 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 765 uint32_t timestampBaroLSB; 766 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 767 uint32_t theoreticalDeltaTimeLSB; 768 bool timestampIsValid; 769 }; 770 771 /* 772 * struct LSM6DSMSelfTestResultPkt: self-test packet result data 773 * @header: describe packet size and application ID of packet. 774 * @dataHeader: payload of message. 775 */ 776 struct LSM6DSMSelfTestResultPkt { 777 struct HostHubRawPacket header; 778 struct SensorAppEventHeader dataHeader; 779 } __attribute__((packed)); 780 781 /* 782 * struct LSM6DSMCalibrationResultPkt: calibration packet result data 783 * @header: describe packet size and application ID of packet. 784 * @dataHeader: payload of header message. 785 * @xBias: raw offset value X axis. 786 * @yBias: raw offset value Y axis. 787 * @zBias: raw offset value Z axis. 788 */ 789 struct LSM6DSMCalibrationResultPkt { 790 struct HostHubRawPacket header; 791 struct SensorAppEventHeader dataHeader; 792 int32_t xBias; 793 int32_t yBias; 794 int32_t zBias; 795 } __attribute__((packed)); 796 797 /* 798 * struct LSM6DSMAccelGyroCfgData: configuration packet data 799 * @hw: chip level calibration data. 800 * @sw: software level calibration data (algos). 801 */ 802 struct LSM6DSMAccelGyroCfgData { 803 int32_t hw[LSM6DSM_TRIAXIAL_NUM_AXIS]; 804 float sw[LSM6DSM_TRIAXIAL_NUM_AXIS]; 805 }; 806 807 /* 808 * struct LSM6DSMTask: driver task data 809 * @sensors: sensor status data list. 810 * @slaveConn: slave interface / communication data. 811 * @accelCal: accelerometer calibration algo data. 812 * @gyroCal: gyroscope calibration algo data. 813 * @overTempCal: gyroscope over temperature calibration algo data. 814 * @magnCal: magnetometer calibration algo data. 815 * @int1: int1 gpio data. 816 * @isr1: isr1 data. 817 * @mDataSlabThreeAxis: memory used to store three axis sensors data. 818 * @mDataSlabOneAxis: memory used to store one axis sensors data. 819 * @fifoCntl: fifo control data. 820 * @time: time calibration data. 821 * @currentTemperature: sensor temperature data value used by gyroscope/accelerometer bias calibration libs. 822 * @lastFifoReadTimestamp: store when last time FIFO was read. 823 * @initState: initialization is done in several steps (enum InitState). 824 * @selftestState: self-test is performed in several steps (enum SelfTestState). 825 * @calibrationState: sensor calibration is done in several steps (enum CalibrationState). 826 * @tid: task id. 827 * @totalNumSteps: total number of steps of step counter sensor. 828 * @fifoDataToRead: number of byte to read in current FIFO read. 829 * @fifoDataToReadPending: in order to reduce txrxBuffer, FIFO read is performed in several read. This value tell how many data still need to read from FIFO. 830 * @baroTimerId: barometer task timer id. 831 * @dataSelftestEnabled: sensor data read during GapSelfTestProgram while self-test bit is set. 832 * @dataSelftestNotEnabled: sensor data read during GapSelfTestProgram while self-test bit is not set. 833 * @dataCalibration: sensor data read during calibration program. 834 * @accelCalibrationData: accelerometer offset value (hw) to store into sensor. 835 * @gyroCalibrationData: gyroscope offset value (hw) applied to each sample (by software). 836 * @state: task state, driver manage operations using a state machine (enum SensorState). 837 * @numSamplesSelftest: temp variable storing number of samples read by self-test program. 838 * @numSamplesCalibration: temp variable storing number of samples read by calibration program. 839 * @mRetryLeft: counter used to retry operations #n times before return a failure. 840 * @pedometerDependencies: dependencies mask of sensors that are using embedded functions. 841 * @masterConfigDependencies: dependencies mask of sensors that are using I2C master. 842 * @int1Register: interrupt 1 register content (addr: 0x0d). 843 * @int2Register: interrupt 2 register content (addr: 0x0e). 844 * @embeddedFunctionsRegister: embedded register content (addr: 0x19). 845 * @pendingFlush: number of flush requested for each sensor. 846 * @masterConfigRegister: i2c master register content (addr: 0x1a). 847 * @readSteps: flag used to indicate if interrupt task need to read number of steps. 848 * @sendFlushEvt: if flush is requested, send it out after FIFO read is completed. 849 * @pendingEnableConfig: pending setEnable operations to be executed. 850 * @pendingRateConfig: pending setRate operations to be executed. 851 * @pendingInt: pending interrupt task to be executed. 852 * @pendingTimeSyncTask: pending time sync task to be executed. 853 * @pendingBaroTimerTask: pending baro read data task to be executed. 854 * @pendingStoreAccelCalibData: pending calibration data store task to be executed. 855 */ 856 typedef struct LSM6DSMTask { 857 struct LSM6DSMSensor sensors[NUM_SENSORS]; 858 struct LSM6DSMSPISlaveInterface slaveConn; 859 860 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED 861 struct AccelCal accelCal; 862 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */ 863 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 864 struct GyroCal gyroCal; 865 #ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED 866 struct OverTempCal overTempCal; 867 #endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */ 868 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 869 #ifdef LSM6DSM_MAGN_CALIB_ENABLED 870 struct MagCal magnCal; 871 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ 872 873 struct Gpio *int1; 874 struct ChainedIsr isr1; 875 struct SlabAllocator *mDataSlabThreeAxis; 876 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 877 struct SlabAllocator *mDataSlabOneAxis; 878 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 879 struct LSM6DSMFifoCntl fifoCntl; 880 struct LSM6DSMTimeCalibration time; 881 882 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED) 883 float currentTemperature; 884 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */ 885 886 uint64_t lastFifoReadTimestamp; 887 888 enum InitState initState; 889 enum SelfTestState selftestState; 890 enum CalibrationState calibrationState; 891 892 uint32_t tid; 893 uint32_t totalNumSteps; 894 uint32_t fifoDataToRead; 895 uint32_t fifoDataToReadPending; 896 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 897 uint32_t baroTimerId; 898 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 899 int32_t dataSelftestEnabled[LSM6DSM_TRIAXIAL_NUM_AXIS]; 900 int32_t dataSelftestNotEnabled[LSM6DSM_TRIAXIAL_NUM_AXIS]; 901 int32_t dataCalibration[LSM6DSM_TRIAXIAL_NUM_AXIS]; 902 int32_t accelCalibrationData[LSM6DSM_TRIAXIAL_NUM_AXIS]; 903 int32_t gyroCalibrationData[LSM6DSM_TRIAXIAL_NUM_AXIS]; 904 905 volatile uint8_t state; 906 907 uint8_t numSamplesSelftest; 908 uint8_t numSamplesCalibration; 909 uint8_t mRetryLeft; 910 uint8_t pedometerDependencies; 911 uint8_t masterConfigDependencies; 912 uint8_t int1Register; 913 uint8_t int2Register; 914 uint8_t embeddedFunctionsRegister; 915 uint8_t pendingFlush[NUM_SENSORS]; 916 #ifdef LSM6DSM_I2C_MASTER_ENABLED 917 uint8_t masterConfigRegister; 918 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 919 920 bool readSteps; 921 bool sendFlushEvt[NUM_SENSORS]; 922 bool pendingEnableConfig[NUM_SENSORS]; 923 bool pendingRateConfig[NUM_SENSORS]; 924 bool pendingInt; 925 bool pendingTimeSyncTask; 926 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 927 bool pendingBaroTimerTask; 928 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 929 bool pendingStoreAccelCalibData; 930 } LSM6DSMTask; 931 932 static LSM6DSMTask mTask; 933 934 #define TASK LSM6DSMTask* const _task 935 #define TDECL() TASK = &mTask; (void)_task 936 #define T(v) (_task->v) 937 #define T_SLAVE_INTERFACE(v) (_task->slaveConn.v) 938 939 #define BIT(x) (0x01 << x) 940 #define SENSOR_HZ_RATE_TO_US(x) (1024000000UL / x) 941 #define NS_TO_US(ns) cpuMathU64DivByU16(ns, 1000) 942 943 /* Atomic get state */ 944 #define GET_STATE() (atomicReadByte(&(_task->state))) 945 946 /* Atomic set state, this set the state to arbitrary value, use with caution */ 947 #define SET_STATE(s) \ 948 do { \ 949 atomicWriteByte(&(_task->state), (s)); \ 950 } while (0) 951 952 static bool trySwitchState_(TASK, enum SensorState newState) 953 { 954 return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState); 955 } 956 #define trySwitchState(s) trySwitchState_(_task, (s)) 957 958 static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext); 959 #define lsm6dsm_readStatusReg(a) lsm6dsm_readStatusReg_(_task, (a)) 960 961 #define DEC_INFO(name, type, axis, inter, samples) \ 962 .sensorName = name, \ 963 .sensorType = type, \ 964 .numAxis = axis, \ 965 .interrupt = inter, \ 966 .minSamples = samples 967 968 #define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ 969 DEC_INFO(name, type, axis, inter, samples), \ 970 .supportedRates = rates 971 972 #define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \ 973 DEC_INFO(name, type, axis, inter, samples), \ 974 .supportedRates = rates, \ 975 .flags1 = SENSOR_INFO_FLAGS1_BIAS, \ 976 .biasType = bias 977 978 #define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \ 979 DEC_INFO(name, type, axis, inter, samples), \ 980 .supportedRates = rates, \ 981 .flags1 = SENSOR_INFO_FLAGS1_RAW, \ 982 .rawType = raw, \ 983 .rawScale = scale 984 985 #define DEC_INFO_RATE_RAW_BIAS(name, rates, type, axis, inter, samples, raw, scale, bias) \ 986 DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale), \ 987 .flags1 = SENSOR_INFO_FLAGS1_RAW | SENSOR_INFO_FLAGS1_BIAS, \ 988 .biasType = bias 989 990 /* 991 * LSM6DSMImuRates: supported frequencies by accelerometer and gyroscope sensors 992 * LSM6DSMImuRatesRegValue, LSM6DSMRatesSamplesToDiscardGyroPowerOn, LSM6DSMAccelRatesSamplesToDiscard, 993 * LSM6DSMGyroRatesSamplesToDiscard must have same length. 994 */ 995 static uint32_t LSM6DSMImuRates[] = { 996 SENSOR_HZ(26.0f / 32.0f), /* 0.8125Hz */ 997 SENSOR_HZ(26.0f / 16.0f), /* 1.625Hz */ 998 SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */ 999 SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */ 1000 SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */ 1001 SENSOR_HZ(26.0f), /* 26Hz */ 1002 SENSOR_HZ(52.0f), /* 52Hz */ 1003 SENSOR_HZ(104.0f), /* 104Hz */ 1004 SENSOR_HZ(208.0f), /* 208Hz */ 1005 SENSOR_HZ(416.0f), /* 416Hz */ 1006 0, 1007 }; 1008 1009 static uint32_t LSM6DSMImuRatesInNs[] = { 1010 1230769230, /* 0.8125Hz */ 1011 615384615, /* 1.625Hz */ 1012 307692308, /* 3.25Hz */ 1013 153846154, /* 6.5Hz */ 1014 80000000, /* 12.5Hz */ 1015 38461538, /* 26Hz */ 1016 19230769, /* 52Hz */ 1017 9615385, /* 104Hz */ 1018 4807692, /* 208Hz */ 1019 2403846, /* 416Hz */ 1020 0, 1021 }; 1022 1023 static uint8_t LSM6DSMImuRatesRegValue[] = { 1024 LSM6DSM_ODR_12HZ_REG_VALUE, /* 0.8125Hz - do not exist, use 12.5Hz */ 1025 LSM6DSM_ODR_12HZ_REG_VALUE, /* 1.625Hz - do not exist, use 12.5Hz */ 1026 LSM6DSM_ODR_12HZ_REG_VALUE, /* 3.25Hz - do not exist, use 12.5Hz */ 1027 LSM6DSM_ODR_12HZ_REG_VALUE, /* 6.5Hz - do not exist, use 12.5Hz */ 1028 LSM6DSM_ODR_12HZ_REG_VALUE, /* 12.5Hz */ 1029 LSM6DSM_ODR_26HZ_REG_VALUE, /* 26Hz */ 1030 LSM6DSM_ODR_52HZ_REG_VALUE, /* 52Hz */ 1031 LSM6DSM_ODR_104HZ_REG_VALUE, /* 104Hz */ 1032 LSM6DSM_ODR_208HZ_REG_VALUE, /* 208Hz */ 1033 LSM6DSM_ODR_416HZ_REG_VALUE, /* 416Hz */ 1034 }; 1035 1036 /* When sensors switch status from power-down, constant boottime must be considered, some samples should be discarded */ 1037 static uint8_t LSM6DSMRatesSamplesToDiscardGyroPowerOn[] = { 1038 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 0.8125Hz - do not exist, use 12.5Hz = 80000us */ 1039 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 1.625Hz - do not exist, use 12.5Hz = 80000us */ 1040 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 3.25Hz - do not exist, use 12.5Hz = 80000us */ 1041 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 6.5Hz - do not exist, use 12.5Hz = 80000us */ 1042 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 12.5Hz = 80000us */ 1043 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 38461, /* 26Hz = 38461us */ 1044 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 19230, /* 52Hz = 19230s */ 1045 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 9615, /* 104Hz = 9615us */ 1046 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 4807, /* 208Hz = 4807us */ 1047 LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 2403, /* 416Hz = 2403us */ 1048 }; 1049 1050 /* When accelerometer change odr but sensor is already on, few samples should be discarded */ 1051 static uint8_t LSM6DSMAccelRatesSamplesToDiscard[] = { 1052 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 0.8125Hz - do not exist, use 12.5Hz */ 1053 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 1.625Hz - do not exist, use 12.5Hz */ 1054 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 3.25Hz - do not exist, use 12.5Hz */ 1055 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 6.5Hz - do not exist, use 12.5Hz */ 1056 LSM6DSM_ODR_12HZ_ACCEL_STD, /* 12.5Hz */ 1057 LSM6DSM_ODR_26HZ_ACCEL_STD, /* 26Hz */ 1058 LSM6DSM_ODR_52HZ_ACCEL_STD, /* 52Hz */ 1059 LSM6DSM_ODR_104HZ_ACCEL_STD, /* 104Hz */ 1060 LSM6DSM_ODR_208HZ_ACCEL_STD, /* 208Hz */ 1061 LSM6DSM_ODR_416HZ_ACCEL_STD, /* 416Hz */ 1062 }; 1063 1064 /* When gyroscope change odr but sensor is already on, few samples should be discarded */ 1065 static uint8_t LSM6DSMGyroRatesSamplesToDiscard[] = { 1066 LSM6DSM_ODR_12HZ_GYRO_STD, /* 0.8125Hz - do not exist, use 12.5Hz */ 1067 LSM6DSM_ODR_12HZ_GYRO_STD, /* 1.625Hz - do not exist, use 12.5Hz */ 1068 LSM6DSM_ODR_12HZ_GYRO_STD, /* 3.25Hz - do not exist, use 12.5Hz */ 1069 LSM6DSM_ODR_12HZ_GYRO_STD, /* 6.5Hz - do not exist, use 12.5Hz */ 1070 LSM6DSM_ODR_12HZ_GYRO_STD, /* 12.5Hz */ 1071 LSM6DSM_ODR_26HZ_GYRO_STD, /* 26Hz */ 1072 LSM6DSM_ODR_52HZ_GYRO_STD, /* 52Hz */ 1073 LSM6DSM_ODR_104HZ_GYRO_STD, /* 104Hz */ 1074 LSM6DSM_ODR_208HZ_GYRO_STD, /* 208Hz */ 1075 LSM6DSM_ODR_416HZ_GYRO_STD, /* 416Hz */ 1076 }; 1077 1078 #ifdef LSM6DSM_I2C_MASTER_ENABLED 1079 static uint32_t LSM6DSMSHRates[] = { 1080 SENSOR_HZ(26.0f / 32.0f), /* 0.8125Hz */ 1081 SENSOR_HZ(26.0f / 16.0f), /* 1.625Hz */ 1082 SENSOR_HZ(26.0f / 8.0f), /* 3.25Hz */ 1083 SENSOR_HZ(26.0f / 4.0f), /* 6.5Hz */ 1084 SENSOR_HZ(26.0f / 2.0f), /* 12.5Hz */ 1085 SENSOR_HZ(26.0f), /* 26Hz */ 1086 SENSOR_HZ(52.0f), /* 52Hz */ 1087 SENSOR_HZ(104.0f), /* 104Hz */ 1088 0, 1089 }; 1090 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 1091 1092 static uint32_t LSM6DSMStepCounterRates[] = { 1093 SENSOR_HZ(1.0f / (128 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 209.715 sec */ 1094 SENSOR_HZ(1.0f / (64 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 104.857 sec */ 1095 SENSOR_HZ(1.0f / (32 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 52.4288 sec */ 1096 SENSOR_HZ(1.0f / (16 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 26.1574 sec */ 1097 SENSOR_HZ(1.0f / (8 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 13.0787 sec */ 1098 SENSOR_HZ(1.0f / (4 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 6.53936 sec */ 1099 SENSOR_HZ(1.0f / (2 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 3.26968 sec */ 1100 SENSOR_HZ(1.0f / (1 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 1.63840 sec */ 1101 SENSOR_RATE_ONCHANGE, 1102 0, 1103 }; 1104 1105 static const struct SensorInfo LSM6DSMSensorInfo[NUM_SENSORS] = { 1106 { 1107 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 1108 DEC_INFO_RATE_BIAS("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) 1109 #else /* LSM6DSM_GYRO_CALIB_ENABLED */ 1110 DEC_INFO_RATE("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) 1111 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 1112 }, 1113 { 1114 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED 1115 DEC_INFO_RATE_RAW_BIAS("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000, 1116 SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE, SENS_TYPE_ACCEL_BIAS) 1117 #else /* LSM6DSM_ACCEL_CALIB_ENABLED */ 1118 DEC_INFO_RATE_RAW("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000, 1119 SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE) 1120 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */ 1121 }, 1122 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1123 { 1124 #ifdef LSM6DSM_MAGN_CALIB_ENABLED 1125 DEC_INFO_RATE_BIAS("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) 1126 #else /* LSM6DSM_MAGN_CALIB_ENABLED */ 1127 DEC_INFO_RATE("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 600) 1128 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ 1129 }, 1130 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 1131 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 1132 { 1133 DEC_INFO_RATE("Pressure", LSM6DSMSHRates, SENS_TYPE_BARO, NUM_AXIS_ONE, NANOHUB_INT_NONWAKEUP, 300) 1134 }, 1135 { 1136 DEC_INFO_RATE("Temperature", LSM6DSMSHRates, SENS_TYPE_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) 1137 }, 1138 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 1139 { 1140 DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 100) 1141 }, 1142 { 1143 DEC_INFO_RATE("Step Counter", LSM6DSMStepCounterRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) 1144 }, 1145 { 1146 DEC_INFO("Significant Motion", SENS_TYPE_SIG_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_WAKEUP, 1) 1147 }, 1148 }; 1149 1150 #define DEC_OPS(power, firmware, rate, flush) \ 1151 .sensorPower = power, \ 1152 .sensorFirmwareUpload = firmware, \ 1153 .sensorSetRate = rate, \ 1154 .sensorFlush = flush 1155 1156 #define DEC_OPS_SEND(power, firmware, rate, flush, send) \ 1157 .sensorPower = power, \ 1158 .sensorFirmwareUpload = firmware, \ 1159 .sensorSetRate = rate, \ 1160 .sensorFlush = flush, \ 1161 .sensorSendOneDirectEvt = send 1162 1163 #define DEC_OPS_CFG_SELFTEST(power, firmware, rate, flush, cfgData, selftest) \ 1164 DEC_OPS(power, firmware, rate, flush), \ 1165 .sensorCfgData = cfgData, \ 1166 .sensorSelfTest = selftest 1167 1168 #define DEC_OPS_CAL_CFG_SELFTEST(power, firmware, rate, flush, cal, cfgData, selftest) \ 1169 DEC_OPS(power, firmware, rate, flush), \ 1170 .sensorCalibrate = cal, \ 1171 .sensorCfgData = cfgData, \ 1172 .sensorSelfTest = selftest 1173 1174 static bool lsm6dsm_setAccelPower(bool on, void *cookie); 1175 static bool lsm6dsm_setGyroPower(bool on, void *cookie); 1176 static bool lsm6dsm_setStepDetectorPower(bool on, void *cookie); 1177 static bool lsm6dsm_setStepCounterPower(bool on, void *cookie); 1178 static bool lsm6dsm_setSignMotionPower(bool on, void *cookie); 1179 static bool lsm6dsm_accelFirmwareUpload(void *cookie); 1180 static bool lsm6dsm_gyroFirmwareUpload(void *cookie); 1181 static bool lsm6dsm_stepDetectorFirmwareUpload(void *cookie); 1182 static bool lsm6dsm_stepCounterFirmwareUpload(void *cookie); 1183 static bool lsm6dsm_signMotionFirmwareUpload(void *cookie); 1184 static bool lsm6dsm_setAccelRate(uint32_t rate, uint64_t latency, void *cookie); 1185 static bool lsm6dsm_setGyroRate(uint32_t rate, uint64_t latency, void *cookie); 1186 static bool lsm6dsm_setStepDetectorRate(uint32_t rate, uint64_t latency, void *cookie); 1187 static bool lsm6dsm_setStepCounterRate(uint32_t rate, uint64_t latency, void *cookie); 1188 static bool lsm6dsm_setSignMotionRate(uint32_t rate, uint64_t latency, void *cookie); 1189 static bool lsm6dsm_accelFlush(void *cookie); 1190 static bool lsm6dsm_gyroFlush(void *cookie); 1191 static bool lsm6dsm_stepDetectorFlush(void *cookie); 1192 static bool lsm6dsm_stepCounterFlush(void *cookie); 1193 static bool lsm6dsm_signMotionFlush(void *cookie); 1194 static bool lsm6dsm_stepCounterSendLastData(void *cookie, uint32_t tid); 1195 static bool lsm6dsm_runAccelSelfTest(void *cookie); 1196 static bool lsm6dsm_runGyroSelfTest(void *cookie); 1197 static bool lsm6dsm_runAccelCalibration(void *cookie); 1198 static bool lsm6dsm_runGyroCalibration(void *cookie); 1199 static bool lsm6dsm_accelCfgData(void *data, void *cookie); 1200 static bool lsm6dsm_gyroCfgData(void *data, void *cookie); 1201 1202 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1203 static bool lsm6dsm_setMagnPower(bool on, void *cookie); 1204 static bool lsm6dsm_magnFirmwareUpload(void *cookie); 1205 static bool lsm6dsm_setMagnRate(uint32_t rate, uint64_t latency, void *cookie); 1206 static bool lsm6dsm_magnFlush(void *cookie); 1207 static bool lsm6dsm_runMagnSelfTest(void *cookie); 1208 static bool lsm6dsm_magnCfgData(void *data, void *cookie); 1209 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 1210 1211 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 1212 static bool lsm6dsm_setPressPower(bool on, void *cookie); 1213 static bool lsm6dsm_pressFirmwareUpload(void *cookie); 1214 static bool lsm6dsm_setPressRate(uint32_t rate, uint64_t latency, void *cookie); 1215 static bool lsm6dsm_pressFlush(void *cookie); 1216 static bool lsm6dsm_setTempPower(bool on, void *cookie); 1217 static bool lsm6dsm_tempFirmwareUpload(void *cookie); 1218 static bool lsm6dsm_setTempRate(uint32_t rate, uint64_t latency, void *cookie); 1219 static bool lsm6dsm_tempFlush(void *cookie); 1220 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 1221 1222 static const struct SensorOps LSM6DSMSensorOps[NUM_SENSORS] = { 1223 { DEC_OPS_CAL_CFG_SELFTEST(lsm6dsm_setGyroPower, lsm6dsm_gyroFirmwareUpload, lsm6dsm_setGyroRate, 1224 lsm6dsm_gyroFlush, lsm6dsm_runGyroCalibration, lsm6dsm_gyroCfgData, lsm6dsm_runGyroSelfTest) }, 1225 { DEC_OPS_CAL_CFG_SELFTEST(lsm6dsm_setAccelPower, lsm6dsm_accelFirmwareUpload, lsm6dsm_setAccelRate, 1226 lsm6dsm_accelFlush, lsm6dsm_runAccelCalibration, lsm6dsm_accelCfgData, lsm6dsm_runAccelSelfTest) }, 1227 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 1228 { DEC_OPS_CFG_SELFTEST(lsm6dsm_setMagnPower, lsm6dsm_magnFirmwareUpload, lsm6dsm_setMagnRate, 1229 lsm6dsm_magnFlush, lsm6dsm_magnCfgData, lsm6dsm_runMagnSelfTest) }, 1230 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 1231 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 1232 { DEC_OPS(lsm6dsm_setPressPower, lsm6dsm_pressFirmwareUpload, lsm6dsm_setPressRate, lsm6dsm_pressFlush) }, 1233 { DEC_OPS(lsm6dsm_setTempPower, lsm6dsm_tempFirmwareUpload, lsm6dsm_setTempRate, lsm6dsm_tempFlush) }, 1234 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 1235 { DEC_OPS(lsm6dsm_setStepDetectorPower, lsm6dsm_stepDetectorFirmwareUpload, lsm6dsm_setStepDetectorRate, lsm6dsm_stepDetectorFlush) }, 1236 { DEC_OPS_SEND(lsm6dsm_setStepCounterPower, lsm6dsm_stepCounterFirmwareUpload, lsm6dsm_setStepCounterRate, 1237 lsm6dsm_stepCounterFlush, lsm6dsm_stepCounterSendLastData) }, 1238 { DEC_OPS(lsm6dsm_setSignMotionPower, lsm6dsm_signMotionFirmwareUpload, lsm6dsm_setSignMotionRate, lsm6dsm_signMotionFlush) }, 1239 }; 1240 1241 static void lsm6dsm_processPendingEvt(void); 1242 1243 /* 1244 * lsm6dsm_spiQueueRead: enqueue a new SPI read that will be performed after lsm6dsm_spiBatchTxRx function is called 1245 * @addr: start reading from this register address. 1246 * @size: number of byte to read. 1247 * @buf: address of pointer where store data. 1248 * @delay: wait `delay time' after read is completed. [us] 1249 */ 1250 static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay) 1251 { 1252 TDECL(); 1253 1254 if (T_SLAVE_INTERFACE(spiInUse)) { 1255 ERROR_PRINT("spiQueueRead: SPI in use, cannot queue read (addr=%x len=%d)\n", addr, (int)size); 1256 return; 1257 } 1258 1259 *buf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]); 1260 1261 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = size + 1; 1262 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]); 1263 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = *buf; 1264 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000; 1265 1266 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr | 0x80; 1267 T_SLAVE_INTERFACE(mWbufCnt) += size; 1268 T_SLAVE_INTERFACE(mRegCnt)++; 1269 } 1270 1271 /* 1272 * lsm6dsm_spiQueueWrite: enqueue a new SPI 1-byte write that will be performed after lsm6dsm_spiBatchTxRx function is called 1273 * @addr: write byte to this register address. 1274 * @data: value to write. 1275 * @delay: wait `delay time' after write is completed. [us] 1276 */ 1277 static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay) 1278 { 1279 TDECL(); 1280 1281 if (T_SLAVE_INTERFACE(spiInUse)) { 1282 ERROR_PRINT("spiQueueWrite: SPI in use, cannot queue 1-byte write (addr=%x data=%x)\n", addr, data); 1283 return; 1284 } 1285 1286 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = 2; 1287 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]); 1288 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]); 1289 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000; 1290 1291 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr; 1292 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = data; 1293 T_SLAVE_INTERFACE(mRegCnt)++; 1294 } 1295 1296 /* 1297 * lsm6dsm_spiQueueMultiwrite: enqueue a new SPI n-byte write that will be performed after lsm6dsm_spiBatchTxRx function is called 1298 * @addr: start writing from this register address. 1299 * @data: array data to write. 1300 * @size: number of byte to write. 1301 * @delay: wait `delay time' after write is completed. [us] 1302 */ 1303 static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay) 1304 { 1305 TDECL(); 1306 uint8_t i; 1307 1308 if (T_SLAVE_INTERFACE(spiInUse)) { 1309 ERROR_PRINT("spiQueueMultiwrite: SPI in use, cannot queue multiwrite (addr=%x size=%d)\n", addr, (int)size); 1310 return; 1311 } 1312 1313 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = 1 + size; 1314 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]); 1315 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]); 1316 T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000; 1317 1318 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr; 1319 1320 for (i = 0; i < size; i++) 1321 T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = data[i]; 1322 1323 T_SLAVE_INTERFACE(mRegCnt)++; 1324 } 1325 1326 /* 1327 * lsm6dsm_spiBatchTxRx: perform SPI read and/or write enqueued before 1328 * @mode: SPI configuration data. 1329 * @callback: callback function triggered when all transactions are terminated. 1330 * @cookie: private data delivered to callback function. 1331 * @src: function name and/or custom string used during print to trace the callstack. 1332 */ 1333 static void lsm6dsm_spiBatchTxRx(struct SpiMode *mode, SpiCbkF callback, void *cookie, const char *src) 1334 { 1335 TDECL(); 1336 uint8_t regCount; 1337 1338 if (T_SLAVE_INTERFACE(mWbufCnt) > SPI_BUF_SIZE) { 1339 ERROR_PRINT("spiBatchTxRx: not enough SPI buffer space, dropping transaction. Ref. %s\n", src); 1340 return; 1341 } 1342 1343 if (T_SLAVE_INTERFACE(mRegCnt) > LSM6DSM_SPI_PACKET_SIZE) { 1344 ERROR_PRINT("spiBatchTxRx: too many packets! Ref. %s\n", src); 1345 return; 1346 } 1347 1348 /* Reset variables before issuing SPI transaction. 1349 SPI may finish before spiMasterRxTx finish */ 1350 regCount = T_SLAVE_INTERFACE(mRegCnt); 1351 T_SLAVE_INTERFACE(spiInUse) = true; 1352 T_SLAVE_INTERFACE(mRegCnt) = 0; 1353 T_SLAVE_INTERFACE(mWbufCnt) = 0; 1354 1355 if (spiMasterRxTx(T_SLAVE_INTERFACE(spiDev), T_SLAVE_INTERFACE(cs), T_SLAVE_INTERFACE(packets), regCount, mode, callback, cookie)) { 1356 ERROR_PRINT("spiBatchTxRx: transaction failed!\n"); 1357 } 1358 } 1359 1360 /* 1361 * lsm6dsm_timerCallback: timer callback routine used to retry WAI read 1362 * @timerId: timer identificator. 1363 * @data: private data delivered to private event handler. 1364 */ 1365 static void lsm6dsm_timerCallback(uint32_t timerId, void *data) 1366 { 1367 osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid); 1368 } 1369 1370 /* 1371 * lsm6dsm_timerSyncCallback: time syncronization timer callback routine 1372 * @timerId: timer identificator. 1373 * @data: private data delivered to private event handler. 1374 */ 1375 static void lsm6dsm_timerSyncCallback(uint32_t timerId, void *data) 1376 { 1377 osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid); 1378 } 1379 1380 /* 1381 * lsm6dsm_spiCallback: SPI callback function 1382 * @cookie: private data from lsm6dsm_spiBatchTxRx function. 1383 * @err: error code from SPI transfer. 1384 */ 1385 static void lsm6dsm_spiCallback(void *cookie, int err) 1386 { 1387 TDECL(); 1388 1389 T_SLAVE_INTERFACE(spiInUse) = false; 1390 osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid); 1391 } 1392 1393 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 1394 /* 1395 * lsm6dsm_baroTimerTask: baro read data task 1396 */ 1397 static void lsm6dsm_baroTimerTask(void) 1398 { 1399 TDECL(); 1400 1401 if (trySwitchState(SENSOR_BARO_READ_DATA)) { 1402 SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBufferBaro)); 1403 SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR + LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN, 1404 LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN, &T_SLAVE_INTERFACE(baroDataBuffer)); 1405 1406 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 1407 } else 1408 T(pendingBaroTimerTask) = true; 1409 1410 return; 1411 } 1412 1413 /* 1414 * lsm6dsm_baroTimerCallback: baro timer callback routine 1415 * @timerId: timer identificator. 1416 * @data: private data delivered to private event handler. 1417 */ 1418 static void lsm6dsm_baroTimerCallback(uint32_t timerId, void *data) 1419 { 1420 lsm6dsm_baroTimerTask(); 1421 } 1422 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 1423 1424 /* 1425 * lsm6dsm_timeSyncTask: time syncronization task by timer 1426 */ 1427 static void lsm6dsm_timeSyncTask(void) 1428 { 1429 TDECL(); 1430 1431 if (T(time).status != TIME_SYNC_TIMER) 1432 return; 1433 1434 if (trySwitchState(SENSOR_TIME_SYNC)) { 1435 SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBuffer)); 1436 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED) 1437 SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer)); 1438 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */ 1439 1440 T(time).timeSyncRtcTime = sensorGetTime(); 1441 1442 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 1443 } else 1444 T(pendingTimeSyncTask) = true; 1445 } 1446 1447 /* 1448 * lsm6dsm_readStatusReg_: read status registers (interrupt arrived) 1449 * @TASK: task id. 1450 * @isInterruptContext: function is called directly by ISR. 1451 */ 1452 static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext) 1453 { 1454 if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) { 1455 if (T(sensors[STEP_DETECTOR]).enabled || T(sensors[STEP_COUNTER]).enabled || T(sensors[SIGN_MOTION]).enabled) 1456 SPI_READ(LSM6DSM_FUNC_SRC_ADDR, 1, &T_SLAVE_INTERFACE(funcSrcBuffer)); 1457 1458 SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer)); 1459 1460 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 1461 } else { 1462 if (isInterruptContext) 1463 osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid)); 1464 else 1465 T(pendingInt) = true; 1466 } 1467 } 1468 1469 /* 1470 * lsm6dsm_isr1: INT-1 line service routine 1471 * @isr: isr data. 1472 */ 1473 static bool lsm6dsm_isr1(struct ChainedIsr *isr) 1474 { 1475 TDECL(); 1476 1477 if (!extiIsPendingGpio(T(int1))) 1478 return false; 1479 1480 lsm6dsm_readStatusReg(true); 1481 1482 extiClearPendingGpio(T(int1)); 1483 1484 return true; 1485 } 1486 1487 /* 1488 * lsm6dsm_enableInterrupt: enable driver interrupt capability 1489 * @pin: gpio data. 1490 * @isr: isr data. 1491 */ 1492 static void lsm6dsm_enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 1493 { 1494 gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE); 1495 syscfgSetExtiPort(pin); 1496 extiEnableIntGpio(pin, EXTI_TRIGGER_RISING); 1497 extiChainIsr(LSM6DSM_INT_IRQ, isr); 1498 } 1499 1500 /* 1501 * lsm6dsm_disableInterrupt: disable driver interrupt capability 1502 * @pin: gpio data. 1503 * @isr: isr data. 1504 */ 1505 static void lsm6dsm_disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr) 1506 { 1507 extiUnchainIsr(LSM6DSM_INT_IRQ, isr); 1508 extiDisableIntGpio(pin); 1509 } 1510 1511 /* 1512 * lsm6dsm_sendSelfTestResult: send to nanohub result of self-test 1513 * @sensorType: android sensor type. 1514 * @result: status message to send (PASS/ERROR). 1515 */ 1516 static void lsm6dsm_sendSelfTestResult(uint8_t sensorType, uint8_t result) 1517 { 1518 struct LSM6DSMSelfTestResultPkt *data; 1519 1520 data = heapAlloc(sizeof(struct LSM6DSMSelfTestResultPkt)); 1521 if (!data) { 1522 ERROR_PRINT("sendSelfTestResult: cannot allocate self-test result packet\n"); 1523 return; 1524 } 1525 1526 data->header.appId = LSM6DSM_APP_ID; 1527 data->header.dataLen = (sizeof(struct LSM6DSMSelfTestResultPkt) - sizeof(struct HostHubRawPacket)); 1528 1529 data->dataHeader.msgId = SENSOR_APP_MSG_ID_TEST_RESULT; 1530 data->dataHeader.sensorType = sensorType; 1531 data->dataHeader.status = result; 1532 1533 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) { 1534 ERROR_PRINT("sendSelfTestResult: failed to enqueue self-test result packet\n"); 1535 } 1536 } 1537 1538 /* 1539 * lsm6dsm_sendCalibrationResult: send to nanohub result of calibration 1540 * @sensorType: android sensor type. 1541 * @result: status message to send (VALID/ERROR). 1542 * @xBias: raw offset value X axis. 1543 * @yBias: raw offset value Y axis. 1544 * @zBias: raw offset value Z axis. 1545 */ 1546 static void lsm6dsm_sendCalibrationResult(uint8_t sensorType, uint8_t result, int32_t xBias, int32_t yBias, int32_t zBias) 1547 { 1548 struct LSM6DSMCalibrationResultPkt *data; 1549 1550 data = heapAlloc(sizeof(struct LSM6DSMCalibrationResultPkt)); 1551 if (!data) { 1552 ERROR_PRINT("sendCalibrationResult: cannot allocate calibration result packet\n"); 1553 return; 1554 } 1555 1556 data->header.appId = LSM6DSM_APP_ID; 1557 data->header.dataLen = (sizeof(struct LSM6DSMCalibrationResultPkt) - sizeof(struct HostHubRawPacket)); 1558 1559 data->dataHeader.msgId = SENSOR_APP_MSG_ID_CAL_RESULT; 1560 data->dataHeader.sensorType = sensorType; 1561 data->dataHeader.status = result; 1562 1563 data->xBias = xBias; 1564 data->yBias = yBias; 1565 data->zBias = zBias; 1566 1567 if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) { 1568 ERROR_PRINT("sendCalibrationResult: failed to enqueue calibration result packet\n"); 1569 } 1570 } 1571 1572 /* 1573 * lsm6dsm_runGapSelfTestProgram: state machine that is executing self-test verifying data gap 1574 * @idx: sensor driver index. 1575 */ 1576 static void lsm6dsm_runGapSelfTestProgram(enum SensorIndex idx) 1577 { 1578 TDECL(); 1579 uint8_t *sensorData, numberOfAverage; 1580 1581 numberOfAverage = LSM6DSM_NUM_AVERAGE_SELFTEST; 1582 1583 switch (T(selftestState)) { 1584 case SELFTEST_INITIALIZATION: 1585 DEBUG_PRINT("runGapSelfTestProgram: initialization\n"); 1586 1587 T(numSamplesSelftest) = 0; 1588 memset(T(dataSelftestEnabled), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 1589 memset(T(dataSelftestNotEnabled), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 1590 1591 /* Enable self-test & power on sensor */ 1592 switch (idx) { 1593 case ACCEL: 1594 SPI_WRITE(LSM6DSM_CTRL5_C_ADDR, LSM6DSM_CTRL5_C_BASE | LSM6DSM_ACCEL_SELFTEST_PS); 1595 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 30000); 1596 break; 1597 case GYRO: 1598 SPI_WRITE(LSM6DSM_CTRL5_C_ADDR, LSM6DSM_CTRL5_C_BASE | LSM6DSM_GYRO_SELFTEST_PS); 1599 SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 30000); 1600 break; 1601 #if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL) 1602 case MAGN: 1603 /* Enable accelerometer and sensor-hub */ 1604 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE); 1605 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON; 1606 1607 uint8_t rateIndex = ARRAY_SIZE(LSM6DSMSHRates) - 2; 1608 1609 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR 1610 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_B_M_ADDR, LSM303AGR_OFFSET_CANCELLATION, SENSOR_HZ(104.0f), MAGN); 1611 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_C_M_ADDR, 1612 LSM303AGR_CFG_REG_C_M_BASE | LSM303AGR_ENABLE_SELFTEST, SENSOR_HZ(104.0f), MAGN); 1613 1614 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR, 1615 LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE | 1616 LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(rateIndex), SENSOR_HZ(104.0f), MAGN, 200000); 1617 #else /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1618 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR, 1619 LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE, SENSOR_HZ(104.0f), MAGN); 1620 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR, 1621 LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(rateIndex) | LIS3MDL_ENABLE_SELFTEST, 1622 SENSOR_HZ(104.0f), MAGN); 1623 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1624 break; 1625 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */ 1626 default: 1627 return; 1628 } 1629 1630 T(selftestState) = SELFTEST_READ_EST_DATA; 1631 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1632 break; 1633 1634 case SELFTEST_READ_EST_DATA: 1635 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR 1636 if (idx == MAGN) 1637 numberOfAverage = LSM6DSM_NUM_AVERAGE_SELFTEST_SLOW; 1638 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1639 1640 if (T(numSamplesSelftest) > 0) { 1641 sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]); 1642 T(dataSelftestEnabled[0]) += (int16_t)*((uint16_t *)&sensorData[0]); 1643 T(dataSelftestEnabled[1]) += (int16_t)*((uint16_t *)&sensorData[2]); 1644 T(dataSelftestEnabled[2]) += (int16_t)*((uint16_t *)&sensorData[4]); 1645 } 1646 T(numSamplesSelftest)++; 1647 1648 if (T(numSamplesSelftest) <= numberOfAverage) { 1649 DEBUG_PRINT("runGapSelfTestProgram: reading output data while self-test is enabled\n"); 1650 1651 switch (idx) { 1652 case ACCEL: 1653 SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000); 1654 break; 1655 case GYRO: 1656 SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000); 1657 break; 1658 #if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL) 1659 case MAGN: 1660 SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 20000); 1661 break; 1662 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */ 1663 default: 1664 return; 1665 } 1666 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1667 break; 1668 } 1669 1670 T(dataSelftestEnabled[0]) /= numberOfAverage; 1671 T(dataSelftestEnabled[1]) /= numberOfAverage; 1672 T(dataSelftestEnabled[2]) /= numberOfAverage; 1673 T(selftestState) = SELFTEST_SECOND_STEP_INITIALIZATION; 1674 1675 case SELFTEST_SECOND_STEP_INITIALIZATION: 1676 DEBUG_PRINT("runGapSelfTestProgram: second step initialization\n"); 1677 1678 T(numSamplesSelftest) = 0; 1679 1680 /* Disable self-test */ 1681 switch (idx) { 1682 case ACCEL: 1683 case GYRO: 1684 SPI_WRITE(LSM6DSM_CTRL5_C_ADDR, LSM6DSM_CTRL5_C_BASE, 30000); 1685 break; 1686 #if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL) 1687 case MAGN: ; 1688 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR 1689 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_C_M_ADDR, LSM303AGR_CFG_REG_C_M_BASE, SENSOR_HZ(104.0f), MAGN, 200000); 1690 #else /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1691 uint8_t rateIndex = ARRAY_SIZE(LSM6DSMSHRates) - 2; 1692 1693 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR, 1694 LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(rateIndex), 1695 SENSOR_HZ(104.0f), MAGN); 1696 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1697 break; 1698 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */ 1699 default: 1700 return; 1701 } 1702 1703 T(selftestState) = SELFTEST_READ_NST_DATA; 1704 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1705 break; 1706 1707 case SELFTEST_READ_NST_DATA: 1708 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR 1709 if (idx == MAGN) 1710 numberOfAverage = LSM6DSM_NUM_AVERAGE_SELFTEST_SLOW; 1711 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1712 1713 if (T(numSamplesSelftest) > 0) { 1714 sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]); 1715 T(dataSelftestNotEnabled[0]) += (int16_t)*((uint16_t *)&sensorData[0]); 1716 T(dataSelftestNotEnabled[1]) += (int16_t)*((uint16_t *)&sensorData[2]); 1717 T(dataSelftestNotEnabled[2]) += (int16_t)*((uint16_t *)&sensorData[4]); 1718 } 1719 T(numSamplesSelftest)++; 1720 1721 if (T(numSamplesSelftest) <= numberOfAverage) { 1722 DEBUG_PRINT("runGapSelfTestProgram: reading output data while self-test is disabled\n"); 1723 1724 switch (idx) { 1725 case ACCEL: 1726 SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000); 1727 break; 1728 case GYRO: 1729 SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000); 1730 break; 1731 #if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL) 1732 case MAGN: 1733 SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 20000); 1734 break; 1735 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */ 1736 default: 1737 return; 1738 } 1739 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1740 break; 1741 } 1742 1743 T(dataSelftestNotEnabled[0]) /= numberOfAverage; 1744 T(dataSelftestNotEnabled[1]) /= numberOfAverage; 1745 T(dataSelftestNotEnabled[2]) /= numberOfAverage; 1746 T(selftestState) = SELFTEST_VERIFICATION; 1747 1748 case SELFTEST_VERIFICATION: ; 1749 uint8_t i, sType; 1750 int32_t dataGap[3]; 1751 bool testPassed = true; 1752 int32_t lower_threshold[3], higher_threshold[3]; 1753 1754 dataGap[0] = abs(T(dataSelftestEnabled[0]) - T(dataSelftestNotEnabled[0])); 1755 dataGap[1] = abs(T(dataSelftestEnabled[1]) - T(dataSelftestNotEnabled[1])); 1756 dataGap[2] = abs(T(dataSelftestEnabled[2]) - T(dataSelftestNotEnabled[2])); 1757 1758 switch (idx) { 1759 case ACCEL: 1760 sType = SENS_TYPE_ACCEL; 1761 lower_threshold[0] = lower_threshold[1] = lower_threshold[2] = LSM6DSM_ACCEL_SELFTEST_LOW_THR_LSB; 1762 higher_threshold[0] = higher_threshold[1] = higher_threshold[2] = LSM6DSM_ACCEL_SELFTEST_HIGH_THR_LSB; 1763 1764 /* Power off sensor */ 1765 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE); 1766 break; 1767 case GYRO: 1768 sType = SENS_TYPE_GYRO; 1769 lower_threshold[0] = lower_threshold[1] = lower_threshold[2] = LSM6DSM_GYRO_SELFTEST_LOW_THR_LSB; 1770 higher_threshold[0] = higher_threshold[1] = higher_threshold[2] = LSM6DSM_GYRO_SELFTEST_HIGH_THR_LSB; 1771 1772 /* Power off sensor */ 1773 SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE); 1774 break; 1775 #if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL) 1776 case MAGN: 1777 sType = SENS_TYPE_MAG; 1778 1779 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR 1780 lower_threshold[0] = lower_threshold[1] = lower_threshold[2] = LSM303AGR_SELFTEST_LOW_THR_LSB; 1781 higher_threshold[0] = higher_threshold[1] = higher_threshold[2] = LSM303AGR_SELFTEST_HIGH_THR_LSB; 1782 1783 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_B_M_ADDR, 0x00, SENSOR_HZ(104.0f), MAGN); 1784 #else /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1785 lower_threshold[0] = lower_threshold[1] = LIS3MDL_SELFTEST_LOW_THR_XY_LSB; 1786 higher_threshold[0] = higher_threshold[1] = LIS3MDL_SELFTEST_HIGH_THR_XY_LSB; 1787 lower_threshold[2] = LIS3MDL_SELFTEST_LOW_THR_Z_LSB; 1788 higher_threshold[2] = LIS3MDL_SELFTEST_HIGH_THR_Z_LSB; 1789 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */ 1790 1791 /* Power off sensor */ 1792 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR, 1793 LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN); 1794 1795 /* Disable accelerometer and sensor-hub */ 1796 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE); 1797 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE); 1798 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON; 1799 break; 1800 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */ 1801 default: 1802 return; 1803 } 1804 1805 for (i = 0; i < 3; i++) { 1806 if ((dataGap[i] < lower_threshold[i]) || (dataGap[i] > higher_threshold[i])) { 1807 testPassed = false; 1808 ERROR_PRINT("runGapSelfTestProgram: axis-%d out of spec! test-enabled: %ldLSB ** test-disabled: %ldLSB, ** delta: %ldLSB\n", 1809 i, T(dataSelftestEnabled[i]), T(dataSelftestNotEnabled[i]), dataGap[i]); 1810 } 1811 } 1812 INFO_PRINT("runGapSelfTestProgram: completed. Test result: %s\n", testPassed ? "pass" : "fail"); 1813 1814 if (testPassed) 1815 lsm6dsm_sendSelfTestResult(sType, SENSOR_APP_EVT_STATUS_SUCCESS); 1816 else 1817 lsm6dsm_sendSelfTestResult(sType, SENSOR_APP_EVT_STATUS_ERROR); 1818 1819 T(selftestState) = SELFTEST_COMPLETED; 1820 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1821 break; 1822 1823 default: 1824 break; 1825 } 1826 } 1827 1828 /* 1829 * lsm6dsm_convertAccelOffsetValue: convert accel LSB value to offset digit 1830 * @val: LSB axis offset value. 1831 */ 1832 static uint8_t lsm6dsm_convertAccelOffsetValue(int32_t val) 1833 { 1834 float temp; 1835 1836 temp = val * LSM6DSM_ACCEL_LSB_TO_OFFSET_DIGIT_SCALE; 1837 if (temp > LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB) 1838 temp = LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB; 1839 1840 if (temp < -LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB) 1841 temp = -LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB; 1842 1843 return (uint8_t)((int8_t)temp); 1844 } 1845 1846 /* 1847 * lsm6dsm_runCalibrationProgram: state machine that is executing calibration 1848 * @idx: sensor driver index. 1849 */ 1850 static void lsm6dsm_runCalibrationProgram(enum SensorIndex idx) 1851 { 1852 TDECL(); 1853 uint8_t *sensorData, numberOfAverage; 1854 uint8_t buffer[LSM6DSM_TRIAXIAL_NUM_AXIS] = { 0 }; 1855 1856 numberOfAverage = LSM6DSM_NUM_AVERAGE_CALIBRATION; 1857 1858 switch (T(calibrationState)) { 1859 case CALIBRATION_INITIALIZATION: 1860 DEBUG_PRINT("runCalibrationProgram: initialization\n"); 1861 1862 T(numSamplesCalibration) = 0; 1863 memset(T(dataCalibration), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 1864 1865 /* Power on sensor */ 1866 switch (idx) { 1867 case ACCEL: 1868 SPI_MULTIWRITE(LSM6DSM_X_OFS_USR_ADDR, buffer, LSM6DSM_TRIAXIAL_NUM_AXIS, 500); 1869 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 30000); 1870 break; 1871 case GYRO: 1872 SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 100000); 1873 break; 1874 default: 1875 return; 1876 } 1877 1878 T(calibrationState) = CALIBRATION_READ_DATA; 1879 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1880 break; 1881 1882 case CALIBRATION_READ_DATA: 1883 if (T(numSamplesCalibration) > 0) { 1884 sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]); 1885 T(dataCalibration[0]) += (int16_t)*((uint16_t *)&sensorData[0]); 1886 T(dataCalibration[1]) += (int16_t)*((uint16_t *)&sensorData[2]); 1887 T(dataCalibration[2]) += (int16_t)*((uint16_t *)&sensorData[4]); 1888 } 1889 T(numSamplesCalibration)++; 1890 1891 if (T(numSamplesCalibration) <= numberOfAverage) { 1892 DEBUG_PRINT("runCalibrationProgram: reading output data\n"); 1893 1894 switch (idx) { 1895 case ACCEL: 1896 SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000); 1897 break; 1898 case GYRO: 1899 SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000); 1900 break; 1901 default: 1902 return; 1903 } 1904 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1905 break; 1906 } 1907 1908 T(dataCalibration[0]) /= numberOfAverage; 1909 T(dataCalibration[1]) /= numberOfAverage; 1910 T(dataCalibration[2]) /= numberOfAverage; 1911 T(calibrationState) = CALIBRATION_VERIFICATION; 1912 1913 case CALIBRATION_VERIFICATION: ; 1914 uint8_t sType; 1915 1916 switch (idx) { 1917 case ACCEL: 1918 sType = SENS_TYPE_ACCEL; 1919 1920 /* Power off sensor */ 1921 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE); 1922 1923 /* Supposed 0,0,1g (Android coordinate system) */ 1924 T(dataCalibration[0]) = -T(dataCalibration[0]); 1925 T(dataCalibration[1]) = -T(dataCalibration[1]); 1926 T(dataCalibration[2]) = T(dataCalibration[2]) - LSM6DSM_1G_IN_LSB_CALIBRATION; 1927 1928 for (int8_t i = 0; i < LSM6DSM_TRIAXIAL_NUM_AXIS; i++) 1929 buffer[i] = lsm6dsm_convertAccelOffsetValue(T(dataCalibration[i])); 1930 1931 SPI_MULTIWRITE(LSM6DSM_X_OFS_USR_ADDR, buffer, LSM6DSM_TRIAXIAL_NUM_AXIS); 1932 break; 1933 case GYRO: 1934 sType = SENS_TYPE_GYRO; 1935 1936 /* Power off sensor */ 1937 SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE); 1938 1939 memcpy(T(gyroCalibrationData), T(dataCalibration), LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 1940 break; 1941 default: 1942 return; 1943 } 1944 1945 INFO_PRINT("runCalibrationProgram: completed. offset [LSB]: %ld %ld %ld\n", T(dataCalibration[0]), T(dataCalibration[1]), T(dataCalibration[2])); 1946 lsm6dsm_sendCalibrationResult(sType, SENSOR_APP_EVT_STATUS_SUCCESS, T(dataCalibration[0]), T(dataCalibration[1]), T(dataCalibration[2])); 1947 1948 T(calibrationState) = CALIBRATION_COMPLETED; 1949 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__); 1950 break; 1951 1952 default: 1953 break; 1954 } 1955 } 1956 1957 1958 #ifdef LSM6DSM_I2C_MASTER_AK09916 1959 /* 1960 * lsm6dsm_runAbsoluteSelfTestProgram: state machine that is executing self-test verifying absolute value 1961 */ 1962 static void lsm6dsm_runAbsoluteSelfTestProgram(void) 1963 { 1964 TDECL(); 1965 uint8_t *sensorData; 1966 1967 switch (T(selftestState)) { 1968 case SELFTEST_INITIALIZATION: ; 1969 DEBUG_PRINT("runAbsoluteSelfTestProgram: initialization\n"); 1970 1971 T(numSamplesSelftest) = 0; 1972 memset(T(dataSelftestEnabled), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 1973 1974 /* Enable accelerometer and sensor-hub */ 1975 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE); 1976 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON; 1977 1978 SPI_WRITE_SLAVE_SENSOR_REGISTER(AK09916_CNTL2_ADDR, AK09916_ENABLE_SELFTEST_MODE, SENSOR_HZ(104.0f), MAGN, 20000); 1979 1980 T(selftestState) = SELFTEST_READ_EST_DATA; 1981 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__); 1982 break; 1983 1984 case SELFTEST_READ_EST_DATA: 1985 if (T(numSamplesSelftest) > 0) { 1986 sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]); 1987 T(dataSelftestEnabled[0]) += (int16_t)*((uint16_t *)&sensorData[0]); 1988 T(dataSelftestEnabled[1]) += (int16_t)*((uint16_t *)&sensorData[2]); 1989 T(dataSelftestEnabled[2]) += (int16_t)*((uint16_t *)&sensorData[4]); 1990 } 1991 T(numSamplesSelftest)++; 1992 1993 if (T(numSamplesSelftest) <= LSM6DSM_NUM_AVERAGE_SELFTEST) { 1994 DEBUG_PRINT("runAbsoluteSelfTestProgram: reading output data while self-test is enabled\n"); 1995 1996 SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 20000); 1997 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__); 1998 break; 1999 } 2000 2001 T(dataSelftestEnabled[0]) /= LSM6DSM_NUM_AVERAGE_SELFTEST; 2002 T(dataSelftestEnabled[1]) /= LSM6DSM_NUM_AVERAGE_SELFTEST; 2003 T(dataSelftestEnabled[2]) /= LSM6DSM_NUM_AVERAGE_SELFTEST; 2004 T(selftestState) = SELFTEST_VERIFICATION; 2005 2006 case SELFTEST_VERIFICATION: ; 2007 bool testPassed = true; 2008 2009 if ((T(dataSelftestEnabled[0]) < AK09916_SELFTEST_LOW_THR_XY_LSB) || 2010 (T(dataSelftestEnabled[0]) > AK09916_SELFTEST_HIGH_THR_XYZ_LSB)) { 2011 testPassed = false; 2012 ERROR_PRINT("runAbsoluteSelfTestProgram: axis-0 out of spec! Read: %ldLSB\n", T(dataSelftestEnabled[0])); 2013 } 2014 if ((T(dataSelftestEnabled[1]) < AK09916_SELFTEST_LOW_THR_XY_LSB) || 2015 (T(dataSelftestEnabled[1]) > AK09916_SELFTEST_HIGH_THR_XYZ_LSB)) { 2016 testPassed = false; 2017 ERROR_PRINT("runAbsoluteSelfTestProgram: axis-1 out of spec! Read: %ldLSB\n", T(dataSelftestEnabled[1])); 2018 } 2019 if ((T(dataSelftestEnabled[2]) < AK09916_SELFTEST_LOW_THR_Z_LSB) || 2020 (T(dataSelftestEnabled[2]) > AK09916_SELFTEST_HIGH_THR_XYZ_LSB)) { 2021 testPassed = false; 2022 ERROR_PRINT("runAbsoluteSelfTestProgram: axis-2 out of spec! Read: %ldLSB\n", T(dataSelftestEnabled[2])); 2023 } 2024 2025 INFO_PRINT("runAbsoluteSelfTestProgram: completed. Test result: %s\n", testPassed ? "pass" : "fail"); 2026 2027 if (testPassed) 2028 lsm6dsm_sendSelfTestResult(SENS_TYPE_MAG, SENSOR_APP_EVT_STATUS_SUCCESS); 2029 else 2030 lsm6dsm_sendSelfTestResult(SENS_TYPE_MAG, SENSOR_APP_EVT_STATUS_ERROR); 2031 2032 /* Disable accelerometer and sensor-hub */ 2033 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE); 2034 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE); 2035 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON; 2036 2037 T(selftestState) = SELFTEST_COMPLETED; 2038 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__); 2039 break; 2040 2041 default: 2042 break; 2043 } 2044 } 2045 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 2046 2047 /* 2048 * lsm6dsm_writeEmbeddedRegister: write embedded register 2049 * @addr: address of register to be written. 2050 * @value: value to write. 2051 */ 2052 static void lsm6dsm_writeEmbeddedRegister(uint8_t addr, uint8_t value) 2053 { 2054 TDECL(); 2055 2056 #ifdef LSM6DSM_I2C_MASTER_ENABLED 2057 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE); 2058 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 2059 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000); 2060 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50); 2061 2062 SPI_WRITE(addr, value); 2063 2064 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50); 2065 #ifdef LSM6DSM_I2C_MASTER_ENABLED 2066 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2067 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 2068 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister)); 2069 } 2070 2071 #ifdef LSM6DSM_I2C_MASTER_ENABLED 2072 /* 2073 * lsm6dsm_writeSlaveRegister: write I2C slave register using sensor-hub feature 2074 * @addr: address of register to be written. 2075 * @value: value to write. 2076 * @accelRate: sensor-hub is using accel odr as trigger. This is current accel odr value. 2077 * @delay: perform a delay after write is completed. 2078 * @si: which slave sensor needs to be written. 2079 */ 2080 static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si) 2081 { 2082 TDECL(); 2083 uint8_t slave_addr, buffer[2]; 2084 uint32_t SHOpCompleteTime; 2085 2086 switch (si) { 2087 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 2088 case MAGN: 2089 slave_addr = LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT; 2090 break; 2091 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 2092 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 2093 case PRESS: 2094 case TEMP: 2095 slave_addr = LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT; 2096 break; 2097 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 2098 default: 2099 return; 2100 } 2101 2102 if (accelRate > SENSOR_HZ(104.0f)) 2103 SHOpCompleteTime = SENSOR_HZ_RATE_TO_US(SENSOR_HZ(104.0f)); 2104 else 2105 SHOpCompleteTime = SENSOR_HZ_RATE_TO_US(accelRate); 2106 2107 /* Perform write to slave sensor and wait write is done (1 accel ODR) */ 2108 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE); 2109 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000); 2110 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50); 2111 2112 buffer[0] = slave_addr << 1; /* LSM6DSM_EMBEDDED_SLV0_ADDR */ 2113 buffer[1] = addr; /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */ 2114 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 2); 2115 SPI_WRITE(LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR, value); 2116 2117 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50); 2118 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister) | LSM6DSM_MASTER_CONFIG_MASTER_ON); 2119 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister), (3 * SHOpCompleteTime) / 2); 2120 2121 /* After write is completed slave 0 must be set to sleep mode */ 2122 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE); 2123 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000); 2124 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50); 2125 2126 buffer[0] = LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP; /* LSM6DSM_EMBEDDED_SLV0_ADDR */ 2127 buffer[1] = addr; /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */ 2128 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 2); 2129 2130 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50); 2131 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2132 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister), delay); 2133 } 2134 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 2135 2136 /* 2137 * lsm6dsm_computeOdr: get index of LSM6DSMImuRates array based on selected rate 2138 * @rate: ODR value expressed in SENSOR_HZ(x). 2139 */ 2140 static uint8_t lsm6dsm_computeOdr(uint32_t rate) 2141 { 2142 int i; 2143 2144 for (i = 0; i < (ARRAY_SIZE(LSM6DSMImuRates) - 1); i++) { 2145 if (LSM6DSMImuRates[i] == rate) 2146 break; 2147 } 2148 if (i == (ARRAY_SIZE(LSM6DSMImuRates) - 1)) { 2149 ERROR_PRINT("computeOdr: ODR not valid! Selected smallest ODR available\n"); 2150 i = 0; 2151 } 2152 2153 return i; 2154 } 2155 2156 /* 2157 * lsm6dsm_sensorHzToNs: return delta time of specifi sensor rate 2158 * @rate: sensor rate expressed in SENSOR_HZ(x). 2159 */ 2160 static uint32_t lsm6dsm_sensorHzToNs(uint32_t rate) 2161 { 2162 int i; 2163 2164 for (i = 0; i < (ARRAY_SIZE(LSM6DSMImuRates) - 1); i++) { 2165 if (LSM6DSMImuRates[i] == rate) 2166 break; 2167 } 2168 if (i == (ARRAY_SIZE(LSM6DSMImuRates) - 1)) { 2169 ERROR_PRINT("sensorHzToNs: rate not available. Selected smaller rate\n"); 2170 i = 0; 2171 } 2172 2173 return LSM6DSMImuRatesInNs[i]; 2174 } 2175 2176 /* 2177 * lsm6dsm_decimatorToFifoDecimatorReg: get decimator reg value based on decimation factor 2178 * @dec: FIFO sample decimation factor. 2179 */ 2180 static uint8_t lsm6dsm_decimatorToFifoDecimatorReg(uint8_t dec) 2181 { 2182 uint8_t regValue; 2183 2184 switch (dec) { 2185 case 1: 2186 regValue = LSM6DSM_FIFO_NO_DECIMATION; 2187 break; 2188 case 2: 2189 regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_2; 2190 break; 2191 case 3: 2192 regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_3; 2193 break; 2194 case 4: 2195 regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_4; 2196 break; 2197 case 8: 2198 regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_8; 2199 break; 2200 case 16: 2201 regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_16; 2202 break; 2203 case 32: 2204 regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_32; 2205 break; 2206 default: 2207 regValue = LSM6DSM_FIFO_SAMPLE_NOT_IN_FIFO; 2208 break; 2209 } 2210 2211 return regValue; 2212 } 2213 2214 /* 2215 * lsm6dsm_calculateFifoDecimators: calculate fifo decimators 2216 * @RequestedRate: list of ODRs requested by driver for each sensor in FIFO. 2217 * @minLatency: the function will set the min latency based on all sensors enabled in FIFO. 2218 */ 2219 static bool lsm6dsm_calculateFifoDecimators(uint32_t RequestedRate[FIFO_NUM], uint64_t *minLatency) 2220 { 2221 TDECL(); 2222 uint8_t i, n, tempDec, decimators[FIFO_NUM] = { 0 }, minDec = UINT8_MAX, maxDec = 0; 2223 enum SensorIndex sidx; 2224 bool changed = false; 2225 2226 T(fifoCntl).triggerRate = T(sensors[ACCEL]).hwRate; 2227 if (T(sensors[GYRO]).hwRate > T(fifoCntl).triggerRate) 2228 T(fifoCntl).triggerRate = T(sensors[GYRO]).hwRate; 2229 2230 for (i = 0; i < FIFO_NUM; i++) { 2231 sidx = T(fifoCntl).decimatorsIdx[i]; 2232 if (sidx >= NUM_SENSORS) 2233 continue; 2234 2235 if (T(sensors[i]).latency < *minLatency) 2236 *minLatency = T(sensors[i]).latency; 2237 } 2238 2239 for (i = 0; i < FIFO_NUM; i++) { 2240 sidx = T(fifoCntl).decimatorsIdx[i]; 2241 if (sidx >= NUM_SENSORS) 2242 continue; 2243 2244 if (RequestedRate[i]) { 2245 decimators[i] = (T(fifoCntl).triggerRate / RequestedRate[i]) <= 32 ? (T(fifoCntl).triggerRate / RequestedRate[i]) : 32; 2246 2247 tempDec = decimators[i]; 2248 while (decimators[i] > 1) { 2249 if (((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * decimators[i]) > *minLatency) 2250 decimators[i] /= 2; 2251 else 2252 break; 2253 } 2254 T(sensors[sidx]).samplesFifoDecimator = tempDec / decimators[i]; 2255 T(sensors[sidx]).samplesFifoDecimatorCounter = T(sensors[sidx]).samplesFifoDecimator - 1; 2256 2257 if (decimators[i] < minDec) 2258 minDec = decimators[i]; 2259 2260 if (decimators[i] > maxDec) 2261 maxDec = decimators[i]; 2262 } 2263 2264 DEBUG_PRINT("calculateFifoDecimators: sensorIndex=%d, fifo decimator=%d, software decimation=%d\n", sidx, decimators[i], T(sensors[sidx]).samplesFifoDecimator); 2265 2266 if (T(fifoCntl).decimators[i] != decimators[i]) { 2267 T(fifoCntl).decimators[i] = decimators[i]; 2268 changed = true; 2269 } 2270 } 2271 2272 /* Embedded timestamp slot */ 2273 T(fifoCntl).decimators[FIFO_DS4] = minDec; 2274 2275 T(fifoCntl).minDecimator = minDec; 2276 T(fifoCntl).maxDecimator = maxDec; 2277 T(fifoCntl).maxMinDecimator = maxDec / minDec; 2278 T(fifoCntl).totalSip = 0; 2279 2280 if (maxDec > 0) { 2281 T(time).theoreticalDeltaTimeLSB = cpuMathU64DivByU16((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).minDecimator, LSM6DSM_TIME_RESOLUTION); 2282 T(time).deltaTimeMarginLSB = ((T(time).theoreticalDeltaTimeLSB) * 10) / 100; 2283 2284 for (i = 0; i < FIFO_NUM; i++) { 2285 if (T(fifoCntl).decimators[i] > 0) 2286 T(fifoCntl).totalSip += (maxDec / T(fifoCntl).decimators[i]); 2287 } 2288 } 2289 2290 DEBUG_PRINT("calculateFifoDecimators: samples in pattern=%d\n", T(fifoCntl).totalSip); 2291 2292 for (i = 0; i < T(fifoCntl).maxMinDecimator; i++) { 2293 T(fifoCntl).timestampPosition[i] = 0; 2294 for (n = 0; n < FIFO_NUM - 1; n++) { 2295 if ((T(fifoCntl).decimators[n] > 0) && ((i % (T(fifoCntl).decimators[n] / T(fifoCntl).minDecimator)) == 0)) 2296 T(fifoCntl).timestampPosition[i] += LSM6DSM_ONE_SAMPLE_BYTE; 2297 } 2298 } 2299 2300 return changed; 2301 } 2302 2303 /* 2304 * lsm6dsm_calculateWatermark: calculate fifo watermark level 2305 * @minLatency: min latency requested by system based on all sensors in FIFO. 2306 */ 2307 static bool lsm6dsm_calculateWatermark(uint64_t *minLatency) 2308 { 2309 TDECL(); 2310 uint64_t patternRate, tempLatency; 2311 uint16_t watermark; 2312 uint16_t i = 1; 2313 2314 if (T(fifoCntl).totalSip > 0) { 2315 patternRate = (uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator; 2316 2317 do { 2318 tempLatency = patternRate * (++i); 2319 } while ((tempLatency < *minLatency) && (i <= LSM6DSM_MAX_WATERMARK_VALUE)); 2320 2321 watermark = (i - 1) * T(fifoCntl).totalSip; 2322 2323 while (watermark > LSM6DSM_MAX_WATERMARK_VALUE) { 2324 watermark /= 2; 2325 watermark = watermark - (watermark % T(fifoCntl).totalSip); 2326 } 2327 2328 DEBUG_PRINT("calculateWatermark: level=#%d, min latency=%lldns\n", watermark, *minLatency); 2329 2330 if (T(fifoCntl).watermark != watermark) { 2331 T(fifoCntl).watermark = watermark; 2332 return true; 2333 } 2334 } 2335 2336 return false; 2337 } 2338 2339 /* 2340 * lsm6dsm_resetTimestampSync: reset all variables used by sync timestamp task 2341 */ 2342 static inline void lsm6dsm_resetTimestampSync(void) 2343 { 2344 TDECL(); 2345 2346 T(lastFifoReadTimestamp) = 0; 2347 2348 T(time).sampleTimestampFromFifoLSB = 0; 2349 T(time).timestampIsValid = false; 2350 T(time).lastSampleTimestamp = 0; 2351 T(time).noTimer.lastTimestampDataAvlRtcTime = 0; 2352 T(time).noTimer.newTimestampDataAvl = false; 2353 T(time).timestampSyncTaskLSB = 0; 2354 2355 time_sync_reset(&T(time).sensorTimeToRtcData); 2356 } 2357 2358 /* 2359 * lsm6dsm_updateSyncTaskMode: set the best way to sync timestamp 2360 * @minLatency: min latency of sensors using FIFO. 2361 */ 2362 static inline void lsm6dsm_updateSyncTaskMode(uint64_t *minLatency) 2363 { 2364 TDECL(); 2365 2366 /* If minLatency is `small` do not use timer to read timestamp and 2367 temperature but read it during FIFO read. */ 2368 if (*minLatency < LSM6DSM_SYNC_DELTA_INTERVAL) { 2369 T(time).status = TIME_SYNC_DURING_FIFO_READ; 2370 } else { 2371 T(time).status = TIME_SYNC_TIMER; 2372 2373 if (!osEnqueuePrivateEvt(EVT_TIME_SYNC, 0, NULL, mTask.tid)) { 2374 T(pendingTimeSyncTask) = true; 2375 ERROR_PRINT("updateSyncTaskMode: failed to enqueue time sync event\n"); 2376 } 2377 } 2378 } 2379 2380 /* 2381 * lsm6dsm_updateOdrs: update ODRs based on rates 2382 */ 2383 static bool lsm6dsm_updateOdrs(void) 2384 { 2385 TDECL(); 2386 bool accelOdrChanged = false, gyroOdrChanged = false, decChanged, watermarkChanged, gyroFirstEnable = false; 2387 uint32_t maxRate, maxPushDataRate[FIFO_NUM] = { 0 }; 2388 uint64_t minLatency = UINT64_MAX; 2389 uint8_t i, regValue, buffer[5]; 2390 uint16_t watermarkReg; 2391 2392 maxRate = 0; 2393 2394 /* Verify accel odr */ 2395 for (i = 0; i < NUM_SENSORS; i++) { 2396 if (T(sensors[ACCEL]).rate[i] > maxRate) 2397 maxRate = T(sensors[ACCEL]).rate[i] < SENSOR_HZ(26.0f / 2.0f) ? SENSOR_HZ(26.0f / 2.0f) : T(sensors[ACCEL]).rate[i]; 2398 2399 if ((T(sensors[ACCEL]).rate[i] > maxPushDataRate[FIFO_ACCEL]) && T(sensors[ACCEL]).dependenciesRequireData[i]) 2400 maxPushDataRate[FIFO_ACCEL] = T(sensors[ACCEL]).rate[i]; 2401 } 2402 if (T(sensors[ACCEL]).hwRate != maxRate) { 2403 T(sensors[ACCEL]).hwRate = maxRate; 2404 accelOdrChanged = true; 2405 } 2406 2407 maxRate = 0; 2408 2409 /* Verify gyro odr */ 2410 for (i = 0; i < NUM_SENSORS; i++) { 2411 if (T(sensors[GYRO]).rate[i] > maxRate) 2412 maxRate = T(sensors[GYRO]).rate[i] < SENSOR_HZ(26.0f / 2.0f) ? SENSOR_HZ(26.0f / 2.0f) : T(sensors[GYRO]).rate[i]; 2413 2414 if (T(sensors[GYRO]).rate[i] > maxPushDataRate[FIFO_GYRO]) 2415 maxPushDataRate[FIFO_GYRO] = T(sensors[GYRO]).rate[i]; 2416 } 2417 if (T(sensors[GYRO]).hwRate != maxRate) { 2418 /* If gyro is enabled from PowerDown more samples needs to be discarded */ 2419 if (T(sensors[GYRO]).hwRate == 0) 2420 gyroFirstEnable = true; 2421 2422 T(sensors[GYRO]).hwRate = maxRate; 2423 gyroOdrChanged = true; 2424 } 2425 2426 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 2427 /* If magnetometer is enabled, FIFO is used for it */ 2428 maxPushDataRate[FIFO_DS3] = T(sensors[MAGN]).rate[MAGN]; 2429 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 2430 2431 #if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 2432 /* If magnetometer is not available FIFO can be used to store barometer sensor data */ 2433 maxPushDataRate[FIFO_DS3] = T(sensors[PRESS]).rate[PRESS] > T(sensors[TEMP]).rate[TEMP] ? T(sensors[PRESS]).rate[PRESS] : T(sensors[TEMP]).rate[TEMP]; 2434 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED, LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 2435 2436 decChanged = lsm6dsm_calculateFifoDecimators(maxPushDataRate, &minLatency); 2437 watermarkChanged = lsm6dsm_calculateWatermark(&minLatency); 2438 watermarkReg = T(fifoCntl).watermark * 3; 2439 2440 if (accelOdrChanged || gyroOdrChanged || decChanged) { 2441 /* read all FIFO content and disable it */ 2442 DEBUG_PRINT("updateOdrs: disabling FIFO\n"); 2443 T(time).status = TIME_SYNC_DISABLED; 2444 2445 SPI_WRITE(LSM6DSM_TIMESTAMP2_REG_ADDR, LSM6DSM_TIMESTAMP2_REG_RESET_TIMESTAMP); 2446 SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_BYPASS_MODE, 25); 2447 } 2448 2449 if (accelOdrChanged) { 2450 if (T(sensors[ACCEL]).hwRate == 0) { 2451 DEBUG_PRINT("updateOdrs: no one is using accel, disabling it\n"); 2452 regValue = 0; 2453 } else { 2454 DEBUG_PRINT("updateOdrs: accel in use, updating odr to %dHz\n", (int)(T(sensors[ACCEL]).hwRate / 1024)); 2455 i = lsm6dsm_computeOdr(T(sensors[ACCEL]).hwRate); 2456 regValue = LSM6DSMImuRatesRegValue[i]; 2457 T(sensors[ACCEL]).samplesToDiscard = LSM6DSMAccelRatesSamplesToDiscard[i] / 2458 (T(sensors[ACCEL]).hwRate / (T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_ACCEL])); 2459 2460 if (T(sensors[ACCEL]).samplesToDiscard == 0) 2461 T(sensors[ACCEL]).samplesToDiscard = 1; 2462 2463 T(sensors[ACCEL]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_ACCEL]) / 2464 T(sensors[ACCEL]).samplesFifoDecimator) / T(sensors[ACCEL]).rate[ACCEL]; 2465 T(sensors[ACCEL]).samplesDecimatorCounter = T(sensors[ACCEL]).samplesDecimator - 1; 2466 } 2467 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | regValue, 30); 2468 } 2469 2470 if (gyroOdrChanged) { 2471 if (T(sensors[GYRO]).hwRate == 0) { 2472 DEBUG_PRINT("updateOdrs: no one is using gyro, disabling it\n"); 2473 regValue = 0; 2474 } else { 2475 DEBUG_PRINT("updateOdrs: gyro in use, updating odr to %dHz\n", (int)(T(sensors[GYRO]).hwRate / 1024)); 2476 i = lsm6dsm_computeOdr(T(sensors[GYRO]).hwRate); 2477 regValue = LSM6DSMImuRatesRegValue[i]; 2478 T(sensors[GYRO]).samplesToDiscard = LSM6DSMGyroRatesSamplesToDiscard[i]; 2479 2480 if (gyroFirstEnable) 2481 T(sensors[GYRO]).samplesToDiscard += LSM6DSMRatesSamplesToDiscardGyroPowerOn[i]; 2482 2483 T(sensors[GYRO]).samplesToDiscard /= (T(sensors[GYRO]).hwRate / (T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_GYRO])); 2484 2485 if (T(sensors[GYRO]).samplesToDiscard == 0) 2486 T(sensors[GYRO]).samplesToDiscard = 1; 2487 2488 T(sensors[GYRO]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_GYRO]) / 2489 T(sensors[GYRO]).samplesFifoDecimator) / T(sensors[GYRO]).rate[GYRO]; 2490 T(sensors[GYRO]).samplesDecimatorCounter = T(sensors[GYRO]).samplesDecimator - 1; 2491 } 2492 SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | regValue, 30); 2493 } 2494 2495 /* Program Fifo and enable or disable it */ 2496 if (accelOdrChanged || gyroOdrChanged || decChanged) { 2497 buffer[0] = *((uint8_t *)&watermarkReg); 2498 buffer[1] = (*((uint8_t *)&watermarkReg + 1) & LSM6DSM_FIFO_CTRL2_FTH_MASK) | LSM6DSM_ENABLE_FIFO_TIMESTAMP; 2499 buffer[2] = (lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_GYRO]) << 3) | 2500 lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_ACCEL]); 2501 buffer[3] = (lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_DS4]) << 3) | 2502 lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_DS3]); 2503 2504 for (i = 0; i < FIFO_NUM - 1; i++) { 2505 if (T(fifoCntl).decimators[i] > 0) 2506 break; 2507 } 2508 if (i < (FIFO_NUM - 1)) { 2509 /* Someone want to use FIFO */ 2510 DEBUG_PRINT("updateOdrs: enabling FIFO in continuos mode\n"); 2511 buffer[4] = LSM6DSM_FIFO_CONTINUOS_MODE; 2512 2513 lsm6dsm_resetTimestampSync(); 2514 lsm6dsm_updateSyncTaskMode(&minLatency); 2515 } else { 2516 /* No one is using FIFO */ 2517 buffer[4] = LSM6DSM_FIFO_BYPASS_MODE; 2518 2519 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 2520 if ((T(sensors[PRESS]).rate[PRESS] > 0) || (T(sensors[TEMP]).rate[TEMP] > 0)) { 2521 uint64_t latencyOnlyBaro = LSM6DSM_SYNC_DELTA_INTERVAL; 2522 lsm6dsm_resetTimestampSync(); 2523 lsm6dsm_updateSyncTaskMode(&latencyOnlyBaro); 2524 } 2525 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 2526 } 2527 2528 SPI_MULTIWRITE(LSM6DSM_FIFO_CTRL1_ADDR, buffer, 5); 2529 } else { 2530 if (watermarkChanged) { 2531 lsm6dsm_updateSyncTaskMode(&minLatency); 2532 2533 buffer[0] = *((uint8_t *)&watermarkReg); 2534 buffer[1] = (*((uint8_t *)&watermarkReg + 1) & LSM6DSM_FIFO_CTRL2_FTH_MASK) | LSM6DSM_ENABLE_FIFO_TIMESTAMP; 2535 SPI_MULTIWRITE(LSM6DSM_FIFO_CTRL1_ADDR, buffer, 2); 2536 } 2537 } 2538 2539 if (accelOdrChanged || gyroOdrChanged || decChanged || watermarkChanged) 2540 return true; 2541 2542 return false; 2543 } 2544 2545 /* 2546 * lsm6dsm_setAccelPower: enable/disable accelerometer sensor 2547 * @on: enable or disable sensor. 2548 * @cookie: private data. 2549 */ 2550 static bool lsm6dsm_setAccelPower(bool on, void *cookie) 2551 { 2552 TDECL(); 2553 2554 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2555 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2556 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2557 INFO_PRINT("setAccelPower: %s\n", on ? "enable" : "disable"); 2558 2559 if (on) 2560 osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[ACCEL]), NULL, mTask.tid); 2561 else { 2562 T(sensors[ACCEL]).rate[ACCEL] = 0; 2563 T(sensors[ACCEL]).latency = UINT64_MAX; 2564 2565 if (lsm6dsm_updateOdrs()) 2566 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__); 2567 else 2568 osEnqueuePrivateEvt(EVT_SENSOR_POWERING_DOWN, &T(sensors[ACCEL]), NULL, mTask.tid); 2569 } 2570 } else { 2571 T(pendingEnableConfig[ACCEL]) = true; 2572 T(sensors[ACCEL]).pConfig.enable = on; 2573 } 2574 2575 return true; 2576 } 2577 2578 /* 2579 * lsm6dsm_setGyroPower: enable/disable gyroscope sensor 2580 * @on: enable or disable sensor. 2581 * @cookie: private data. 2582 */ 2583 static bool lsm6dsm_setGyroPower(bool on, void *cookie) 2584 { 2585 TDECL(); 2586 2587 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2588 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2589 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2590 INFO_PRINT("setGyroPower: %s\n", on ? "enable" : "disable"); 2591 2592 if (on) 2593 osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[GYRO]), NULL, mTask.tid); 2594 else { 2595 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 2596 T(sensors[ACCEL]).rate[GYRO] = 0; 2597 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 2598 T(sensors[GYRO]).rate[GYRO] = 0; 2599 T(sensors[GYRO]).latency = UINT64_MAX; 2600 2601 if (lsm6dsm_updateOdrs()) { 2602 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__); 2603 } else 2604 osEnqueuePrivateEvt(EVT_SENSOR_POWERING_DOWN, &T(sensors[GYRO]), NULL, mTask.tid); 2605 } 2606 } else { 2607 T(pendingEnableConfig[GYRO]) = true; 2608 T(sensors[GYRO]).pConfig.enable = on; 2609 } 2610 2611 return true; 2612 } 2613 2614 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 2615 /* 2616 * lsm6dsm_setMagnPower: enable/disable magnetometer sensor 2617 * @on: enable or disable sensor. 2618 * @cookie: private data. 2619 */ 2620 static bool lsm6dsm_setMagnPower(bool on, void *cookie) 2621 { 2622 TDECL(); 2623 2624 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2625 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2626 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2627 INFO_PRINT("setMagnPower: %s\n", on ? "enable" : "disable"); 2628 2629 if (on) { 2630 if (T(masterConfigDependencies) != 0) { 2631 T(masterConfigDependencies) |= BIT(MAGN); 2632 2633 osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[MAGN]), NULL, mTask.tid); 2634 } else { 2635 T(masterConfigDependencies) |= BIT(MAGN); 2636 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON; 2637 2638 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2639 2640 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__); 2641 } 2642 } else { 2643 T(masterConfigDependencies) &= ~BIT(MAGN); 2644 2645 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR, 2646 LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, 2647 T(sensors[ACCEL]).hwRate, MAGN); 2648 2649 if (T(masterConfigDependencies) == 0) { 2650 DEBUG_PRINT("setMagnPower: no sensors enabled on i2c master, disabling it\n"); 2651 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON; 2652 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2653 } 2654 2655 T(sensors[ACCEL]).rate[MAGN] = 0; 2656 T(sensors[MAGN]).rate[MAGN] = 0; 2657 T(sensors[MAGN]).latency = UINT64_MAX; 2658 T(sensors[MAGN]).hwRate = 0; 2659 2660 lsm6dsm_updateOdrs(); 2661 2662 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__); 2663 } 2664 } else { 2665 T(pendingEnableConfig[MAGN]) = true; 2666 T(sensors[MAGN]).pConfig.enable = on; 2667 } 2668 2669 return true; 2670 } 2671 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 2672 2673 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 2674 /* 2675 * lsm6dsm_setPressPower: enable/disable pressure sensor 2676 * @on: enable or disable sensor. 2677 * @cookie: private data. 2678 */ 2679 static bool lsm6dsm_setPressPower(bool on, void *cookie) 2680 { 2681 TDECL(); 2682 2683 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2684 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2685 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2686 INFO_PRINT("setPressPower: %s\n", on ? "enable" : "disable"); 2687 2688 if (on) { 2689 if (T(masterConfigDependencies) != 0) { 2690 T(masterConfigDependencies) |= BIT(PRESS); 2691 2692 osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[PRESS]), NULL, mTask.tid); 2693 } else { 2694 T(masterConfigDependencies) |= BIT(PRESS); 2695 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON; 2696 2697 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2698 2699 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__); 2700 } 2701 } else { 2702 uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE; 2703 2704 T(masterConfigDependencies) &= ~BIT(PRESS); 2705 2706 if (T(sensors[TEMP]).enabled) { 2707 i = lsm6dsm_computeOdr(T(sensors[TEMP]).rate[TEMP]); 2708 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i); 2709 } else 2710 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE; 2711 2712 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value, 2713 T(sensors[ACCEL]).hwRate, PRESS); 2714 2715 if (T(masterConfigDependencies) == 0) { 2716 DEBUG_PRINT("setPressPower: no sensors enabled on i2c master, disabling it\n"); 2717 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON; 2718 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2719 } 2720 2721 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 2722 if (T(baroTimerId)) { 2723 timTimerCancel(T(baroTimerId)); 2724 T(baroTimerId) = 0; 2725 2726 T(pendingBaroTimerTask) = false; 2727 T(time).timestampBaroLSB = 0; 2728 2729 if (T(sensors[TEMP]).enabled) 2730 T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(T(sensors[TEMP]).rate[TEMP]), 0, 50, lsm6dsm_baroTimerCallback, NULL, false); 2731 } 2732 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 2733 2734 T(sensors[ACCEL]).rate[PRESS] = 0; 2735 T(sensors[PRESS]).rate[PRESS] = 0; 2736 T(sensors[PRESS]).latency = UINT64_MAX; 2737 T(sensors[PRESS]).hwRate = 0; 2738 2739 lsm6dsm_updateOdrs(); 2740 2741 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__); 2742 } 2743 } else { 2744 T(pendingEnableConfig[PRESS]) = true; 2745 T(sensors[PRESS]).pConfig.enable = on; 2746 } 2747 2748 return true; 2749 } 2750 2751 /* 2752 * lsm6dsm_setTempPower: enable/disable temperature sensor 2753 * @on: enable or disable sensor. 2754 * @cookie: private data. 2755 */ 2756 static bool lsm6dsm_setTempPower(bool on, void *cookie) 2757 { 2758 TDECL(); 2759 2760 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2761 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2762 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2763 INFO_PRINT("setTempPower: %s\n", on ? "enable" : "disable"); 2764 2765 if (on) { 2766 if (T(masterConfigDependencies) != 0) { 2767 T(masterConfigDependencies) |= BIT(TEMP); 2768 2769 osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[TEMP]), NULL, mTask.tid); 2770 } else { 2771 T(masterConfigDependencies) |= BIT(TEMP); 2772 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON; 2773 2774 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2775 2776 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__); 2777 } 2778 } else { 2779 uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE; 2780 2781 T(masterConfigDependencies) &= ~BIT(TEMP); 2782 2783 if (T(sensors[PRESS]).enabled) { 2784 i = lsm6dsm_computeOdr(T(sensors[PRESS]).rate[PRESS]); 2785 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i); 2786 } else 2787 reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE; 2788 2789 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value, 2790 T(sensors[ACCEL]).hwRate, TEMP); 2791 2792 if (T(masterConfigDependencies) == 0) { 2793 DEBUG_PRINT("setTempPower: no sensors enabled on i2c master, disabling it\n"); 2794 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON; 2795 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister)); 2796 } 2797 2798 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 2799 if (T(baroTimerId)) { 2800 timTimerCancel(T(baroTimerId)); 2801 T(baroTimerId) = 0; 2802 2803 T(pendingBaroTimerTask) = false; 2804 T(time).timestampBaroLSB = 0; 2805 2806 if (T(sensors[PRESS]).enabled) 2807 T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(T(sensors[PRESS]).rate[PRESS]), 0, 50, lsm6dsm_baroTimerCallback, NULL, false); 2808 } 2809 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 2810 2811 T(sensors[ACCEL]).rate[TEMP] = 0; 2812 T(sensors[TEMP]).rate[TEMP] = 0; 2813 T(sensors[TEMP]).latency = UINT64_MAX; 2814 T(sensors[TEMP]).hwRate = 0; 2815 2816 lsm6dsm_updateOdrs(); 2817 2818 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__); 2819 } 2820 } else { 2821 T(pendingEnableConfig[TEMP]) = true; 2822 T(sensors[TEMP]).pConfig.enable = on; 2823 } 2824 2825 return true; 2826 } 2827 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 2828 2829 /* 2830 * lsm6dsm_setStepDetectorPower: enable/disable step detector sensor 2831 * @on: enable or disable sensor. 2832 * @cookie: private data. 2833 */ 2834 static bool lsm6dsm_setStepDetectorPower(bool on, void *cookie) 2835 { 2836 TDECL(); 2837 2838 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2839 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2840 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2841 INFO_PRINT("setStepDetectorPower: %s\n", on ? "enable" : "disable"); 2842 2843 if (on) { 2844 T(pedometerDependencies) |= BIT(STEP_DETECTOR); 2845 T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC; 2846 T(int1Register) |= LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE; 2847 2848 T(sensors[ACCEL]).rate[STEP_DETECTOR] = SENSOR_HZ(26.0f); 2849 lsm6dsm_updateOdrs(); 2850 2851 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister)); 2852 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register)); 2853 } else { 2854 T(pedometerDependencies) &= ~BIT(STEP_DETECTOR); 2855 T(int1Register) &= ~LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE; 2856 2857 if ((T(pedometerDependencies) & (BIT(STEP_COUNTER) | BIT(SIGN_MOTION))) == 0) { 2858 DEBUG_PRINT("setStepDetectorPower: no more need pedometer algo, disabling it\n"); 2859 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC; 2860 } 2861 2862 T(sensors[ACCEL]).rate[STEP_DETECTOR] = 0; 2863 lsm6dsm_updateOdrs(); 2864 2865 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register)); 2866 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister)); 2867 } 2868 2869 /* 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 */ 2870 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[STEP_DETECTOR]), __FUNCTION__); 2871 } else { 2872 T(pendingEnableConfig[STEP_DETECTOR]) = true; 2873 T(sensors[STEP_DETECTOR]).pConfig.enable = on; 2874 } 2875 2876 return true; 2877 } 2878 2879 /* 2880 * lsm6dsm_setStepCounterPower: enable/disable step counter sensor 2881 * @on: enable or disable sensor. 2882 * @cookie: private data. 2883 */ 2884 static bool lsm6dsm_setStepCounterPower(bool on, void *cookie) 2885 { 2886 TDECL(); 2887 2888 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2889 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2890 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2891 INFO_PRINT("setStepCounterPower: %s\n", on ? "enable" : "disable"); 2892 2893 if (on) { 2894 T(readSteps) = false; 2895 T(pedometerDependencies) |= BIT(STEP_COUNTER); 2896 T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC; 2897 T(int2Register) |= LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE; 2898 2899 T(sensors[ACCEL]).rate[STEP_COUNTER] = SENSOR_HZ(26.0f); 2900 lsm6dsm_updateOdrs(); 2901 2902 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister)); 2903 SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register)); 2904 } else { 2905 T(pedometerDependencies) &= ~BIT(STEP_COUNTER); 2906 T(int2Register) &= ~LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE; 2907 2908 if ((T(pedometerDependencies) & (BIT(STEP_DETECTOR) | BIT(SIGN_MOTION))) == 0) { 2909 DEBUG_PRINT("setStepCounterPower: no more need pedometer algo, disabling it\n"); 2910 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC; 2911 } 2912 2913 T(sensors[ACCEL]).rate[STEP_COUNTER] = 0; 2914 lsm6dsm_updateOdrs(); 2915 2916 SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register)); 2917 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister)); 2918 } 2919 2920 /* 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 */ 2921 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[STEP_COUNTER]), __FUNCTION__); 2922 } else { 2923 T(pendingEnableConfig[STEP_COUNTER]) = true; 2924 T(sensors[STEP_COUNTER]).pConfig.enable = on; 2925 } 2926 2927 return true; 2928 } 2929 2930 /* 2931 * lsm6dsm_setSignMotionPower: enable/disable significant motion sensor 2932 * @on: enable or disable sensor. 2933 * @cookie: private data. 2934 */ 2935 static bool lsm6dsm_setSignMotionPower(bool on, void *cookie) 2936 { 2937 TDECL(); 2938 2939 /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly. 2940 If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */ 2941 if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { 2942 INFO_PRINT("setSignMotionPower: %s\n", on ? "enable" : "disable"); 2943 2944 if (on) { 2945 T(pedometerDependencies) |= BIT(SIGN_MOTION); 2946 T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC | LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC); 2947 T(int1Register) |= LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE; 2948 2949 T(sensors[ACCEL]).rate[SIGN_MOTION] = SENSOR_HZ(26.0f); 2950 lsm6dsm_updateOdrs(); 2951 2952 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister)); 2953 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register)); 2954 } else { 2955 T(pedometerDependencies) &= ~BIT(SIGN_MOTION); 2956 T(int1Register) &= ~LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE; 2957 2958 if ((T(pedometerDependencies) & (BIT(STEP_DETECTOR) | BIT(STEP_COUNTER))) == 0) { 2959 DEBUG_PRINT("setSignMotionPower: no more need pedometer algo, disabling it\n"); 2960 T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC; 2961 } 2962 2963 T(sensors[ACCEL]).rate[SIGN_MOTION] = 0; 2964 lsm6dsm_updateOdrs(); 2965 2966 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register), 50000); 2967 SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister)); 2968 } 2969 2970 /* 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 */ 2971 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[SIGN_MOTION]), __FUNCTION__); 2972 } else { 2973 T(pendingEnableConfig[SIGN_MOTION]) = true; 2974 T(sensors[SIGN_MOTION]).pConfig.enable = on; 2975 } 2976 2977 return true; 2978 } 2979 2980 /* 2981 * lsm6dsm_accelFirmwareUpload: upload accelerometer firmware 2982 * @cookie: private data. 2983 */ 2984 static bool lsm6dsm_accelFirmwareUpload(void *cookie) 2985 { 2986 TDECL(); 2987 2988 sensorSignalInternalEvt(T(sensors[ACCEL]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 2989 2990 return true; 2991 } 2992 2993 /* 2994 * lsm6dsm_gyroFirmwareUpload: upload gyroscope firmware 2995 * @cookie: private data. 2996 */ 2997 static bool lsm6dsm_gyroFirmwareUpload(void *cookie) 2998 { 2999 TDECL(); 3000 3001 sensorSignalInternalEvt(T(sensors[GYRO]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 3002 3003 return true; 3004 } 3005 3006 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 3007 /* 3008 * lsm6dsm_magnFirmwareUpload: upload magnetometer firmware 3009 * @cookie: private data. 3010 */ 3011 static bool lsm6dsm_magnFirmwareUpload(void *cookie) 3012 { 3013 TDECL(); 3014 3015 sensorSignalInternalEvt(T(sensors[MAGN]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 3016 3017 return true; 3018 } 3019 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3020 3021 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 3022 /* 3023 * lsm6dsm_pressFirmwareUpload: upload pressure firmware 3024 * @cookie: private data. 3025 */ 3026 static bool lsm6dsm_pressFirmwareUpload(void *cookie) 3027 { 3028 TDECL(); 3029 3030 sensorSignalInternalEvt(T(sensors[PRESS]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 3031 3032 return true; 3033 } 3034 3035 /* 3036 * lsm6dsm_tempFirmwareUpload: upload pressure firmware 3037 * @cookie: private data. 3038 */ 3039 static bool lsm6dsm_tempFirmwareUpload(void *cookie) 3040 { 3041 TDECL(); 3042 3043 sensorSignalInternalEvt(T(sensors[TEMP]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 3044 3045 return true; 3046 } 3047 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 3048 3049 /* 3050 * lsm6dsm_stepDetectorFirmwareUpload: upload step detector firmware 3051 * @cookie: private data. 3052 */ 3053 static bool lsm6dsm_stepDetectorFirmwareUpload(void *cookie) 3054 { 3055 TDECL(); 3056 3057 sensorSignalInternalEvt(T(sensors[STEP_DETECTOR]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 3058 3059 return true; 3060 } 3061 3062 /* 3063 * lsm6dsm_stepCounterFirmwareUpload: upload step counter firmware 3064 * @cookie: private data. 3065 */ 3066 static bool lsm6dsm_stepCounterFirmwareUpload(void *cookie) 3067 { 3068 TDECL(); 3069 3070 sensorSignalInternalEvt(T(sensors[STEP_COUNTER]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 3071 3072 return true; 3073 } 3074 3075 /* 3076 * lsm6dsm_signMotionFirmwareUpload: upload significant motion firmware 3077 * @cookie: private data. 3078 */ 3079 static bool lsm6dsm_signMotionFirmwareUpload(void *cookie) 3080 { 3081 TDECL(); 3082 3083 sensorSignalInternalEvt(T(sensors[SIGN_MOTION]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); 3084 3085 return true; 3086 } 3087 3088 /* 3089 * lsm6dsm_setAccelRate: set accelerometer ODR and report latency (FIFO watermark related) 3090 * @rate: sensor rate expressed in SENSOR_HZ(x). 3091 * @latency: max latency valud in ns. 3092 * @cookie: private data. 3093 */ 3094 static bool lsm6dsm_setAccelRate(uint32_t rate, uint64_t latency, void *cookie) 3095 { 3096 TDECL(); 3097 3098 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 3099 INFO_PRINT("setAccelRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency); 3100 3101 T(sensors[ACCEL]).rate[ACCEL] = rate; 3102 T(sensors[ACCEL]).latency = latency; 3103 3104 if (lsm6dsm_updateOdrs()) 3105 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__); 3106 else 3107 osEnqueuePrivateEvt(EVT_SENSOR_CONFIG_CHANGING, &T(sensors[ACCEL]), NULL, mTask.tid); 3108 } else { 3109 T(pendingRateConfig[ACCEL]) = true; 3110 T(sensors[ACCEL].pConfig.rate) = rate; 3111 T(sensors[ACCEL]).pConfig.latency = latency; 3112 } 3113 3114 return true; 3115 } 3116 3117 /* 3118 * lsm6dsm_setGyroRate: set gyroscope ODR and report latency (FIFO watermark related) 3119 * @rate: sensor rate expressed in SENSOR_HZ(x). 3120 * @latency: max latency valud in ns. 3121 * @cookie: private data. 3122 */ 3123 static bool lsm6dsm_setGyroRate(uint32_t rate, uint64_t latency, void *cookie) 3124 { 3125 TDECL(); 3126 3127 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 3128 INFO_PRINT("setGyroRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency); 3129 3130 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 3131 T(sensors[ACCEL]).rate[GYRO] = rate; 3132 T(sensors[ACCEL]).dependenciesRequireData[GYRO] = true; 3133 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 3134 T(sensors[GYRO]).rate[GYRO] = rate; 3135 T(sensors[GYRO]).latency = latency; 3136 3137 if (lsm6dsm_updateOdrs()) 3138 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__); 3139 else 3140 osEnqueuePrivateEvt(EVT_SENSOR_CONFIG_CHANGING, &T(sensors[GYRO]), NULL, mTask.tid); 3141 } else { 3142 T(pendingRateConfig[GYRO]) = true; 3143 T(sensors[GYRO]).pConfig.rate = rate; 3144 T(sensors[GYRO]).pConfig.latency = latency; 3145 } 3146 3147 return true; 3148 } 3149 3150 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 3151 /* 3152 * lsm6dsm_setMagnRate: set magnetometer ODR and report latency (FIFO watermark related) 3153 * @rate: sensor rate expressed in SENSOR_HZ(x). 3154 * @latency: max latency valud in ns. 3155 * @cookie: private data. 3156 */ 3157 static bool lsm6dsm_setMagnRate(uint32_t rate, uint64_t latency, void *cookie) 3158 { 3159 TDECL(); 3160 uint8_t i; 3161 3162 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 3163 INFO_PRINT("setMagnRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency); 3164 3165 T(sensors[ACCEL]).rate[MAGN] = rate; 3166 #ifdef LSM6DSM_MAGN_CALIB_ENABLED 3167 T(sensors[ACCEL]).dependenciesRequireData[MAGN] = true; 3168 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ 3169 T(sensors[MAGN]).rate[MAGN] = rate; 3170 T(sensors[MAGN]).latency = latency; 3171 3172 lsm6dsm_updateOdrs(); 3173 3174 /* This call return index of LSM6DSMImuRates struct element */ 3175 i = lsm6dsm_computeOdr(rate); 3176 T(sensors[MAGN]).hwRate = LSM6DSMSHRates[i]; 3177 T(sensors[MAGN]).samplesToDiscard = 3; 3178 3179 T(sensors[MAGN]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[MAGN]).samplesFifoDecimator) / T(sensors[MAGN]).rate[MAGN]; 3180 T(sensors[MAGN]).samplesDecimatorCounter = T(sensors[MAGN]).samplesDecimator - 1; 3181 3182 #ifdef LSM6DSM_I2C_MASTER_LIS3MDL 3183 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR, 3184 LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE, 3185 T(sensors[ACCEL]).hwRate, MAGN); 3186 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR, 3187 LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i), 3188 T(sensors[ACCEL]).hwRate, MAGN); 3189 #else /* LSM6DSM_I2C_MASTER_LIS3MDL */ 3190 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR, 3191 LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i), 3192 T(sensors[ACCEL]).hwRate, MAGN); 3193 #endif /* LSM6DSM_I2C_MASTER_LIS3MDL */ 3194 3195 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__); 3196 } else { 3197 T(pendingRateConfig[MAGN]) = true; 3198 T(sensors[MAGN]).pConfig.rate = rate; 3199 T(sensors[MAGN]).pConfig.latency = latency; 3200 } 3201 3202 return true; 3203 } 3204 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3205 3206 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 3207 /* 3208 * lsm6dsm_setPressRate: set pressure ODR and report latency (FIFO watermark related) 3209 * @rate: sensor rate expressed in SENSOR_HZ(x). 3210 * @latency: max latency valud in ns. 3211 * @cookie: private data. 3212 */ 3213 static bool lsm6dsm_setPressRate(uint32_t rate, uint64_t latency, void *cookie) 3214 { 3215 TDECL(); 3216 uint8_t i; 3217 3218 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 3219 INFO_PRINT("setPressRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency); 3220 3221 T(sensors[ACCEL]).rate[PRESS] = rate; 3222 T(sensors[PRESS]).rate[PRESS] = rate; 3223 T(sensors[PRESS]).latency = latency; 3224 3225 lsm6dsm_updateOdrs(); 3226 3227 if (T(sensors[TEMP]).enabled) { 3228 if (rate < T(sensors[TEMP]).rate[TEMP]) 3229 rate = T(sensors[TEMP]).rate[TEMP]; 3230 } 3231 3232 /* This call return index of LSM6DSMImuRates struct element */ 3233 i = lsm6dsm_computeOdr(rate); 3234 T(sensors[PRESS]).hwRate = LSM6DSMSHRates[i]; 3235 T(sensors[PRESS]).samplesToDiscard = 3; 3236 3237 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 3238 if (T(baroTimerId)) { 3239 timTimerCancel(T(baroTimerId)); 3240 T(baroTimerId) = 0; 3241 T(pendingBaroTimerTask) = false; 3242 } 3243 3244 T(sensors[PRESS]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS]; 3245 T(sensors[TEMP]).samplesDecimator = rate / T(sensors[TEMP]).rate[TEMP]; 3246 T(time).timestampBaroLSB = 0; 3247 3248 T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(rate), 0, 50, lsm6dsm_baroTimerCallback, NULL, false); 3249 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3250 T(sensors[PRESS]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[PRESS]).rate[PRESS]; 3251 T(sensors[TEMP]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[TEMP]).rate[TEMP]; 3252 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3253 3254 T(sensors[PRESS]).samplesDecimatorCounter = T(sensors[PRESS]).samplesDecimator - 1; 3255 T(sensors[TEMP]).samplesDecimatorCounter = T(sensors[TEMP]).samplesDecimator - 1; 3256 3257 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR, 3258 LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i), 3259 T(sensors[ACCEL]).hwRate, PRESS); 3260 3261 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__); 3262 } else { 3263 T(pendingRateConfig[PRESS]) = true; 3264 T(sensors[PRESS]).pConfig.rate = rate; 3265 T(sensors[PRESS]).pConfig.latency = latency; 3266 } 3267 3268 return true; 3269 } 3270 3271 /* 3272 * lsm6dsm_setTempRate: set temperature ODR and report latency (FIFO watermark related) 3273 * @rate: sensor rate expressed in SENSOR_HZ(x). 3274 * @latency: max latency valud in ns. 3275 * @cookie: private data. 3276 */ 3277 static bool lsm6dsm_setTempRate(uint32_t rate, uint64_t latency, void *cookie) 3278 { 3279 TDECL(); 3280 uint8_t i; 3281 3282 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 3283 INFO_PRINT("setTempRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency); 3284 3285 T(sensors[ACCEL]).rate[TEMP] = rate; 3286 T(sensors[TEMP]).rate[TEMP] = rate; 3287 T(sensors[TEMP]).latency = latency; 3288 3289 lsm6dsm_updateOdrs(); 3290 3291 if (T(sensors[PRESS]).enabled) { 3292 if (rate < T(sensors[PRESS]).rate[PRESS]) 3293 rate = T(sensors[PRESS]).rate[PRESS]; 3294 } 3295 3296 /* This call return index of LSM6DSMImuRates struct element */ 3297 i = lsm6dsm_computeOdr(rate); 3298 T(sensors[TEMP]).hwRate = LSM6DSMSHRates[i]; 3299 T(sensors[TEMP]).samplesToDiscard = 3; 3300 3301 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 3302 if (T(baroTimerId)) { 3303 timTimerCancel(T(baroTimerId)); 3304 T(baroTimerId) = 0; 3305 T(pendingBaroTimerTask) = false; 3306 } 3307 3308 T(sensors[TEMP]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS]; 3309 T(sensors[PRESS]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS]; 3310 T(time).timestampBaroLSB = 0; 3311 3312 T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(rate), 0, 50, lsm6dsm_baroTimerCallback, NULL, false); 3313 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3314 T(sensors[TEMP]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[TEMP]).rate[TEMP]; 3315 T(sensors[PRESS]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[PRESS]).rate[TEMP]; 3316 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3317 3318 T(sensors[TEMP]).samplesDecimatorCounter = T(sensors[TEMP]).samplesDecimator - 1; 3319 T(sensors[PRESS]).samplesDecimatorCounter = T(sensors[PRESS]).samplesDecimator - 1; 3320 3321 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR, 3322 LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i), 3323 T(sensors[ACCEL]).hwRate, TEMP); 3324 3325 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__); 3326 } else { 3327 T(pendingRateConfig[TEMP]) = true; 3328 T(sensors[TEMP]).pConfig.rate = rate; 3329 T(sensors[TEMP]).pConfig.latency = latency; 3330 } 3331 3332 return true; 3333 } 3334 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 3335 3336 /* 3337 * lsm6dsm_setStepDetectorRate: set step detector report latency 3338 * @rate: sensor rate expressed in SENSOR_HZ(x). 3339 * @latency: max latency valud in ns. 3340 * @cookie: private data. 3341 */ 3342 static bool lsm6dsm_setStepDetectorRate(uint32_t rate, uint64_t latency, void *cookie) 3343 { 3344 TDECL(); 3345 3346 INFO_PRINT("setStepDetectorRate: latency=%lldns\n", latency); 3347 3348 T(sensors[STEP_DETECTOR]).hwRate = rate; 3349 T(sensors[STEP_DETECTOR]).latency = latency; 3350 3351 sensorSignalInternalEvt(T(sensors[STEP_DETECTOR]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 3352 3353 return true; 3354 } 3355 3356 /* 3357 * lsm6dsm_setStepCounterRate: set step counter report latency 3358 * @rate: sensor rate expressed in SENSOR_HZ(x). 3359 * @latency: max latency valud in ns. 3360 * @cookie: private data. 3361 */ 3362 static bool lsm6dsm_setStepCounterRate(uint32_t rate, uint64_t latency, void *cookie) 3363 { 3364 TDECL(); 3365 uint8_t i, regValue; 3366 3367 if (trySwitchState(SENSOR_CONFIG_CHANGING)) { 3368 if (rate == SENSOR_RATE_ONCHANGE) { 3369 INFO_PRINT("setStepCounterRate: delivery-rate=on_change, latency=%lldns\n", latency); 3370 } else 3371 INFO_PRINT("setStepCounterRate: delivery_rate=%dms, latency=%lldns\n", (int)((1024.0f / rate) * 1000.0f), latency); 3372 3373 T(sensors[STEP_COUNTER]).hwRate = rate; 3374 T(sensors[STEP_COUNTER]).latency = latency; 3375 3376 if (rate != SENSOR_RATE_ONCHANGE) { 3377 for (i = 0; i < ARRAY_SIZE(LSM6DSMStepCounterRates); i++) { 3378 if (rate == LSM6DSMStepCounterRates[i]) 3379 break; 3380 } 3381 if (i >= (ARRAY_SIZE(LSM6DSMStepCounterRates) - 2)) 3382 regValue = 0; 3383 else 3384 regValue = (128 >> i); 3385 } else 3386 regValue = 0; 3387 3388 lsm6dsm_writeEmbeddedRegister(LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR, regValue); 3389 3390 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__); 3391 } else { 3392 T(pendingRateConfig[STEP_COUNTER]) = true; 3393 T(sensors[STEP_COUNTER]).pConfig.rate = rate; 3394 T(sensors[STEP_COUNTER]).pConfig.latency = latency; 3395 } 3396 3397 return true; 3398 } 3399 3400 /* 3401 * lsm6dsm_setSignMotionRate: set significant motion report latency 3402 * @rate: sensor rate expressed in SENSOR_HZ(x). 3403 * @latency: max latency valud in ns. 3404 * @cookie: private data. 3405 */ 3406 static bool lsm6dsm_setSignMotionRate(uint32_t rate, uint64_t latency, void *cookie) 3407 { 3408 TDECL(); 3409 3410 DEBUG_PRINT("setSignMotionRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency); 3411 3412 T(sensors[SIGN_MOTION]).rate[SIGN_MOTION] = rate; 3413 T(sensors[SIGN_MOTION]).latency = latency; 3414 3415 sensorSignalInternalEvt(T(sensors[SIGN_MOTION]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency); 3416 3417 return true; 3418 } 3419 3420 /* 3421 * lsm6dsm_accelFlush: send accelerometer flush event 3422 * @cookie: private data. 3423 */ 3424 static bool lsm6dsm_accelFlush(void *cookie) 3425 { 3426 TDECL(); 3427 3428 if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) { 3429 INFO_PRINT("accelFlush: flush accelerometer data\n"); 3430 3431 if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) { 3432 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL); 3433 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3434 return true; 3435 } 3436 3437 T(sendFlushEvt[ACCEL]) = true; 3438 3439 SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer)); 3440 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3441 } else 3442 T(pendingFlush[ACCEL])++; 3443 3444 return true; 3445 } 3446 3447 /* 3448 * lsm6dsm_gyroFlush: send gyroscope flush event 3449 * @cookie: private data. 3450 */ 3451 static bool lsm6dsm_gyroFlush(void *cookie) 3452 { 3453 TDECL(); 3454 3455 if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) { 3456 INFO_PRINT("gyroFlush: flush gyroscope data\n"); 3457 3458 if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) { 3459 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO), SENSOR_DATA_EVENT_FLUSH, NULL); 3460 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3461 return true; 3462 } 3463 3464 T(sendFlushEvt[GYRO]) = true; 3465 3466 SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer)); 3467 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3468 } else 3469 T(pendingFlush[GYRO])++; 3470 3471 return true; 3472 } 3473 3474 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 3475 /* 3476 * lsm6dsm_magnFlush: send magnetometer flush event 3477 * @cookie: private data. 3478 */ 3479 static bool lsm6dsm_magnFlush(void *cookie) 3480 { 3481 TDECL(); 3482 3483 if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) { 3484 INFO_PRINT("magnFlush: flush magnetometer data\n"); 3485 3486 if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) { 3487 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL); 3488 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3489 return true; 3490 } 3491 3492 T(sendFlushEvt[MAGN]) = true; 3493 3494 SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer)); 3495 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3496 } else 3497 T(pendingFlush[MAGN])++; 3498 3499 return true; 3500 } 3501 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3502 3503 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 3504 /* 3505 * lsm6dsm_pressFlush: send pressure flush event 3506 * @cookie: private data. 3507 */ 3508 static bool lsm6dsm_pressFlush(void *cookie) 3509 { 3510 TDECL(); 3511 3512 #if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 3513 if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) { 3514 INFO_PRINT("pressFlush: flush pressure data\n"); 3515 3516 if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) { 3517 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL); 3518 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3519 return true; 3520 } 3521 3522 T(sendFlushEvt[PRESS]) = true; 3523 3524 SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer)); 3525 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3526 } else 3527 T(pendingFlush[PRESS])++; 3528 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3529 INFO_PRINT("pressFlush: flush pressure data\n"); 3530 3531 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL); 3532 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3533 3534 return true; 3535 } 3536 3537 /* 3538 * lsm6dsm_tempFlush: send temperature flush event 3539 * @cookie: private data. 3540 */ 3541 static bool lsm6dsm_tempFlush(void *cookie) 3542 { 3543 TDECL(); 3544 3545 #if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 3546 if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) { 3547 INFO_PRINT("tempFlush: flush temperature data\n"); 3548 3549 if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) { 3550 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL); 3551 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3552 return true; 3553 } 3554 3555 T(sendFlushEvt[TEMP]) = true; 3556 3557 SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer)); 3558 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3559 } else 3560 T(pendingFlush[TEMP])++; 3561 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3562 INFO_PRINT("tempFlush: flush temperature data\n"); 3563 3564 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL); 3565 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3566 3567 return true; 3568 } 3569 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 3570 3571 /* 3572 * lsm6dsm_stepDetectorFlush: send step detector flush event 3573 * @cookie: private data. 3574 */ 3575 static bool lsm6dsm_stepDetectorFlush(void *cookie) 3576 { 3577 TDECL(); 3578 3579 INFO_PRINT("stepDetectorFlush: flush step detector data\n"); 3580 3581 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), SENSOR_DATA_EVENT_FLUSH, NULL); 3582 3583 return true; 3584 } 3585 3586 /* 3587 * lsm6dsm_stepCounterFlush: send step counter flush event 3588 * @cookie: private data. 3589 */ 3590 static bool lsm6dsm_stepCounterFlush(void *cookie) 3591 { 3592 TDECL(); 3593 3594 INFO_PRINT("stepCounterFlush: flush step counter data\n"); 3595 3596 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), SENSOR_DATA_EVENT_FLUSH, NULL); 3597 3598 return true; 3599 } 3600 3601 /* 3602 * lsm6dsm_signMotionFlush: send significant motion flush event 3603 * @cookie: private data. 3604 */ 3605 static bool lsm6dsm_signMotionFlush(void *cookie) 3606 { 3607 TDECL(); 3608 3609 INFO_PRINT("signMotionFlush: flush significant motion data\n"); 3610 3611 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), SENSOR_DATA_EVENT_FLUSH, NULL); 3612 3613 return true; 3614 } 3615 3616 /* 3617 * lsm6dsm_stepCounterSendLastData: send last number of steps 3618 * @cookie: private data. 3619 * @tid: task id. 3620 */ 3621 static bool lsm6dsm_stepCounterSendLastData(void *cookie, uint32_t tid) 3622 { 3623 TDECL(); 3624 3625 INFO_PRINT("stepCounterSendLastData: %lu steps\n", T(totalNumSteps)); 3626 3627 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), &T(totalNumSteps), NULL); 3628 3629 return true; 3630 } 3631 3632 /* 3633 * lsm6dsm_runAccelSelfTest: execute accelerometer self-test 3634 * @cookie: private data. 3635 */ 3636 static bool lsm6dsm_runAccelSelfTest(void *cookie) 3637 { 3638 TDECL(); 3639 3640 if (trySwitchState(SENSOR_SELFTEST)) { 3641 if (!T(sensors[ACCEL]).enabled && (T(sensors[ACCEL]).hwRate == 0) && (T(sensors[GYRO]).hwRate == 0)) { 3642 INFO_PRINT("runAccelSelfTest: executing accelerometer selftest\n"); 3643 T(selftestState) = SELFTEST_INITIALIZATION; 3644 lsm6dsm_runGapSelfTestProgram(ACCEL); 3645 return true; 3646 } else 3647 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3648 } 3649 3650 ERROR_PRINT("runAccelSelfTest: cannot run selftest because sensor is busy!\n"); 3651 lsm6dsm_sendSelfTestResult(SENS_TYPE_ACCEL, SENSOR_APP_EVT_STATUS_BUSY); 3652 3653 return false; 3654 } 3655 3656 /* 3657 * lsm6dsm_runGyroSelfTest: execute gyroscope self-test 3658 * @cookie: private data. 3659 */ 3660 static bool lsm6dsm_runGyroSelfTest(void *cookie) 3661 { 3662 TDECL(); 3663 3664 if (trySwitchState(SENSOR_SELFTEST)) { 3665 if (!T(sensors[GYRO]).enabled && (T(sensors[GYRO]).hwRate == 0) && (T(sensors[ACCEL]).hwRate == 0)) { 3666 INFO_PRINT("runGyroSelfTest: executing gyroscope selftest\n"); 3667 T(selftestState) = SELFTEST_INITIALIZATION; 3668 lsm6dsm_runGapSelfTestProgram(GYRO); 3669 return true; 3670 } else 3671 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3672 } 3673 3674 ERROR_PRINT("runGyroSelfTest: cannot run selftest because sensor is busy!\n"); 3675 lsm6dsm_sendSelfTestResult(SENS_TYPE_GYRO, SENSOR_APP_EVT_STATUS_BUSY); 3676 3677 return false; 3678 } 3679 3680 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 3681 /* 3682 * lsm6dsm_runMagnSelfTest: execute magnetometer self-test 3683 * @cookie: private data. 3684 */ 3685 static bool lsm6dsm_runMagnSelfTest(void *cookie) 3686 { 3687 TDECL(); 3688 3689 if (trySwitchState(SENSOR_SELFTEST)) { 3690 if (!T(sensors[MAGN]).enabled && (T(sensors[MAGN]).hwRate == 0) && (T(sensors[GYRO]).hwRate == 0) && (T(sensors[ACCEL]).hwRate == 0)) { 3691 INFO_PRINT("runMagnSelfTest: executing magnetometer selftest\n"); 3692 T(selftestState) = SELFTEST_INITIALIZATION; 3693 #ifdef LSM6DSM_I2C_MASTER_AK09916 3694 lsm6dsm_runAbsoluteSelfTestProgram(); 3695 #else 3696 lsm6dsm_runGapSelfTestProgram(MAGN); 3697 #endif 3698 return true; 3699 } else 3700 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3701 } 3702 3703 ERROR_PRINT("runMagnSelfTest: cannot run selftest because sensor is busy!\n"); 3704 lsm6dsm_sendSelfTestResult(SENS_TYPE_MAG, SENSOR_APP_EVT_STATUS_BUSY); 3705 3706 return false; 3707 } 3708 3709 /* 3710 * lsm6dsm_magnCfgData: set sw magnetometer calibration values 3711 * @data: calibration data struct. 3712 * @cookie: private data. 3713 */ 3714 static bool lsm6dsm_magnCfgData(void *data, void *cookie) 3715 { 3716 TDECL(); 3717 const struct AppToSensorHalDataPayload *p = data; 3718 3719 if (p->type == HALINTF_TYPE_MAG_CAL_BIAS && p->size == sizeof(struct MagCalBias)) { 3720 const struct MagCalBias *d = p->magCalBias; 3721 INFO_PRINT("lsm6dsm_magnCfgData: calibration %ldnT, %ldnT, %ldnT\n", 3722 (int32_t)(d->bias[0] * 1000), 3723 (int32_t)(d->bias[1] * 1000), 3724 (int32_t)(d->bias[2] * 1000)); 3725 3726 T(magnCal).x_bias = d->bias[0]; 3727 T(magnCal).y_bias = d->bias[1]; 3728 T(magnCal).z_bias = d->bias[2]; 3729 } else if (p->type == HALINTF_TYPE_MAG_LOCAL_FIELD && p->size == sizeof(struct MagLocalField)) { 3730 const struct MagLocalField *d = p->magLocalField; 3731 INFO_PRINT("lsm6dsm_magnCfgData: local field strength %dnT, dec %ddeg, inc %ddeg\n", 3732 (int)(d->strength * 1000), 3733 (int)(d->declination * 180 / M_PI + 0.5f), 3734 (int)(d->inclination * 180 / M_PI + 0.5f)); 3735 3736 // Passing local field information to mag calibration routine 3737 #ifdef DIVERSITY_CHECK_ENABLED 3738 diversityCheckerLocalFieldUpdate(&T(magnCal).diversity_checker, d->strength); 3739 #endif 3740 // TODO: pass local field information to rotation vector sensor. 3741 } else { 3742 ERROR_PRINT("lsm6dsm_magnCfgData: unknown type 0x%04x, size %d", p->type, p->size); 3743 } 3744 3745 return true; 3746 } 3747 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3748 3749 /* 3750 * lsm6dsm_runAccelCalibration: execute accelerometer calibration 3751 * @cookie: private data. 3752 */ 3753 static bool lsm6dsm_runAccelCalibration(void *cookie) 3754 { 3755 TDECL(); 3756 3757 if (trySwitchState(SENSOR_CALIBRATION)) { 3758 if (!T(sensors[ACCEL]).enabled && (T(sensors[ACCEL]).hwRate == 0) && (T(sensors[GYRO]).hwRate == 0)) { 3759 INFO_PRINT("runAccelCalibration: executing accelerometer calibration\n"); 3760 T(calibrationState) = CALIBRATION_INITIALIZATION; 3761 lsm6dsm_runCalibrationProgram(ACCEL); 3762 return true; 3763 } else 3764 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3765 } 3766 3767 ERROR_PRINT("runAccelCalibration: cannot run selftest because sensor is busy!\n"); 3768 lsm6dsm_sendCalibrationResult(SENS_TYPE_ACCEL, SENSOR_APP_EVT_STATUS_BUSY, 0, 0, 0); 3769 3770 return true; 3771 } 3772 3773 /* 3774 * lsm6dsm_runGyroCalibration: execute gyroscope calibration 3775 * @cookie: private data. 3776 */ 3777 static bool lsm6dsm_runGyroCalibration(void *cookie) 3778 { 3779 TDECL(); 3780 3781 if (trySwitchState(SENSOR_CALIBRATION)) { 3782 if (!T(sensors[GYRO]).enabled && (T(sensors[GYRO]).hwRate == 0) && (T(sensors[ACCEL]).hwRate == 0)) { 3783 INFO_PRINT("runGyroCalibration: executing gyroscope calibration\n"); 3784 T(calibrationState) = CALIBRATION_INITIALIZATION; 3785 lsm6dsm_runCalibrationProgram(GYRO); 3786 return true; 3787 } else 3788 osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid); 3789 } 3790 3791 ERROR_PRINT("runGyroCalibration: cannot run selftest because sensor is busy!\n"); 3792 lsm6dsm_sendCalibrationResult(SENS_TYPE_GYRO, SENSOR_APP_EVT_STATUS_BUSY, 0, 0, 0); 3793 3794 return true; 3795 } 3796 3797 /* 3798 * lsm6dsm_storeAccelCalibrationData: store hw calibration into sensor 3799 */ 3800 static bool lsm6dsm_storeAccelCalibrationData(void) 3801 { 3802 TDECL(); 3803 uint8_t buffer[LSM6DSM_TRIAXIAL_NUM_AXIS]; 3804 3805 if (trySwitchState(SENSOR_STORE_CALIBRATION_DATA)) { 3806 for (uint8_t i = 0; i < LSM6DSM_TRIAXIAL_NUM_AXIS; i++) 3807 buffer[i] = lsm6dsm_convertAccelOffsetValue(T(accelCalibrationData[i])); 3808 3809 SPI_MULTIWRITE(LSM6DSM_X_OFS_USR_ADDR, buffer, LSM6DSM_TRIAXIAL_NUM_AXIS); 3810 3811 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, NULL, __FUNCTION__); 3812 } else 3813 return false; 3814 3815 return true; 3816 } 3817 3818 /* 3819 * lsm6dsm_accelCfgData: set hw and sw accelerometer calibration values 3820 * @data: calibration data struct. 3821 * @cookie: private data. 3822 */ 3823 static bool lsm6dsm_accelCfgData(void *data, void *cookie) 3824 { 3825 TDECL(); 3826 struct LSM6DSMAccelGyroCfgData *cfgData = data; 3827 3828 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED 3829 accelCalBiasSet(&T(accelCal) , cfgData->sw[0], cfgData->sw[1], cfgData->sw[2]); 3830 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */ 3831 3832 DEBUG_PRINT("Accel hw bias data [LSB]: %ld %ld %ld\n", cfgData->hw[0], cfgData->hw[1], cfgData->hw[2]); 3833 3834 memcpy(T(accelCalibrationData), cfgData->hw, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 3835 3836 if (!lsm6dsm_storeAccelCalibrationData()) 3837 T(pendingStoreAccelCalibData) = true; 3838 3839 return true; 3840 } 3841 3842 /* 3843 * lsm6dsm_gyroCfgData: set hw and sw gyroscope calibration values 3844 * @data: calibration data struct. 3845 * @cookie: private data. 3846 */ 3847 static bool lsm6dsm_gyroCfgData(void *data, void *cookie) 3848 { 3849 TDECL(); 3850 struct LSM6DSMAccelGyroCfgData *cfgData = data; 3851 3852 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 3853 gyroCalSetBias(&T(gyroCal), cfgData->sw[0], cfgData->sw[1], cfgData->sw[2], sensorGetTime()); 3854 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 3855 3856 DEBUG_PRINT("Gyro hw bias data [LSB]: %ld %ld %ld\n", cfgData->hw[0], cfgData->hw[1], cfgData->hw[2]); 3857 3858 memcpy(T(gyroCalibrationData), cfgData->hw, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 3859 3860 return true; 3861 } 3862 3863 /* 3864 * lsm6dsm_sensorInit: initial sensors configuration 3865 */ 3866 static void lsm6dsm_sensorInit(void) 3867 { 3868 TDECL(); 3869 uint8_t buffer[5]; 3870 3871 switch (T(initState)) { 3872 case RESET_LSM6DSM: 3873 INFO_PRINT("Performing soft-reset\n"); 3874 3875 T(initState) = INIT_LSM6DSM; 3876 3877 /* Sensor SW-reset */ 3878 SPI_WRITE(LSM6DSM_CTRL3_C_ADDR, LSM6DSM_SW_RESET, 20000); 3879 3880 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3881 break; 3882 3883 case INIT_LSM6DSM: 3884 INFO_PRINT("Initial registers configuration\n"); 3885 3886 /* During init, reset all configurable registers to default values */ 3887 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50); 3888 SPI_WRITE(LSM6DSM_DRDY_PULSE_CFG_ADDR, LSM6DSM_DRDY_PULSE_CFG_BASE); 3889 3890 buffer[0] = LSM6DSM_CTRL1_XL_BASE; /* LSM6DSM_CTRL1_XL */ 3891 buffer[1] = LSM6DSM_CTRL2_G_BASE; /* LSM6DSM_CTRL2_G */ 3892 buffer[2] = LSM6DSM_CTRL3_C_BASE; /* LSM6DSM_CTRL3_C */ 3893 buffer[3] = LSM6DSM_CTRL4_C_BASE; /* LSM6DSM_CTRL4_C */ 3894 buffer[4] = LSM6DSM_CTRL5_C_BASE; /* LSM6DSM_CTRL4_C */ 3895 SPI_MULTIWRITE(LSM6DSM_CTRL1_XL_ADDR, buffer, 5); 3896 3897 buffer[0] = LSM6DSM_CTRL10_C_BASE | LSM6DSM_RESET_PEDOMETER; /* LSM6DSM_CTRL10_C */ 3898 buffer[1] = LSM6DSM_MASTER_CONFIG_BASE; /* LSM6DSM_MASTER_CONFIG */ 3899 SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2); 3900 3901 SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, LSM6DSM_INT1_CTRL_BASE); 3902 SPI_WRITE(LSM6DSM_WAKE_UP_DUR_ADDR, LSM6DSM_WAKE_UP_DUR_BASE); 3903 3904 #ifdef LSM6DSM_I2C_MASTER_ENABLED 3905 T(initState) = INIT_I2C_MASTER_REGS_CONF; 3906 #else /* LSM6DSM_I2C_MASTER_ENABLED */ 3907 INFO_PRINT("Initialization completed successfully!\n"); 3908 T(initState) = INIT_DONE; 3909 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 3910 3911 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3912 break; 3913 3914 #ifdef LSM6DSM_I2C_MASTER_ENABLED 3915 case INIT_I2C_MASTER_REGS_CONF: 3916 INFO_PRINT("Initial I2C master registers configuration\n"); 3917 3918 /* Enable access for embedded registers */ 3919 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50); 3920 3921 /* I2C-0 configuration */ 3922 buffer[0] = LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP; /* LSM6DSM_EMBEDDED_SLV0_ADDR */ 3923 buffer[1] = 0x00; /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */ 3924 buffer[2] = LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE; /* LSM6DSM_EMBEDDED_SLV0_CONFIG */ 3925 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 3); 3926 3927 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) /* Magn & Baro both enabled */ 3928 /* I2C-1 configuration */ 3929 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */ 3930 buffer[1] = LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */ 3931 buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV1_CONFIG */ 3932 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3); 3933 3934 /* I2C-2 configuration */ 3935 buffer[0] = (LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV2_ADDR */ 3936 buffer[1] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV2_SUBADDR */ 3937 buffer[2] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV2_CONFIG */ 3938 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR, buffer, 3); 3939 3940 #ifdef LSM6DSM_I2C_MASTER_AK09916 3941 /* I2C-3 configuration */ 3942 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV3_ADDR */ 3943 buffer[1] = AK09916_STATUS_DATA_ADDR; /* LSM6DSM_EMBEDDED_SLV3_SUBADDR */ 3944 buffer[2] = 1; /* LSM6DSM_EMBEDDED_SLV3_CONFIG */ 3945 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR, buffer, 3); 3946 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 3947 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */ 3948 3949 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) /* Magn only enabled */ 3950 /* I2C-1 configuration */ 3951 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */ 3952 buffer[1] = LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */ 3953 buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV1_CONFIG */ 3954 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3); 3955 3956 #ifdef LSM6DSM_I2C_MASTER_AK09916 3957 /* I2C-2 configuration */ 3958 buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV2_ADDR */ 3959 buffer[1] = AK09916_STATUS_DATA_ADDR; /* LSM6DSM_EMBEDDED_SLV2_SUBADDR */ 3960 buffer[2] = 0x01; /* LSM6DSM_EMBEDDED_SLV2_CONFIG */ 3961 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR, buffer, 3); 3962 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 3963 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */ 3964 3965 #if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) /* Baro only enabled */ 3966 /* I2C-1 configuration */ 3967 buffer[0] = (LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */ 3968 buffer[1] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR; /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */ 3969 buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN; /* LSM6DSM_EMBEDDED_SLV1_CONFIG */ 3970 SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3); 3971 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */ 3972 3973 /* Disable access for embedded registers */ 3974 SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50); 3975 3976 T(initState) = INIT_I2C_MASTER_SENSOR_RESET; 3977 3978 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 3979 break; 3980 3981 case INIT_I2C_MASTER_SENSOR_RESET: 3982 INFO_PRINT("Performing soft-reset slave sensors\n"); 3983 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 3984 T(initState) = INIT_I2C_MASTER_MAGN_SENSOR; 3985 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3986 T(initState) = INIT_I2C_MASTER_BARO_SENSOR; 3987 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3988 3989 /* Enable accelerometer and sensor-hub to initialize slave sensor */ 3990 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE); 3991 T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON; 3992 3993 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 3994 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR, LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE, SENSOR_HZ(104.0f), MAGN, 20000); 3995 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 3996 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 3997 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR, LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE, SENSOR_HZ(104.0f), PRESS, 20000); 3998 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 3999 4000 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4001 break; 4002 4003 case INIT_I2C_MASTER_MAGN_SENSOR: 4004 INFO_PRINT("Initial slave magnetometer sensor registers configuration\n"); 4005 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 4006 T(initState) = INIT_I2C_MASTER_BARO_SENSOR; 4007 #else /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4008 T(initState) = INIT_I2C_MASTER_SENSOR_END; 4009 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4010 4011 #ifdef LSM6DSM_I2C_MASTER_LIS3MDL 4012 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL1_ADDR, LIS3MDL_CTRL1_BASE, SENSOR_HZ(104.0f), MAGN); 4013 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL2_ADDR, LIS3MDL_CTRL2_BASE, SENSOR_HZ(104.0f), MAGN); 4014 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL3_ADDR, LIS3MDL_CTRL3_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN); 4015 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL4_ADDR, LIS3MDL_CTRL4_BASE, SENSOR_HZ(104.0f), MAGN); 4016 SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL5_ADDR, LIS3MDL_CTRL5_BASE, SENSOR_HZ(104.0f), MAGN); 4017 #endif /* LSM6DSM_I2C_MASTER_LIS3MDL */ 4018 4019 #ifdef LSM6DSM_I2C_MASTER_LSM303AGR 4020 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); 4021 SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_C_M_ADDR, LSM303AGR_CFG_REG_C_M_BASE, SENSOR_HZ(104.0f), MAGN); 4022 #endif /* LSM6DSM_I2C_MASTER_LSM303AGR */ 4023 4024 #ifdef LSM6DSM_I2C_MASTER_AK09916 4025 SPI_WRITE_SLAVE_SENSOR_REGISTER(AK09916_CNTL2_ADDR, AK09916_CNTL2_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN); 4026 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 4027 4028 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4029 break; 4030 4031 case INIT_I2C_MASTER_BARO_SENSOR: 4032 INFO_PRINT("Initial slave barometer sensor registers configuration\n"); 4033 T(initState) = INIT_I2C_MASTER_SENSOR_END; 4034 4035 #ifdef LSM6DSM_I2C_MASTER_LPS22HB 4036 SPI_WRITE_SLAVE_SENSOR_REGISTER(LPS22HB_CTRL1_ADDR, LPS22HB_CTRL1_BASE | LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE, SENSOR_HZ(104.0f), PRESS); 4037 SPI_WRITE_SLAVE_SENSOR_REGISTER(LPS22HB_CTRL2_ADDR, LPS22HB_CTRL2_BASE, SENSOR_HZ(104.0f), PRESS); 4038 #endif /* LSM6DSM_I2C_MASTER_LPS22HB */ 4039 4040 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4041 break; 4042 4043 case INIT_I2C_MASTER_SENSOR_END: 4044 INFO_PRINT("Initialization completed successfully!\n"); 4045 T(initState) = INIT_DONE; 4046 4047 /* Disable accelerometer and sensor-hub */ 4048 SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE); 4049 SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE); 4050 T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON; 4051 4052 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4053 break; 4054 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 4055 4056 default: 4057 break; 4058 } 4059 } 4060 4061 /* 4062 * lsm6dsm_processPendingEvt: process pending events 4063 */ 4064 static void lsm6dsm_processPendingEvt(void) 4065 { 4066 TDECL(); 4067 enum SensorIndex i; 4068 4069 SET_STATE(SENSOR_IDLE); 4070 4071 for (i = 0; i < NUM_SENSORS; i++) { 4072 if (T(pendingEnableConfig[i])) { 4073 T(pendingEnableConfig[i]) = false; 4074 LSM6DSMSensorOps[i].sensorPower(T(sensors[i]).pConfig.enable, (void *)i); 4075 return; 4076 } 4077 4078 if (T(pendingRateConfig[i])) { 4079 T(pendingRateConfig[i]) = false; 4080 LSM6DSMSensorOps[i].sensorSetRate(T(sensors[i]).pConfig.rate, T(sensors[i]).pConfig.latency, (void *)i); 4081 return; 4082 } 4083 4084 if (T(pendingFlush[i]) > 0) { 4085 T(pendingFlush[i])--; 4086 LSM6DSMSensorOps[i].sensorFlush((void *)i); 4087 return; 4088 } 4089 } 4090 4091 if (T(pendingTimeSyncTask)) { 4092 T(pendingTimeSyncTask) = false; 4093 lsm6dsm_timeSyncTask(); 4094 return; 4095 } 4096 4097 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 4098 if (T(pendingBaroTimerTask)) { 4099 T(pendingBaroTimerTask) = false; 4100 lsm6dsm_baroTimerTask(); 4101 return; 4102 } 4103 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4104 4105 if (T(pendingStoreAccelCalibData)) { 4106 T(pendingStoreAccelCalibData) = lsm6dsm_storeAccelCalibrationData(); 4107 return; 4108 } 4109 4110 if (T(pendingInt)) { 4111 T(pendingInt) = false; 4112 lsm6dsm_readStatusReg(false); 4113 return; 4114 } 4115 4116 if (gpioGet(T(int1))) 4117 lsm6dsm_readStatusReg(false); 4118 } 4119 4120 /* 4121 * lsm6dsm_allocateThreeAxisDataEvt: allocate slab for three axis sensor data 4122 * @mSensor: sensor info. 4123 * @rtcTime: time of first sample in this block. 4124 */ 4125 static bool lsm6dsm_allocateThreeAxisDataEvt(struct LSM6DSMSensor *mSensor, uint64_t rtcTime) 4126 { 4127 TDECL(); 4128 4129 mSensor->tADataEvt = slabAllocatorAlloc(T(mDataSlabThreeAxis)); 4130 if (!mSensor->tADataEvt) { 4131 ERROR_PRINT("Failed to allocate memory!\n"); 4132 return false; 4133 } 4134 4135 memset(&mSensor->tADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample)); 4136 mSensor->tADataEvt->referenceTime = rtcTime; 4137 mSensor->pushedTimestamp = rtcTime; 4138 4139 return true; 4140 } 4141 4142 /* 4143 * lsm6dsm_threeAxisDataEvtFree: deallocate slab of three axis sensor. 4144 * @ptr: sensor data pointer. 4145 */ 4146 static void lsm6dsm_threeAxisDataEvtFree(void *ptr) 4147 { 4148 TDECL(); 4149 4150 slabAllocatorFree(T(mDataSlabThreeAxis), (struct TripleAxisDataEvent *)ptr); 4151 } 4152 4153 #if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 4154 /* 4155 * lsm6dsm_allocateOneAxisDataEvt: allocate slab for one axis sensor data 4156 * @mSensor: sensor info. 4157 * @rtcTime: time of first sample in this block. 4158 */ 4159 static bool lsm6dsm_allocateOneAxisDataEvt(struct LSM6DSMSensor *mSensor, uint64_t rtcTime) 4160 { 4161 TDECL(); 4162 4163 mSensor->sADataEvt = slabAllocatorAlloc(T(mDataSlabOneAxis)); 4164 if (!mSensor->sADataEvt) { 4165 ERROR_PRINT("Failed to allocate memory!\n"); 4166 return false; 4167 } 4168 4169 memset(&mSensor->sADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample)); 4170 mSensor->sADataEvt->referenceTime = rtcTime; 4171 mSensor->pushedTimestamp = rtcTime; 4172 4173 return true; 4174 } 4175 4176 /* 4177 * lsm6dsm_oneAxisDataEvtFree: deallocate slab of one axis sensor 4178 * @ptr: sensor data pointer. 4179 */ 4180 static void lsm6dsm_oneAxisDataEvtFree(void *ptr) 4181 { 4182 TDECL(); 4183 4184 slabAllocatorFree(T(mDataSlabOneAxis), (struct SingleAxisDataEvent *)ptr); 4185 } 4186 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4187 4188 /* 4189 * lsm6dsm_processSensorThreeAxisData: process three axis sensors data 4190 * @mSensor: sensor info. 4191 * @data: sensor data. 4192 * @sampleNum: number of samples in the current slab. 4193 * @timestamp: current sample timestamp; 4194 */ 4195 static bool lsm6dsm_processSensorThreeAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, uint16_t *sampleNum, uint64_t *timestamp) 4196 { 4197 TDECL(); 4198 int16_t x, y, z; 4199 float x_remap, y_remap, z_remap; 4200 struct TripleAxisDataPoint *samples; 4201 4202 if (*timestamp == 0) 4203 return false; 4204 4205 if (mSensor->tADataEvt == NULL) { 4206 if (!lsm6dsm_allocateThreeAxisDataEvt(mSensor, *timestamp)) 4207 return false; 4208 } 4209 samples = mSensor->tADataEvt->samples; 4210 4211 x = (int16_t)(data[1] << 8) | data[0]; 4212 y = (int16_t)(data[3] << 8) | data[2]; 4213 z = (int16_t)(data[5] << 8) | data[4]; 4214 4215 switch (mSensor->idx) { 4216 case ACCEL: 4217 x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE; 4218 y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE; 4219 z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE; 4220 4221 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED 4222 accelCalRun(&T(accelCal), *timestamp, x_remap, y_remap, z_remap, T(currentTemperature)); 4223 accelCalBiasRemove(&T(accelCal), &x_remap, &y_remap, &z_remap); 4224 4225 if (accelCalUpdateBias(&T(accelCal), &samples[*sampleNum].x, &samples[*sampleNum].y, &samples[*sampleNum].z)) { 4226 if (!samples->firstSample.biasCurrent) { 4227 samples->firstSample.biasCurrent = true; 4228 samples->firstSample.biasPresent = 1; 4229 samples->firstSample.biasSample = *sampleNum; 4230 4231 if (*sampleNum > 0) 4232 samples[*sampleNum].deltaTime = 0; 4233 4234 *sampleNum += 1; 4235 } 4236 } 4237 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */ 4238 4239 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 4240 if (T(sensors[GYRO].enabled)) 4241 gyroCalUpdateAccel(&T(gyroCal), *timestamp, x_remap, y_remap, z_remap); 4242 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 4243 4244 break; 4245 4246 case GYRO: 4247 x -= (int16_t)T(gyroCalibrationData)[0]; 4248 y -= (int16_t)T(gyroCalibrationData)[1]; 4249 z -= (int16_t)T(gyroCalibrationData)[2]; 4250 4251 x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE; 4252 y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE; 4253 z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE; 4254 4255 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 4256 gyroCalUpdateGyro(&T(gyroCal), *timestamp, x_remap, y_remap, z_remap, T(currentTemperature)); 4257 4258 #ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED 4259 overTempCalSetTemperature(&T(overTempCal), *timestamp, T(currentTemperature)); 4260 #else /* LSM6DSM_OVERTEMP_CALIB_ENABLED */ 4261 gyroCalRemoveBias(&T(gyroCal), x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap); 4262 #endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */ 4263 4264 if (gyroCalNewBiasAvailable(&T(gyroCal))) { 4265 float biasTemperature, gyroOffset[3] = { 0.0f, 0.0f, 0.0f }; 4266 4267 gyroCalGetBias(&T(gyroCal), &gyroOffset[0], &gyroOffset[1], &gyroOffset[2], &biasTemperature); 4268 4269 if (!samples->firstSample.biasCurrent) { 4270 samples->firstSample.biasCurrent = true; 4271 samples->firstSample.biasPresent = 1; 4272 samples->firstSample.biasSample = *sampleNum; 4273 4274 if (*sampleNum > 0) 4275 samples[*sampleNum].deltaTime = 0; 4276 4277 samples[*sampleNum].x = gyroOffset[0]; 4278 samples[*sampleNum].y = gyroOffset[1]; 4279 samples[*sampleNum].z = gyroOffset[2]; 4280 4281 *sampleNum += 1; 4282 } 4283 4284 #ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED 4285 overTempCalUpdateSensorEstimate(&T(overTempCal), *timestamp, gyroOffset, biasTemperature); 4286 overTempCalRemoveOffset(&T(overTempCal), *timestamp, x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap); 4287 #endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */ 4288 } else { 4289 #ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED 4290 overTempCalRemoveOffset(&T(overTempCal), *timestamp, x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap); 4291 #endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */ 4292 } 4293 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 4294 break; 4295 4296 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 4297 case MAGN: ; 4298 #ifdef LSM6DSM_MAGN_CALIB_ENABLED 4299 bool newMagnCalibData; 4300 float magnOffX, magnOffY, magnOffZ; 4301 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ 4302 4303 x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE; 4304 y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE; 4305 z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE; 4306 4307 #ifdef LSM6DSM_MAGN_CALIB_ENABLED 4308 magCalRemoveSoftiron(&T(magnCal), x_remap, y_remap, z_remap, &magnOffX, &magnOffY, &magnOffZ); 4309 newMagnCalibData = magCalUpdate(&T(magnCal), NS_TO_US(*timestamp), magnOffX, magnOffY, magnOffZ); 4310 magCalRemoveBias(&T(magnCal), magnOffX, magnOffY, magnOffZ, &x_remap, &y_remap, &z_remap); 4311 4312 if (newMagnCalibData && !samples->firstSample.biasCurrent) { 4313 samples->firstSample.biasCurrent = true; 4314 samples->firstSample.biasPresent = 1; 4315 samples->firstSample.biasSample = *sampleNum; 4316 4317 if (*sampleNum > 0) 4318 samples[*sampleNum].deltaTime = 0; 4319 4320 magCalGetBias(&T(magnCal), &samples[*sampleNum].x, &samples[*sampleNum].y, &samples[*sampleNum].z); 4321 4322 *sampleNum += 1; 4323 } 4324 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ 4325 4326 break; 4327 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 4328 4329 default: 4330 return false; 4331 } 4332 4333 if (++mSensor->samplesDecimatorCounter >= mSensor->samplesDecimator) { 4334 samples[*sampleNum].x = x_remap; 4335 samples[*sampleNum].y = y_remap; 4336 samples[*sampleNum].z = z_remap; 4337 4338 if (*sampleNum > 0) { 4339 samples[*sampleNum].deltaTime = *timestamp - mSensor->pushedTimestamp; 4340 mSensor->pushedTimestamp = *timestamp; 4341 } 4342 4343 *sampleNum += 1; 4344 4345 mSensor->samplesDecimatorCounter = 0; 4346 } 4347 4348 return true; 4349 } 4350 4351 #if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 4352 /* 4353 * lsm6dsm_processSensorOneAxisData: process single axis sensors data 4354 * @mSensor: sensor info. 4355 * @data: sensor data. 4356 * @sampleNum: number of samples in the current slab. 4357 * @timestamp: current sample timestamp; 4358 */ 4359 static bool lsm6dsm_processSensorOneAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, uint16_t *sampleNum, uint64_t *timestamp) 4360 { 4361 TDECL(); 4362 4363 if (*timestamp == 0) 4364 return false; 4365 4366 if (++mSensor->samplesDecimatorCounter >= mSensor->samplesDecimator) { 4367 if (mSensor->sADataEvt == NULL) { 4368 if (!lsm6dsm_allocateOneAxisDataEvt(mSensor, *timestamp)) 4369 return false; 4370 } 4371 4372 switch (mSensor->idx) { 4373 case PRESS: 4374 mSensor->sADataEvt->samples[*sampleNum].fdata = ((data[2] << 16) | (data[1] << 8) | data[0]) * LSM6DSM_PRESS_KSCALE; 4375 break; 4376 default: 4377 return false; 4378 } 4379 4380 if (*sampleNum > 0) { 4381 mSensor->sADataEvt->samples[*sampleNum].deltaTime = *timestamp - mSensor->pushedTimestamp; 4382 mSensor->pushedTimestamp = *timestamp; 4383 } 4384 4385 *sampleNum += 1; 4386 4387 mSensor->samplesDecimatorCounter = 0; 4388 } 4389 4390 return true; 4391 } 4392 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4393 4394 /* 4395 * lsm6dsm_pushData: push slab to nanohub 4396 * @sidx: sensor index. 4397 * @numSamples: number of samples in the slab. 4398 */ 4399 static void lsm6dsm_pushData(enum SensorIndex sidx, uint16_t *numSamples) 4400 { 4401 TDECL(); 4402 bool triaxial = true; 4403 4404 #if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 4405 if (sidx == PRESS) 4406 triaxial = false; 4407 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4408 4409 if (triaxial) { 4410 T(sensors[sidx]).tADataEvt->samples[0].firstSample.numSamples = *numSamples; 4411 osEnqueueEvtOrFree(sensorGetMyEventType(LSM6DSMSensorInfo[sidx].sensorType), T(sensors[sidx]).tADataEvt, lsm6dsm_threeAxisDataEvtFree); 4412 T(sensors[sidx]).tADataEvt = NULL; 4413 } else { 4414 #if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 4415 T(sensors[sidx]).sADataEvt->samples[0].firstSample.numSamples = *numSamples; 4416 osEnqueueEvtOrFree(sensorGetMyEventType(LSM6DSMSensorInfo[sidx].sensorType), T(sensors[sidx]).sADataEvt, lsm6dsm_oneAxisDataEvtFree); 4417 T(sensors[sidx]).sADataEvt = NULL; 4418 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4419 } 4420 4421 *numSamples = 0; 4422 } 4423 4424 /* 4425 * lsm6dsm_parseFifoData: processing FIFO data. 4426 * @data: FIFO data. 4427 * @numPattern: number of pattern inside data. 4428 */ 4429 static void lsm6dsm_parseFifoData(uint8_t *data, uint16_t numPattern) 4430 { 4431 TDECL(); 4432 uint16_t j, fifoCounter = 0, samplesCounter[FIFO_NUM] = { 0 }; 4433 struct LSM6DSMSensor *sensor; 4434 uint32_t sampleTimestamp; 4435 int32_t timestampDiffLSB; 4436 uint64_t timestamp = 0; 4437 enum SensorIndex sidx; 4438 uint8_t i, n; 4439 4440 for (j = 0; j < numPattern; j++) { 4441 for (i = 0; i < T(fifoCntl).maxMinDecimator; i++) { 4442 sampleTimestamp = ((data[fifoCounter + T(fifoCntl).timestampPosition[i] + 1] << 16) | 4443 (data[fifoCounter + T(fifoCntl).timestampPosition[i]] << 8) | 4444 data[fifoCounter + T(fifoCntl).timestampPosition[i] + 3]); 4445 4446 if (T(time).sampleTimestampFromFifoLSB > 0) { 4447 timestampDiffLSB = (int32_t)sampleTimestamp - (int32_t)(T(time).sampleTimestampFromFifoLSB & LSM6DSM_MASK_24BIT_TIMESTAMP); 4448 4449 if ((timestampDiffLSB < 0) || (timestampDiffLSB > (T(time).theoreticalDeltaTimeLSB + T(time).deltaTimeMarginLSB))) { 4450 if (timestampDiffLSB < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB) { 4451 T(time).sampleTimestampFromFifoLSB += (UINT32_MAX >> 8) + 1; 4452 } else { 4453 if (T(time).timestampIsValid) 4454 sampleTimestamp = (T(time).sampleTimestampFromFifoLSB & LSM6DSM_MASK_24BIT_TIMESTAMP) + T(time).theoreticalDeltaTimeLSB; 4455 else 4456 sampleTimestamp = 0; 4457 } 4458 } else 4459 T(time).timestampIsValid = true; 4460 } 4461 4462 T(time).sampleTimestampFromFifoLSB = (T(time).sampleTimestampFromFifoLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sampleTimestamp; 4463 4464 if (T(time).timestampIsValid) { 4465 if (!time_sync_estimate_time1(&T(time).sensorTimeToRtcData, (uint64_t)T(time).sampleTimestampFromFifoLSB * LSM6DSM_TIME_RESOLUTION, ×tamp)) { 4466 timestamp = 0; 4467 } else { 4468 if (T(time).lastSampleTimestamp > 0) { 4469 if ((int64_t)timestamp <= (int64_t)T(time).lastSampleTimestamp) 4470 timestamp = 0; 4471 } 4472 4473 T(time).lastSampleTimestamp = timestamp > 0 ? timestamp : T(time).lastSampleTimestamp; 4474 4475 } 4476 } 4477 4478 for (n = 0; n < FIFO_NUM; n++) { 4479 if ((T(fifoCntl).decimators[n] > 0) && ((i % (T(fifoCntl).decimators[n] / T(fifoCntl).minDecimator)) == 0)) { 4480 sidx = T(fifoCntl).decimatorsIdx[n]; 4481 if (sidx != EMBEDDED_TIMESTAMP) { 4482 sensor = &T(sensors[sidx]); 4483 4484 if (sensor->samplesToDiscard == 0) { 4485 if (++sensor->samplesFifoDecimatorCounter >= sensor->samplesFifoDecimator) { 4486 switch (sidx) { 4487 case GYRO: 4488 case ACCEL: 4489 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 4490 case MAGN: 4491 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 4492 lsm6dsm_processSensorThreeAxisData(sensor, &data[fifoCounter], &samplesCounter[n], ×tamp); 4493 break; 4494 4495 #if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) 4496 case PRESS: 4497 if (T(sensors[PRESS]).enabled) 4498 lsm6dsm_processSensorOneAxisData(sensor, &data[fifoCounter], &samplesCounter[n], ×tamp); 4499 4500 if (T(sensors[TEMP]).enabled) { 4501 union EmbeddedDataPoint tempData; 4502 4503 tempData.fdata = ((int16_t)(data[fifoCounter + LSM6DSM_PRESS_OUTDATA_LEN + 1] << 8) | 4504 data[fifoCounter + LSM6DSM_PRESS_OUTDATA_LEN]) * LSM6DSM_TEMP_KSCALE; 4505 4506 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL); 4507 } 4508 4509 break; 4510 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED, LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 4511 4512 default: 4513 break; 4514 } 4515 4516 sensor->samplesFifoDecimatorCounter = 0; 4517 4518 if (samplesCounter[n] >= (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE - 1)) 4519 lsm6dsm_pushData(sidx, &samplesCounter[n]); 4520 } 4521 } else 4522 sensor->samplesToDiscard--; 4523 } else { 4524 if (T(sensors[STEP_COUNTER].enabled) && !T(readSteps)) { 4525 uint16_t steps = data[fifoCounter + 4] | (data[fifoCounter + 5] << 8); 4526 4527 if (steps != T(totalNumSteps)) { 4528 union EmbeddedDataPoint stepCntData; 4529 4530 stepCntData.idata = steps; 4531 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), stepCntData.vptr, NULL); 4532 DEBUG_PRINT("Step Counter update: %ld steps\n", stepCntData.idata); 4533 T(totalNumSteps) = stepCntData.idata; 4534 } 4535 } 4536 } 4537 4538 fifoCounter += LSM6DSM_ONE_SAMPLE_BYTE; 4539 } 4540 } 4541 } 4542 } 4543 4544 for (n = 0; n < FIFO_NUM; n++) { 4545 if (samplesCounter[n]) 4546 lsm6dsm_pushData(T(fifoCntl).decimatorsIdx[n], &samplesCounter[n]); 4547 } 4548 } 4549 4550 /* 4551 * lsm6dsm_updateSyncTaskValues: read timestamp used for time calibration and temperature 4552 */ 4553 static inline void lsm6dsm_updateSyncTaskValues(void) 4554 { 4555 TDECL(); 4556 uint32_t sensorTimestamp; 4557 4558 sensorTimestamp = ((T_SLAVE_INTERFACE(timestampDataBuffer[1]) << 0) | 4559 (T_SLAVE_INTERFACE(timestampDataBuffer[2]) << 8) | 4560 (T_SLAVE_INTERFACE(timestampDataBuffer[3]) << 16)); 4561 4562 if (T(time).timestampSyncTaskLSB > 0) { 4563 if (((int32_t)sensorTimestamp - (int32_t)(T(time).timestampSyncTaskLSB & LSM6DSM_MASK_24BIT_TIMESTAMP)) < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB) 4564 T(time).timestampSyncTaskLSB += (UINT32_MAX >> 8) + 1; 4565 } 4566 4567 T(time).timestampSyncTaskLSB = (T(time).timestampSyncTaskLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sensorTimestamp; 4568 4569 time_sync_add(&T(time).sensorTimeToRtcData, T(time).timeSyncRtcTime, (uint64_t)T(time).timestampSyncTaskLSB * LSM6DSM_TIME_RESOLUTION); 4570 4571 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED) 4572 T(currentTemperature) = LSM6DSM_TEMP_OFFSET + 4573 (float)((int16_t)((T_SLAVE_INTERFACE(tempDataBuffer[2]) << 8) | T_SLAVE_INTERFACE(tempDataBuffer[1]))) / 256.0f; 4574 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */ 4575 } 4576 4577 /* 4578 * lsm6dsm_handleSpiDoneEvt: all SPI operation fall back here 4579 * @evtData: event data. 4580 */ 4581 static void lsm6dsm_handleSpiDoneEvt(const void *evtData) 4582 { 4583 TDECL(); 4584 bool returnIdle = false, resetFIFO = false; 4585 struct LSM6DSMSensor *mSensor; 4586 int i; 4587 4588 switch (GET_STATE()) { 4589 case SENSOR_BOOT: 4590 SET_STATE(SENSOR_VERIFY_WAI); 4591 4592 SPI_READ(LSM6DSM_WAI_ADDR, 1, &T_SLAVE_INTERFACE(tmpDataBuffer)); 4593 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4594 break; 4595 4596 case SENSOR_VERIFY_WAI: 4597 if (T_SLAVE_INTERFACE(tmpDataBuffer[1]) != LSM6DSM_WAI_VALUE) { 4598 T(mRetryLeft)--; 4599 if (T(mRetryLeft) == 0) 4600 break; 4601 4602 ERROR_PRINT("`Who-Am-I` register value not valid: %x\n", T_SLAVE_INTERFACE(tmpDataBuffer[1])); 4603 SET_STATE(SENSOR_BOOT); 4604 timTimerSet(100000000, 100, 100, lsm6dsm_timerCallback, NULL, true); 4605 } else { 4606 SET_STATE(SENSOR_INITIALIZATION); 4607 T(initState) = RESET_LSM6DSM; 4608 lsm6dsm_sensorInit(); 4609 } 4610 4611 break; 4612 4613 case SENSOR_INITIALIZATION: 4614 if (T(initState) == INIT_DONE) { 4615 for (i = 0; i < NUM_SENSORS; i++) { 4616 sensorRegisterInitComplete(T(sensors[i]).handle); 4617 } 4618 4619 returnIdle = true; 4620 } else 4621 lsm6dsm_sensorInit(); 4622 4623 break; 4624 4625 case SENSOR_POWERING_UP: 4626 mSensor = (struct LSM6DSMSensor *)evtData; 4627 4628 mSensor->enabled = true; 4629 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0); 4630 returnIdle = true; 4631 break; 4632 4633 case SENSOR_POWERING_DOWN: 4634 mSensor = (struct LSM6DSMSensor *)evtData; 4635 4636 mSensor->enabled = false; 4637 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0); 4638 returnIdle = true; 4639 break; 4640 4641 case SENSOR_CONFIG_CHANGING: 4642 mSensor = (struct LSM6DSMSensor *)evtData; 4643 4644 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate[mSensor->idx], mSensor->latency); 4645 returnIdle = true; 4646 break; 4647 4648 case SENSOR_CONFIG_WATERMARK_CHANGING: 4649 returnIdle = true; 4650 break; 4651 4652 case SENSOR_CALIBRATION: 4653 mSensor = (struct LSM6DSMSensor *)evtData; 4654 4655 if (T(calibrationState == CALIBRATION_COMPLETED)) { 4656 returnIdle = true; 4657 } else { 4658 lsm6dsm_runCalibrationProgram(mSensor->idx); 4659 } 4660 break; 4661 4662 case SENSOR_STORE_CALIBRATION_DATA: 4663 returnIdle = true; 4664 break; 4665 4666 case SENSOR_SELFTEST: 4667 mSensor = (struct LSM6DSMSensor *)evtData; 4668 4669 if (T(selftestState == SELFTEST_COMPLETED)) { 4670 returnIdle = true; 4671 } else { 4672 #ifdef LSM6DSM_I2C_MASTER_AK09916 4673 if (mSensor->idx == MAGN) { 4674 lsm6dsm_runAbsoluteSelfTestProgram(); 4675 } else { 4676 lsm6dsm_runGapSelfTestProgram(mSensor->idx); 4677 } 4678 #else /* LSM6DSM_I2C_MASTER_AK09916 */ 4679 lsm6dsm_runGapSelfTestProgram(mSensor->idx); 4680 #endif /* LSM6DSM_I2C_MASTER_AK09916 */ 4681 } 4682 4683 break; 4684 4685 case SENSOR_INT1_STATUS_REG_HANDLING: 4686 if (T(sensors[STEP_DETECTOR].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_DETECTED)) { 4687 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), NULL, NULL); 4688 DEBUG_PRINT("Step Detected!\n"); 4689 } 4690 4691 if (T(sensors[SIGN_MOTION].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_SIGN_MOTION)) { 4692 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), NULL, NULL); 4693 DEBUG_PRINT("Significant Motion event!\n"); 4694 } 4695 4696 if ((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_ERROR) == 0) { 4697 T(fifoDataToRead) = (((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_CTRL2_FTH_MASK) << 8) | T_SLAVE_INTERFACE(fifoStatusRegBuffer[1])) * 2; 4698 4699 if (T(fifoDataToRead) > LSM6DSM_SPI_FIFO_SIZE) { 4700 T(fifoDataToReadPending) = T(fifoDataToRead); 4701 T(fifoDataToRead) = LSM6DSM_SPI_FIFO_SIZE - (LSM6DSM_SPI_FIFO_SIZE % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE)); 4702 T(fifoDataToReadPending) -= T(fifoDataToRead); 4703 } else { 4704 T(fifoDataToReadPending) = 0; 4705 4706 if (T(fifoDataToRead) >= (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE)) 4707 T(fifoDataToRead) -= T(fifoDataToRead) % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE); 4708 else 4709 T(fifoDataToRead) = 0; 4710 } 4711 4712 if (T(fifoDataToRead) > 0) { 4713 if (T(time).status == TIME_SYNC_DURING_FIFO_READ) { 4714 uint64_t time = sensorGetTime(); 4715 if ((time - T(time).noTimer.lastTimestampDataAvlRtcTime) > LSM6DSM_SYNC_DELTA_INTERVAL) { 4716 T(time).noTimer.newTimestampDataAvl = true; 4717 T(time).noTimer.lastTimestampDataAvlRtcTime = time; 4718 4719 SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBuffer)); 4720 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED) 4721 SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer)); 4722 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */ 4723 } 4724 } 4725 4726 SPI_READ(LSM6DSM_FIFO_DATA_OUT_L_ADDR, T(fifoDataToRead), &T_SLAVE_INTERFACE(fifoDataBuffer)); 4727 } 4728 } else { 4729 T(fifoDataToRead) = 0; 4730 4731 if ((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART) || 4732 (T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN)) { 4733 resetFIFO = true; 4734 SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_BYPASS_MODE, 25); 4735 SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_CONTINUOS_MODE); 4736 } 4737 4738 if (T(sensors[STEP_COUNTER].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA)) { 4739 T(readSteps) = true; 4740 SPI_READ(LSM6DSM_STEP_COUNTER_L_ADDR, 2, &T_SLAVE_INTERFACE(stepCounterDataBuffer)); 4741 } 4742 } 4743 4744 if (!T(readSteps) && (T(fifoDataToRead) == 0)) { 4745 for (i = 0; i < NUM_SENSORS; i++) { 4746 if (T(sendFlushEvt[i])) { 4747 osEnqueueEvt(sensorGetMyEventType(LSM6DSMSensorInfo[i].sensorType), SENSOR_DATA_EVENT_FLUSH, NULL); 4748 T(sendFlushEvt[i]) = false; 4749 } 4750 } 4751 4752 if (resetFIFO) { 4753 SET_STATE(SENSOR_INVALID_STATE); 4754 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4755 } else 4756 returnIdle = true; 4757 4758 break; 4759 } 4760 4761 SET_STATE(SENSOR_INT1_OUTPUT_DATA_HANDLING); 4762 4763 if (T(fifoDataToRead) > 0) { 4764 T(lastFifoReadTimestamp) = sensorGetTime(); 4765 4766 if (T(time).noTimer.newTimestampDataAvl) 4767 T(time).timeSyncRtcTime = T(lastFifoReadTimestamp); 4768 } 4769 4770 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4771 4772 break; 4773 4774 case SENSOR_INT1_OUTPUT_DATA_HANDLING: 4775 if (T(fifoDataToRead) > 0) { 4776 if (T(time).noTimer.newTimestampDataAvl) { 4777 T(time).noTimer.newTimestampDataAvl = false; 4778 lsm6dsm_updateSyncTaskValues(); 4779 } 4780 4781 lsm6dsm_parseFifoData(&T_SLAVE_INTERFACE(fifoDataBuffer[1]), (T(fifoDataToRead) / 6) / T(fifoCntl).totalSip); 4782 4783 if (T(fifoDataToReadPending) > 0) { 4784 T(fifoDataToRead) = T(fifoDataToReadPending); 4785 4786 if (T(fifoDataToRead) > LSM6DSM_SPI_FIFO_SIZE) { 4787 T(fifoDataToReadPending) = T(fifoDataToRead); 4788 T(fifoDataToRead) = LSM6DSM_SPI_FIFO_SIZE - (LSM6DSM_SPI_FIFO_SIZE % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE)); 4789 T(fifoDataToReadPending) -= T(fifoDataToRead); 4790 } else { 4791 T(fifoDataToReadPending) = 0; 4792 4793 if (T(fifoDataToRead) >= (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE)) 4794 T(fifoDataToRead) -= T(fifoDataToRead) % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE); 4795 else 4796 T(fifoDataToRead) = 0; 4797 } 4798 4799 if (T(fifoDataToRead) > 0) { 4800 SPI_READ(LSM6DSM_FIFO_DATA_OUT_L_ADDR, T(fifoDataToRead), &T_SLAVE_INTERFACE(fifoDataBuffer)); 4801 lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__); 4802 return; 4803 } 4804 } else 4805 T(fifoDataToRead) = 0; 4806 } 4807 4808 for (i = 0; i < NUM_SENSORS; i++) { 4809 if (T(sendFlushEvt[i])) { 4810 osEnqueueEvt(sensorGetMyEventType(LSM6DSMSensorInfo[i].sensorType), SENSOR_DATA_EVENT_FLUSH, NULL); 4811 T(sendFlushEvt[i]) = false; 4812 } 4813 } 4814 4815 if (T(readSteps)) { 4816 union EmbeddedDataPoint stepCntData; 4817 4818 stepCntData.idata = T_SLAVE_INTERFACE(stepCounterDataBuffer[1]) | (T_SLAVE_INTERFACE(stepCounterDataBuffer[2]) << 8); 4819 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), stepCntData.vptr, NULL); 4820 DEBUG_PRINT("Step Counter update: %ld steps\n", stepCntData.idata); 4821 T(totalNumSteps) = stepCntData.idata; 4822 T(readSteps) = false; 4823 } 4824 4825 returnIdle = true; 4826 break; 4827 4828 case SENSOR_TIME_SYNC: ; 4829 lsm6dsm_updateSyncTaskValues(); 4830 4831 if (T(time).status == TIME_SYNC_TIMER) { 4832 if (timTimerSet(LSM6DSM_SYNC_DELTA_INTERVAL, 100, 100, lsm6dsm_timerSyncCallback, NULL, true) == 0) 4833 ERROR_PRINT("Failed to set a timer for time sync\n"); 4834 } 4835 4836 returnIdle = true; 4837 break; 4838 4839 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 4840 case SENSOR_BARO_READ_DATA: ; 4841 uint16_t samplesCounter = 0; 4842 uint32_t sensorTimestamp; 4843 uint64_t timestamp; 4844 4845 sensorTimestamp = ((T_SLAVE_INTERFACE(timestampDataBufferBaro[1]) << 0) | 4846 (T_SLAVE_INTERFACE(timestampDataBufferBaro[2]) << 8) | 4847 (T_SLAVE_INTERFACE(timestampDataBufferBaro[3]) << 16)); 4848 4849 if (T(time).timestampBaroLSB > 0) { 4850 if (((int32_t)sensorTimestamp - (int32_t)(T(time).timestampBaroLSB & LSM6DSM_MASK_24BIT_TIMESTAMP)) < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB) 4851 T(time).timestampBaroLSB += (UINT32_MAX >> 8) + 1; 4852 } 4853 4854 T(time).timestampBaroLSB = (T(time).timestampBaroLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sensorTimestamp; 4855 4856 if (time_sync_estimate_time1(&T(time).sensorTimeToRtcData, (uint64_t)T(time).timestampBaroLSB * LSM6DSM_TIME_RESOLUTION, ×tamp)) { 4857 if (T(sensors[PRESS]).enabled) { 4858 lsm6dsm_processSensorOneAxisData(&T(sensors[PRESS]), &T_SLAVE_INTERFACE(baroDataBuffer[1]), &samplesCounter, ×tamp); 4859 lsm6dsm_pushData(PRESS, &samplesCounter); 4860 } 4861 } 4862 4863 if (T(sensors[TEMP]).enabled) { 4864 union EmbeddedDataPoint tempData; 4865 4866 tempData.fdata = ((int16_t)(T_SLAVE_INTERFACE(baroDataBuffer[LSM6DSM_PRESS_OUTDATA_LEN + 2]) << 8) | 4867 T_SLAVE_INTERFACE(baroDataBuffer[LSM6DSM_PRESS_OUTDATA_LEN + 1])) * LSM6DSM_TEMP_KSCALE; 4868 4869 osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL); 4870 } 4871 4872 returnIdle = true; 4873 break; 4874 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4875 4876 default: 4877 returnIdle = true; 4878 break; 4879 } 4880 4881 if (returnIdle) 4882 lsm6dsm_processPendingEvt(); 4883 } 4884 4885 /* 4886 * lsm6dsm_handleEvent: handle driver events 4887 * @evtType: event type. 4888 * @evtData: event data. 4889 */ 4890 static void lsm6dsm_handleEvent(uint32_t evtType, const void *evtData) 4891 { 4892 TDECL(); 4893 struct LSM6DSMSensor *mSensor; 4894 4895 switch (evtType) { 4896 case EVT_APP_START: ; 4897 uint64_t currTime; 4898 4899 T(mRetryLeft) = LSM6DSM_RETRY_CNT_WAI; 4900 SET_STATE(SENSOR_BOOT); 4901 osEventUnsubscribe(T(tid), EVT_APP_START); 4902 4903 /* Sensor need 100ms to boot, use a timer callback to continue */ 4904 currTime = timGetTime(); 4905 if (currTime < 100000000ULL) { 4906 timTimerSet(100000000 - currTime, 100, 100, lsm6dsm_timerCallback, NULL, true); 4907 break; 4908 } 4909 4910 /* If 100ms already passed just fall through next step */ 4911 case EVT_SPI_DONE: 4912 lsm6dsm_handleSpiDoneEvt(evtData); 4913 break; 4914 4915 case EVT_SENSOR_INTERRUPT_1: 4916 lsm6dsm_readStatusReg(false); 4917 break; 4918 4919 case EVT_SENSOR_POWERING_UP: 4920 mSensor = (struct LSM6DSMSensor *)evtData; 4921 4922 mSensor->enabled = true; 4923 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0); 4924 lsm6dsm_processPendingEvt(); 4925 break; 4926 4927 case EVT_SENSOR_POWERING_DOWN: 4928 mSensor = (struct LSM6DSMSensor *)evtData; 4929 4930 mSensor->enabled = false; 4931 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0); 4932 lsm6dsm_processPendingEvt(); 4933 break; 4934 4935 case EVT_SENSOR_CONFIG_CHANGING: 4936 mSensor = (struct LSM6DSMSensor *)evtData; 4937 4938 sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate[mSensor->idx], mSensor->latency); 4939 lsm6dsm_processPendingEvt(); 4940 break; 4941 4942 case EVT_APP_FROM_HOST: 4943 break; 4944 4945 case EVT_SENSOR_RESTORE_IDLE: 4946 lsm6dsm_processPendingEvt(); 4947 break; 4948 4949 case EVT_TIME_SYNC: 4950 lsm6dsm_timeSyncTask(); 4951 break; 4952 4953 default: 4954 break; 4955 } 4956 } 4957 4958 /* 4959 * lsm6dsm_initSensorStruct: initialize sensor struct variable 4960 * @sensor: sensor info. 4961 * @idx: sensor index. 4962 */ 4963 static void lsm6dsm_initSensorStruct(struct LSM6DSMSensor *sensor, enum SensorIndex idx) 4964 { 4965 TDECL(); 4966 uint8_t i; 4967 4968 for (i = 0; i < NUM_SENSORS; i++) { 4969 if (i == idx) 4970 sensor->dependenciesRequireData[i] = true; 4971 else 4972 sensor->dependenciesRequireData[i] = false; 4973 4974 sensor->rate[i] = 0; 4975 } 4976 4977 sensor->idx = idx; 4978 sensor->hwRate = 0; 4979 sensor->latency = UINT64_MAX; 4980 sensor->enabled = false; 4981 sensor->samplesToDiscard = 0; 4982 sensor->samplesDecimator = 1; 4983 sensor->samplesDecimatorCounter = 0; 4984 sensor->samplesFifoDecimator = 1; 4985 sensor->samplesFifoDecimatorCounter = 0; 4986 sensor->tADataEvt = NULL; 4987 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 4988 sensor->sADataEvt = NULL; 4989 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 4990 } 4991 4992 /* 4993 * lsm6dsm_startTask: first function executed when App start 4994 * @taskId: task id. 4995 */ 4996 static bool lsm6dsm_startTask(uint32_t taskId) 4997 { 4998 TDECL(); 4999 enum SensorIndex i; 5000 size_t slabSize; 5001 int err; 5002 5003 DEBUG_PRINT("IMU: %lu\n", taskId); 5004 5005 T(tid) = taskId; 5006 T(int1) = gpioRequest(LSM6DSM_INT1_GPIO); 5007 T(isr1).func = lsm6dsm_isr1; 5008 5009 T_SLAVE_INTERFACE(mode).speed = LSM6DSM_SPI_SLAVE_FREQUENCY_HZ; 5010 T_SLAVE_INTERFACE(mode).bitsPerWord = 8; 5011 T_SLAVE_INTERFACE(mode).cpol = SPI_CPOL_IDLE_HI; 5012 T_SLAVE_INTERFACE(mode).cpha = SPI_CPHA_TRAILING_EDGE; 5013 T_SLAVE_INTERFACE(mode).nssChange = true; 5014 T_SLAVE_INTERFACE(mode).format = SPI_FORMAT_MSB_FIRST; 5015 T_SLAVE_INTERFACE(cs) = LSM6DSM_SPI_SLAVE_CS_GPIO; 5016 5017 DEBUG_PRINT("Requested SPI on bus #%d @%dHz, int1 on gpio#%d\n", 5018 LSM6DSM_SPI_SLAVE_BUS_ID, LSM6DSM_SPI_SLAVE_FREQUENCY_HZ, LSM6DSM_INT1_GPIO); 5019 5020 err = spiMasterRequest(LSM6DSM_SPI_SLAVE_BUS_ID, &T_SLAVE_INTERFACE(spiDev)); 5021 if (err < 0) { 5022 ERROR_PRINT("Failed to request SPI on this bus: #%d\n", LSM6DSM_SPI_SLAVE_BUS_ID); 5023 return false; 5024 } 5025 5026 T(int1Register) = LSM6DSM_INT1_CTRL_BASE; 5027 T(int2Register) = LSM6DSM_INT2_CTRL_BASE; 5028 T(embeddedFunctionsRegister) = LSM6DSM_CTRL10_C_BASE; 5029 T(pedometerDependencies) = 0; 5030 T(pendingInt) = false; 5031 T(pendingTimeSyncTask) = false; 5032 T(lastFifoReadTimestamp) = 0; 5033 T(totalNumSteps) = 0; 5034 T(time).status = TIME_SYNC_DISABLED; 5035 #if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED) 5036 T(currentTemperature) = 0; 5037 #endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */ 5038 #ifdef LSM6DSM_I2C_MASTER_ENABLED 5039 T(masterConfigRegister) = LSM6DSM_MASTER_CONFIG_BASE; 5040 T(masterConfigDependencies) = 0; 5041 #endif /* LSM6DSM_I2C_MASTER_ENABLED */ 5042 #if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) 5043 T(baroTimerId) = 0; 5044 T(pendingBaroTimerTask) = false; 5045 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 5046 memset(T(gyroCalibrationData), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t)); 5047 5048 slabSize = sizeof(struct TripleAxisDataEvent) + (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE * sizeof(struct TripleAxisDataPoint)); 5049 5050 T(mDataSlabThreeAxis) = slabAllocatorNew(slabSize, 4, 20); 5051 if (!T(mDataSlabThreeAxis)) { 5052 ERROR_PRINT("Failed to allocate mDataSlabThreeAxis memory\n"); 5053 spiMasterRelease(T_SLAVE_INTERFACE(spiDev)); 5054 return false; 5055 } 5056 5057 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 5058 slabSize = sizeof(struct SingleAxisDataEvent) + (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE * sizeof(struct SingleAxisDataPoint)); 5059 5060 T(mDataSlabOneAxis) = slabAllocatorNew(slabSize, 4, 20); 5061 if (!T(mDataSlabOneAxis)) { 5062 ERROR_PRINT("Failed to allocate mDataSlabOneAxis memory\n"); 5063 slabAllocatorDestroy(T(mDataSlabThreeAxis)); 5064 spiMasterRelease(T_SLAVE_INTERFACE(spiDev)); 5065 return false; 5066 } 5067 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 5068 5069 for (i = 0; i < NUM_SENSORS; i++) { 5070 T(pendingEnableConfig[i]) = false; 5071 T(pendingRateConfig[i]) = false; 5072 T(pendingFlush[i]) = 0; 5073 T(sendFlushEvt[i]) = false; 5074 lsm6dsm_initSensorStruct(&T(sensors[i]), i); 5075 T(sensors[i]).handle = sensorRegister(&LSM6DSMSensorInfo[i], &LSM6DSMSensorOps[i], NULL, false); 5076 } 5077 5078 T(fifoCntl).decimatorsIdx[FIFO_GYRO] = GYRO; 5079 T(fifoCntl).decimatorsIdx[FIFO_ACCEL] = ACCEL; 5080 T(fifoCntl).decimatorsIdx[FIFO_DS3] = NUM_SENSORS; 5081 T(fifoCntl).decimatorsIdx[FIFO_DS4] = EMBEDDED_TIMESTAMP; 5082 5083 #ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED 5084 T(fifoCntl).decimatorsIdx[FIFO_DS3] = MAGN; 5085 #else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 5086 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 5087 T(fifoCntl).decimatorsIdx[FIFO_DS3] = PRESS; 5088 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 5089 #endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */ 5090 5091 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED 5092 accelCalInit(&T(accelCal), 5093 800000000, /* stillness period [ns] */ 5094 5, /* minimum sample number */ 5095 0.00025, /* threshold */ 5096 15, /* nx bucket count */ 5097 15, /* nxb bucket count */ 5098 15, /* ny bucket count */ 5099 15, /* nyb bucket count */ 5100 15, /* nz bucket count */ 5101 15, /* nzb bucket count */ 5102 15); /* nle bucket count */ 5103 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */ 5104 5105 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 5106 gyroCalInit(&T(gyroCal), 5107 5e9, /* min stillness period [ns] */ 5108 6e9, /* max stillness period [ns] */ 5109 0, 0, 0, /* initial bias offset calibration values [rad/sec] */ 5110 0, /* timestamp of initial bias calibration [ns] */ 5111 1.5e9, /* analysis window length = 1.5 seconds [ns] */ 5112 5e-5f, /* gyroscope variance threshold [(rad/sec)^2] */ 5113 1e-5f, /* gyroscope confidence delta [(rad/sec)^2] */ 5114 8e-3f, /* accelerometer variance threshold [(m/sec^2)^2] */ 5115 1.6e-3f, /* accelerometer confidence delta [(m/sec^2)^2] */ 5116 1.4f, /* magnetometer variance threshold [uT^2] */ 5117 0.25, /* magnetometer confidence delta [uT^2] */ 5118 0.95f, /* stillness threshold [0, 1] */ 5119 40.0e-3f * M_PI / 180.0f, /* stillness mean variation limit [rad/sec] */ 5120 1.5f, /* maximum temperature deviation during stillness */ 5121 true); /* gyro calibration enable */ 5122 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 5123 5124 #ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED 5125 overTempCalInit(&T(overTempCal), 5126 5, /* min num of points to enable model update */ 5127 5000000000, /* min model update interval [ns] */ 5128 0.75f, /* temperature span of bin method [C] */ 5129 50.0e-3f * M_PI / 180.0f, /* model fit tolerance [rad/sec/] */ 5130 172800000000000, /* model data point age limit [ns] */ 5131 50.0e-3f * M_PI / 180.0f, /* limit for temperature sensitivity [(rad/sec)/C] */ 5132 3.0f * M_PI / 180.0f, /* limit for model intercept parameter [rad/sec] */ 5133 true); /* over temperature compensation enable */ 5134 #endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */ 5135 5136 #ifdef LSM6DSM_MAGN_CALIB_ENABLED 5137 #ifdef DIVERSITY_CHECK_ENABLED 5138 initMagCal(&T(magnCal), 5139 0.0f, 0.0f, 0.0f, /* magn offset x - y - z */ 5140 1.0f, 0.0f, 0.0f, /* magn scale matrix c00 - c01 - c02 */ 5141 0.0f, 1.0f, 0.0f, /* magn scale matrix c10 - c11 - c12 */ 5142 0.0f, 0.0f, 1.0f, /* magn scale matrix c20 - c21 - c22 */ 5143 3000000, /* min_batch_window_in_micros */ 5144 8, /* min_num_diverse_vectors */ 5145 1, /* max_num_max_distance */ 5146 6.0f, /* var_threshold */ 5147 10.0f, /* max_min_threshold */ 5148 48.f, /* local_field */ 5149 0.5f, /* threshold_tuning_param */ 5150 2.552f); /* max_distance_tuning_param */ 5151 #else 5152 initMagCal(&T(magnCal), 5153 0.0f, 0.0f, 0.0f, /* magn offset x - y - z */ 5154 1.0f, 0.0f, 0.0f, /* magn scale matrix c00 - c01 - c02 */ 5155 0.0f, 1.0f, 0.0f, /* magn scale matrix c10 - c11 - c12 */ 5156 0.0f, 0.0f, 1.0f, /* magn scale matrix c20 - c21 - c22 */ 5157 3000000); /* min_batch_window_in_micros */ 5158 #endif 5159 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ 5160 5161 /* Initialize index used to fill/get data from buffer */ 5162 T_SLAVE_INTERFACE(mWbufCnt) = 0; 5163 T_SLAVE_INTERFACE(mRegCnt) = 0; 5164 5165 time_sync_init(&T(time).sensorTimeToRtcData); 5166 5167 osEventSubscribe(T(tid), EVT_APP_START); 5168 5169 DEBUG_PRINT("Enabling gpio#%d connected to int1\n", LSM6DSM_INT1_GPIO); 5170 lsm6dsm_enableInterrupt(T(int1), &T(isr1)); 5171 5172 return true; 5173 } 5174 5175 /* 5176 * lsm6dsm_endTask: last function executed when App end 5177 */ 5178 static void lsm6dsm_endTask(void) 5179 { 5180 TDECL(); 5181 enum SensorIndex i; 5182 5183 #ifdef LSM6DSM_ACCEL_CALIB_ENABLED 5184 accelCalDestroy(&T(accelCal)); 5185 #endif /* LSM6DSM_ACCEL_CALIB_ENABLED */ 5186 #ifdef LSM6DSM_MAGN_CALIB_ENABLED 5187 magCalDestroy(&T(magnCal)); 5188 #endif /* LSM6DSM_MAGN_CALIB_ENABLED */ 5189 #ifdef LSM6DSM_GYRO_CALIB_ENABLED 5190 gyroCalDestroy(&T(gyroCal)); 5191 #endif /* LSM6DSM_GYRO_CALIB_ENABLED */ 5192 5193 lsm6dsm_disableInterrupt(T(int1), &T(isr1)); 5194 #ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED 5195 slabAllocatorDestroy(T(mDataSlabOneAxis)); 5196 #endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */ 5197 slabAllocatorDestroy(T(mDataSlabThreeAxis)); 5198 spiMasterRelease(T_SLAVE_INTERFACE(spiDev)); 5199 5200 for (i = 0; i < NUM_SENSORS; i++) 5201 sensorUnregister(T(sensors[i]).handle); 5202 5203 gpioRelease(T(int1)); 5204 } 5205 5206 INTERNAL_APP_INIT(LSM6DSM_APP_ID, LSM6DSM_APP_VERSION, lsm6dsm_startTask, lsm6dsm_endTask, lsm6dsm_handleEvent); 5207
