/* * Copyright (C) 2018 InvenSense Inc. * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and * limitations under the License. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef ACCEL_CAL_ENABLED #include #endif #ifdef GYRO_CAL_ENABLED #include #endif #define INFO_PRINT(fmt, ...) do { \ osLog(LOG_INFO, "%s " fmt, "[ICM40600]", ##__VA_ARGS__); \ } while (0); #define ERROR_PRINT(fmt, ...) do { \ osLog(LOG_ERROR, "%s " fmt, "[ICM40600] ERROR:", ##__VA_ARGS__); \ } while (0); #define DEBUG_PRINT(fmt, ...) do { \ if (DBG_ENABLE) { \ INFO_PRINT(fmt, ##__VA_ARGS__); \ } \ } while (0); #define DEBUG_PRINT_IF(cond, fmt, ...) do { \ if ((cond) && DBG_ENABLE) { \ INFO_PRINT(fmt, ##__VA_ARGS__); \ } \ } while (0); #define DBG_ENABLE 0 #define DBG_STATE 0 #define DBG_TIMESTAMP 0 #define ICM40600_APP_ID APP_ID_MAKE(NANOHUB_VENDOR_INVENSENSE, 0x2) #define ICM40600_APP_VERSION 1 #define ICM40600_SPI_WRITE 0x00 #define ICM40600_SPI_READ 0x80 // SPI speeds officialy supported: 5MHz (low speed), 17MHz (high speed) #define ICM40600_SPI_LOW_SPEED_HZ 5000000 #define ICM40600_SPI_HIGH_SPEED_HZ 17000000 #define ICM40600_SPI_DEFAULT_SPEED_HZ ICM40600_SPI_LOW_SPEED_HZ #define ICM40600_SPI_MODE 3 #ifndef ICM40600_SPI_BUS_ID #define ICM40600_SPI_BUS_ID 1 #endif #ifndef ICM40600_INT1_PIN #define ICM40600_INT1_PIN GPIO_PB(6) #endif #ifndef ICM40600_INT1_IRQ #define ICM40600_INT1_IRQ EXTI9_5_IRQn #endif #define ICM40600_SPI_SPEED_HZ ICM40600_SPI_HIGH_SPEED_HZ // set SPI speed register value #if ICM40600_SPI_SPEED_HZ == ICM40600_SPI_LOW_SPEED_HZ #define ICM40600_SPI_SPEED_REG_VALUE BIT_SPI_SPEED_5MHZ #elif ICM40600_SPI_SPEED_HZ == ICM40600_SPI_HIGH_SPEED_HZ #define ICM40600_SPI_SPEED_REG_VALUE BIT_SPI_SPEED_17MHZ #else #error "Invalid ICM40600_SPI_SPEED_HZ setting: valid values are 5MHz or 17MHz" #endif /* ********************** * Chip configuration ********************** */ /* Full-scale range */ // Default accel range is 8g #ifndef ICM40600_ACC_RANGE_G #define ICM40600_ACC_RANGE_G 8 #endif // Default gyro range is 2000dps #ifndef ICM40600_GYR_RANGE_DPS #define ICM40600_GYR_RANGE_DPS 2000 #endif // 0 -> +-16g, 1 -> +-8g, 2 -> +-4g, 3 -> +-2g, 4 -> +-1g #if ICM40600_ACC_RANGE_G == 16 #define ICM40600_ACC_FS 0 #elif ICM40600_ACC_RANGE_G == 8 #define ICM40600_ACC_FS 1 #elif ICM40600_ACC_RANGE_G == 4 #define ICM40600_ACC_FS 2 #else #error "Invalid ICM40600_ACC_RANGE_G setting: valid values are 16, 8, 4 (no HiFi)" #endif // 0 -> +-2000dps, 1 -> +-1000dps, 2 -> +-500dps, 3 -> +-250dps, 4 -> +-125dps, 5 -> +-62.5dps, 6 -> +-31.25dps, 7 -> +-15.6225dps #if ICM40600_GYR_RANGE_DPS == 2000 #define ICM40600_GYR_FS 0 #elif ICM40600_GYR_RANGE_DPS == 1000 #define ICM40600_GYR_FS 1 #else #error "Invalid ICM40600_GYR_RANGE_DPS setting: valid values are 2000, 1000" #endif // Bandwidth for low-pass filters, using ODR/2 FIR #define ICM40600_ACC_BW_IND BIT_ACCEL_UI_BW_2_FIR #define ICM40600_GYR_BW_IND BIT_GYRO_UI_BW_2_FIR /* INT1 configuration */ // Polarity: 0 -> Active Low, 1 -> Active High #define INT1_POLARITY 1 // Drive circuit: 0 -> Open Drain, 1 -> Push-Pull, fixed for INT1 do not change! #define INT1_DRIVE_CIRCUIT 1 // Mode: 0 -> Pulse, 1 -> Latch #define INT1_MODE 0 /* Wake-on-Motion/No-Motion */ #define ICM40600_WOM_THRESHOLD_MG 160 // 160mg @ 25Hz #define ICM40600_WOM_COMPUTE(val_mg) ((256 * val_mg) / 1000) // No-Motion duration: 5s #define ICM40600_NOM_DURATION_NS (5 * 1000000000ULL) /* ********************** * Factory calibration ********************** */ #define CALIBRATION_ODR 7 // 200Hz (support both LPM and LNM) #define CALIBRATION_ACC_FS 0 // +-16g (= resolution of offset register) #define CALIBRATION_GYR_FS 1 // +-1000dps (= resolution of offset register) #define CALIBRATION_ACC_1G 2048 // LSB/g @+-16g #define CALIBRATION_ACC_BW_IND BIT_ACCEL_UI_BW_4_IIR #define CALIBRATION_GYR_BW_IND BIT_GYRO_UI_BW_4_IIR #define CALIBRATION_READ_INTERVAL_US (200 * 1000) // 200ms (200/5ms=40 packets -> 40 * 16 = 640 bytes) #define CALIBRATION_SAMPLE_NB 200 // 200/40packets = 5 loops #define RETRY_CNT_CALIBRATION 10 // > 5 loops /* ********************** * Selftest ********************** */ #define SELF_TEST_ODR 6 // 1000Hz #define SELF_TEST_ACC_FS 3 // +-2g #define SELF_TEST_GYR_FS 3 // +-250dps #define SELF_TEST_ACC_BW_IND BIT_ACCEL_UI_BW_10_IIR #define SELF_TEST_GYR_BW_IND BIT_GYRO_UI_BW_10_IIR #define SELF_TEST_MIN_ACC_MG 225 // mg #define SELF_TEST_MAX_ACC_MG 675 // mg #define SELF_TEST_MIN_GYR_DPS 60 // dps #define SELF_TEST_MAX_GYR_OFF_DPS 20 // dps #define SELF_TEST_ACC_SHIFT_DELTA 500 // = 0.5 #define SELF_TEST_GYR_SHIFT_DELTA 500 // = 0.5 #define SELF_TEST_ACC_SCALE 32768 / (16 >> SELF_TEST_ACC_FS) / 1000 #define SELF_TEST_GYR_SCALE 32768 / (2000 >> SELF_TEST_GYR_FS) #define SELF_TEST_READ_INTERVAL_US (20 * 1000) // 20ms (20/1ms=20 packets -> 20 * 16 = 320 bytes) #define SELF_TEST_SAMPLE_NB 200 // 200/20packets = 10 loops #define RETRY_CNT_SELF_TEST 20 // > 10 loops #define SELF_TEST_PRECISION 1000 #define SELF_TEST_SETTLE_TIME_MS 25 #define SELF_TEST_MIN_ACC (SELF_TEST_MIN_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION) #define SELF_TEST_MAX_ACC (SELF_TEST_MAX_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION) #define SELF_TEST_MIN_GYR (SELF_TEST_MIN_GYR_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION) #define SELF_TEST_MAX_GYR (SELF_TEST_MAX_GYR_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION) #define SELF_TEST_MAX_GYR_OFFSET (SELF_TEST_MAX_GYR_OFF_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION) /* **************** * register map * **************** */ /* Bank 0 */ #define REG_DEVICE_CONFIG 0x11 #define REG_SPI_SPEED 0x13 #define REG_INT_CONFIG 0x14 #define REG_FIFO_CONFIG 0x16 #define REG_INT_STATUS 0x2D #define REG_FIFO_BYTE_COUNT1 0x2E #define REG_FIFO_BYTE_COUNT2 0x2F #define REG_FIFO_DATA 0x30 #define REG_SIGNAL_PATH_RESET 0x4B #define REG_INTF_CONFIG0 0x4C #define REG_PWR_MGMT_0 0x4E #define REG_GYRO_CONFIG0 0x4F #define REG_ACCEL_CONFIG0 0x50 #define REG_GYRO_CONFIG1 0x51 #define REG_GYRO_ACCEL_CONFIG0 0x52 #define REG_ACCEL_CONFIG1 0x53 #define REG_ACCEL_WOM_X_THR 0x54 #define REG_ACCEL_WOM_Y_THR 0x55 #define REG_ACCEL_WOM_Z_THR 0x56 #define REG_SMD_CONFIG 0x57 #define REG_INT_STATUS2 0x59 #define REG_TMST_CONFIG 0x5A #define REG_FIFO_CONFIG1 0x5F #define REG_FIFO_CONFIG2 0x60 #define REG_FIFO_CONFIG3 0x61 #define REG_INT_CONFIG0 0x63 #define REG_INT_CONFIG1 0x64 #define REG_INT_SOURCE0 0x65 #define REG_INT_SOURCE1 0x66 #define REG_SELF_TEST_CONFIG 0x70 #define REG_WHO_AM_I 0x75 #define REG_REG_BANK_SEL 0x76 #define REG_GOS_USER0 0x77 #define REG_GOS_USER1 0x78 #define REG_GOS_USER2 0x79 #define REG_GOS_USER3 0x7A #define REG_GOS_USER4 0x7B #define REG_GOS_USER5 0x7C #define REG_GOS_USER6 0x7D #define REG_GOS_USER7 0x7E #define REG_GOS_USER8 0x7F /* Bank 1 */ #define REG_XG_ST_DATA 0x5F #define REG_YG_ST_DATA 0x60 #define REG_ZG_ST_DATA 0x61 /* Bank 2 */ #define REG_XA_ST_DATA 0x3B #define REG_YA_ST_DATA 0x3C #define REG_ZA_ST_DATA 0x3D /* REG_WHO_AM_I */ #define WHO_AM_I_ICM40604 0x32 #define WHO_AM_I_ICM40605 0x33 /* REG_REG_BANK_SEL */ #define BIT_BANK_SEL_0 0x00 #define BIT_BANK_SEL_1 0x01 #define BIT_BANK_SEL_2 0x02 /* REG_DEVICE_CONFIG */ #define BIT_SOFT_RESET 0x01 /* REG_SPI_SPEED */ #define BIT_SPI_SPEED_5MHZ 0x03 #define BIT_SPI_SPEED_17MHZ 0x05 /* REG_GYRO_CONFIG0/REG_ACCEL_CONFIG0 */ #define SHIFT_GYRO_FS_SEL 5 #define SHIFT_ACCEL_FS_SEL 5 #define SHIFT_ODR_CONF 0 /* REG_INT_CONFIG */ #define SHIFT_INT1_POLARITY 0 #define SHIFT_INT1_DRIVE_CIRCUIT 1 #define SHIFT_INT1_MODE 2 /* REG_PWR_MGMT_0 */ #define BIT_TEMP_DIS 0x20 #define BIT_IDLE 0x10 #define BIT_GYRO_MODE_OFF 0x00 #define BIT_GYRO_MODE_STBY 0x04 #define BIT_GYRO_MODE_LN 0x0C #define BIT_ACCEL_MODE_OFF 0x00 #define BIT_ACCEL_MODE_LN 0x03 /* REG_SIGNAL_PATH_RESET */ #define BIT_FIFO_FLUSH 0x02 /* REG_INTF_CONFIG0 */ #define BIT_FIFO_COUNT_REC 0x40 #define BIT_COUNT_BIG_ENDIAN 0x20 #define BIT_SENS_DATA_BIG_ENDIAN 0x10 #define BIT_UI_SIFS_DISABLE_SPI 0x02 #define BIT_UI_SIFS_DISABLE_I2C 0x03 /* REG_FIFO_CONFIG1 */ #define BIT_FIFO_ACCEL_EN 0x01 #define BIT_FIFO_GYRO_EN 0x02 #define BIT_FIFO_TEMP_EN 0x04 #define BIT_FIFO_TMST_FSYNC_EN 0x08 #define BIT_FIFO_HIRES_EN 0x10 #define BIT_FIFO_WM_TH 0x20 #define BIT_FIFO_RESUME_PART_RD 0x40 /* REG_INT_CONFIG1 */ #define BIT_INT_ASY_RST_DISABLE 0x10 /* REG_INT_SOURCE0 */ #define BIT_INT_UI_AGC_RDY_INT1_EN 0x01 #define BIT_INT_FIFO_FULL_INT1_EN 0x02 #define BIT_INT_FIFO_THS_INT1_EN 0x04 #define BIT_INT_UI_DRDY_INT1_EN 0x08 #define BIT_INT_RESET_DONE_INT1_EN 0x10 #define BIT_INT_PLL_RDY_INT1_EN 0x20 #define BIT_INT_UI_FSYNC_INT1_EN 0x40 /* REG_INT_SOURCE1 */ #define BIT_INT_WOM_X_INT1_EN 0x01 #define BIT_INT_WOM_Y_INT1_EN 0x02 #define BIT_INT_WOM_Z_INT1_EN 0x04 #define BIT_INT_SMD_INT1_EN 0x08 #define BIT_INT_WOM_XYZ_INT1_EN (BIT_INT_WOM_X_INT1_EN | BIT_INT_WOM_Y_INT1_EN | BIT_INT_WOM_Z_INT1_EN) /* REG_SELF_TEST_CONFIG */ #define BIT_SELF_TEST_REGULATOR_EN 0x40 #define BIT_TEST_AZ_EN 0x20 #define BIT_TEST_AY_EN 0x10 #define BIT_TEST_AX_EN 0x08 #define BIT_TEST_GZ_EN 0x04 #define BIT_TEST_GY_EN 0x02 #define BIT_TEST_GX_EN 0x01 /* REG_INT_STATUS */ #define BIT_INT_STATUS_AGC_RDY 0x01 #define BIT_INT_STATUS_FIFO_FULL 0x02 #define BIT_INT_STATUS_FIFO_THS 0x04 #define BIT_INT_STATUS_DRDY 0x08 #define BIT_INT_STATUS_RESET_DONE 0x10 #define BIT_INT_STATUS_PLL_DRY 0x20 #define BIT_INT_STATUS_UI_FSYNC 0x40 /* REG_INT_STATUS2 */ #define BIT_INT_STATUS_WOM_X 0x01 #define BIT_INT_STATUS_WOM_Y 0x02 #define BIT_INT_STATUS_WOM_Z 0x04 #define BIT_INT_STATUS_SMD 0x08 #define BIT_INT_STATUS_WOM_XYZ (BIT_INT_STATUS_WOM_X | BIT_INT_STATUS_WOM_Y | BIT_INT_STATUS_WOM_Z) /* REG_FIFO_CONFIG */ #define BIT_FIFO_MODE_BYPASS 0x00 #define BIT_FIFO_MODE_STREAM 0x40 #define BIT_FIFO_MODE_STOP_FULL 0x80 /* REG_GYRO_ACCEL_CONFIG0 */ #define BIT_ACCEL_UI_BW_2_FIR 0x00 #define BIT_ACCEL_UI_BW_4_IIR 0x10 #define BIT_ACCEL_UI_BW_5_IIR 0x20 #define BIT_ACCEL_UI_BW_8_IIR 0x30 #define BIT_ACCEL_UI_BW_10_IIR 0x40 #define BIT_ACCEL_UI_BW_16_IIR 0x50 #define BIT_ACCEL_UI_BW_20_IIR 0x60 #define BIT_ACCEL_UI_BW_40_IIR 0x70 #define BIT_GYRO_UI_BW_2_FIR 0x00 #define BIT_GYRO_UI_BW_4_IIR 0x01 #define BIT_GYRO_UI_BW_5_IIR 0x02 #define BIT_GYRO_UI_BW_8_IIR 0x03 #define BIT_GYRO_UI_BW_10_IIR 0x04 #define BIT_GYRO_UI_BW_16_IIR 0x05 #define BIT_GYRO_UI_BW_20_IIR 0x06 #define BIT_GYRO_UI_BW_40_IIR 0x07 /* fifo data packet header */ #define BIT_FIFO_HEAD_MSG 0x80 #define BIT_FIFO_HEAD_ACCEL 0x40 #define BIT_FIFO_HEAD_GYRO 0x20 #define BIT_FIFO_HEAD_TMSP_ODR 0x08 #define BIT_FIFO_HEAD_TMSP_NO_ODR 0x04 #define BIT_FIFO_HEAD_TMSP_FSYNC 0x0C #define BIT_FIFO_HEAD_ODR_ACCEL 0x02 #define BIT_FIFO_HEAD_ODR_GYRO 0x01 /* REG_SMD_CONFIG */ #define BIT_WOM_INT_MODE_OR 0x00 #define BIT_WOM_INT_MODE_AND 0x08 #define BIT_WOM_MODE_INITIAL 0x00 #define BIT_WOM_MODE_PREV 0x04 #define BIT_SMD_MODE_OFF 0x00 #define BIT_SMD_MODE_OLD 0x01 #define BIT_SMD_MODE_SHORT 0x02 #define BIT_SMD_MODE_LONG 0x03 /* REG_TMST_CONFIG */ #define BIT_EN_DREG_FIFO_D2A 0x20 #define BIT_TMST_TO_REGS_EN 0x10 #define BIT_TMST_RESOL 0x08 #define BIT_TMST_DELTA_EN 0x04 #define BIT_TMST_FSYNC_EN 0x02 #define BIT_TMST_EN 0x01 /* FIFO data definitions */ #define FIFO_PACKET_SIZE 16 #define FIFO_MAX_PACKET_NB 65 #define FIFO_MIN_PACKET_NB 62 /* sensor startup time */ #define ICM40600_GYRO_START_TIME_MS 40 #define ICM40600_ACCEL_START_TIME_MS 10 /* temperature sensor */ #define TEMP_SCALE (128.0f / 115.49f) // scale, #9447 #define TEMP_OFFSET 25 // 25 degC offset #define SPI_WRITE_0(addr, data) spiQueueWrite(addr, data, 2) #define SPI_WRITE_1(addr, data, delay) spiQueueWrite(addr, data, delay) #define GET_SPI_WRITE_MACRO(_1,_2,_3,NAME,...) NAME #define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__) #define SPI_READ_0(addr, size, buf) spiQueueRead(addr, size, buf, 0) #define SPI_READ_1(addr, size, buf, delay) spiQueueRead(addr, size, buf, delay) #define GET_SPI_READ_MACRO(_1,_2,_3,_4,NAME,...) NAME #define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__) #define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG) #define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION) #define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION) #define MAX_NUM_COMMS_EVENT_SAMPLES 15 #define SPI_PACKET_SIZE 30 #define FIFO_READ_SIZE (FIFO_MAX_PACKET_NB * FIFO_PACKET_SIZE) #define BUF_MARGIN 32 // some extra buffer for additional reg RW when a FIFO read happens #define SPI_BUF_SIZE (FIFO_READ_SIZE + BUF_MARGIN) #define ACC_FS_RANGE (16 >> ICM40600_ACC_FS) #define GYR_FS_RANGE (2000 >> ICM40600_GYR_FS) #define GRAVITY_NORM 9.80665f #define kScale_acc (GRAVITY_NORM * ACC_FS_RANGE / 32768.0f) #define kScale_gyr (NANO_PI / 180.0f * GYR_FS_RANGE / 32768.0f) #define RATE_TO_HZ SENSOR_HZ(1.0f) #define DECIMATION_HIGH_RATE SENSOR_HZ(1000.0f) #define DECIMATION_LOW_RATE SENSOR_HZ(25.0f) #define NO_DECIMATION_MAX_RATE SENSOR_HZ(200.0f) // will set ODR to DECIMATION_HIGH_RATE if more than this rate #define NO_DECIMATION_MIN_RATE SENSOR_HZ(25.0f) // will set ODR to DECIMATION_LOW_RATE if less than this rate #define MAX_BATCH_SIZE (((FIFO_MIN_PACKET_NB * 90) / 100) * FIFO_PACKET_SIZE) // 90% of FIFO size /* skip first some samples */ #define ACC_SKIP_SAMPLE_NB 0 #define GYR_SKIP_SAMPLE_NB 3 #define CHIP_TIME_RES_US 1 #define CHIP_TIME_OFFSET_US 64000000ULL // 64sec #define MIN_INCREMENT_TIME_NS 1000000ULL // 1ms for 1000Hz enum SensorIndex { FIRST_CONT_SENSOR = 0, ACC = FIRST_CONT_SENSOR, GYR, NUM_CONT_SENSOR, FIRST_ONESHOT_SENSOR = NUM_CONT_SENSOR, WOM = FIRST_ONESHOT_SENSOR, NOMO, NUM_OF_SENSOR, }; enum SensorEvents { NO_EVT = -1, EVT_SPI_DONE = EVT_APP_START + 1, EVT_SENSOR_INTERRUPT_1, }; enum IntState { INT_READ_STATUS, INT_PROCESS_STATUS, INT_READ_FIFO_DATA, INT_DONE, }; enum InitState { RESET_ICM40600, INIT_ICM40600, INIT_DONE, }; enum CalibrationState { CALIBRATION_START, CALIBRATION_READ_STATUS, CALIBRATION_READ_DATA, CALIBRATION_SET_OFFSET, CALIBRATION_DONE }; enum SelfTestState { TEST_START, TEST_READ_STATUS, TEST_READ_DATA, TEST_READ_OTP, TEST_REPORT, TEST_DONE }; enum SensorState { // keep this in sync with getStateName SENSOR_BOOT, SENSOR_VERIFY_ID, SENSOR_INITIALIZING, SENSOR_IDLE, SENSOR_POWERING_UP, SENSOR_POWERING_DOWN, SENSOR_CONFIG_CHANGING, SENSOR_INT_1_HANDLING, SENSOR_CALIBRATING, SENSOR_TESTING, SENSOR_SAVE_CALIBRATION, SENSOR_NUM_OF_STATE }; #if DBG_STATE #define PRI_STATE "s" static const char * getStateName(int32_t s) { // keep this in sync with SensorState static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP", "PWR_DN", "CFG_CHANGE", "INT1", "CALIB", "TEST", "SAVE_CALIB"}; if (s >= 0 && s < SENSOR_NUM_OF_STATE) { return l[s]; } return "???"; #else #define PRI_STATE PRIi32 static int32_t getStateName(int32_t s) { return s; #endif } #if DBG_TIMESTAMP struct StatisticsSet { uint32_t sync_reset_count; uint32_t sync_count; uint32_t sync_adjust_plus; uint32_t sync_adjust_minus; uint32_t sync_truncate; }; #endif struct ConfigStat { uint64_t latency; uint32_t rate; bool enable; }; struct CalibrationData { struct HostHubRawPacket header; struct SensorAppEventHeader data_header; int32_t xBias; int32_t yBias; int32_t zBias; } __attribute__((packed)); struct TestResultData { struct HostHubRawPacket header; struct SensorAppEventHeader data_header; } __attribute__((packed)); struct ICM40600Sensor { struct ConfigStat pConfig; // pending config status request struct TripleAxisDataEvent *data_evt; uint32_t handle; uint32_t rate; uint64_t latency; uint64_t prev_rtc_time; int16_t offset[3]; int16_t data[3]; bool updated; bool powered; // activate status bool configed; // configure status bool wait_for_odr; enum SensorIndex idx; uint8_t flush; uint8_t decimator; uint8_t data_cnt; uint8_t skip_sample_cnt; }; struct FifoPacketData { int16_t accel[3]; int16_t gyro[3]; uint16_t timestamp; bool odr_accel; bool odr_gyro; bool valid_accel; bool valid_gyro; int8_t temp; }; struct ICM40600FactoryCal { int32_t data[3]; int32_t data_count; }; struct ICM40600SelfTest { int32_t data[3]; int32_t data_st_on[3]; int32_t data_st_off[3]; int32_t data_count; bool st_mode; uint8_t otp_st_data_gyro[3]; uint8_t otp_st_data_accel[3]; }; struct ICM40600Config { uint32_t accel_rate; uint32_t gyro_rate; uint32_t wom_threshold; uint32_t fifo_rate; uint16_t fifo_watermark; uint8_t fifo_sample_size; bool accel_on; bool gyro_on; bool wom_on; }; struct ICM40600Task { uint32_t tid; struct ICM40600Sensor sensors[NUM_OF_SENSOR]; // spi and interrupt spi_cs_t cs; struct SpiMode mode; struct SpiPacket packets[SPI_PACKET_SIZE]; struct SpiDevice *spiDev; struct Gpio *Int1; IRQn_Type Irq1; struct ChainedIsr Isr1; bool Int1_EN; // spi buffers uint8_t *dataBuffer[3]; uint8_t txrxBuffer[SPI_BUF_SIZE]; // states volatile uint8_t state; //task state, type enum SensorState, do NOT change this directly enum InitState init_state; enum IntState int_state; enum CalibrationState calibration_state; enum SelfTestState selftest_state; // pending config bool pending_int[1]; bool pending_flush; bool pending_config[NUM_OF_SENSOR]; bool pending_calibration_save; struct ICM40600Config config; uint32_t noMotionTimerHandle; uint16_t fifo_count; bool powered; bool flush; // calibration #ifdef ACCEL_CAL_ENABLED struct AccelCal accel_cal; #endif #ifdef GYRO_CAL_ENABLED struct GyroCal gyro_cal; #endif // timestamping time_sync_t gSensorTime2RTC; uint64_t data_timestamp; uint64_t chip_time_us; uint64_t last_sync_time; uint64_t last_sync_data_ts; uint16_t chip_timestamp; bool fifo_start_sync; // temperature data from chip in degC float chip_temperature; // spi rw struct SlabAllocator *mDataSlab; uint16_t mWbufCnt; uint8_t mRegCnt; uint8_t mRetryLeft; bool spiInUse; struct ICM40600FactoryCal factory_cal; struct ICM40600SelfTest self_test; #if DBG_TIMESTAMP struct StatisticsSet statistics_set; #endif }; static uint32_t AccRates[] = { SENSOR_HZ(25.0f/8.0f), SENSOR_HZ(25.0f/4.0f), SENSOR_HZ(25.0f/2.0f), SENSOR_HZ(25.0f), SENSOR_HZ(50.0f), SENSOR_HZ(100.0f), SENSOR_HZ(200.0f), SENSOR_HZ(500.0f), 0, }; static uint32_t GyrRates[] = { SENSOR_HZ(25.0f/8.0f), SENSOR_HZ(25.0f/4.0f), SENSOR_HZ(25.0f/2.0f), SENSOR_HZ(25.0f), SENSOR_HZ(50.0f), SENSOR_HZ(100.0f), SENSOR_HZ(200.0f), SENSOR_HZ(500.0f), 0, }; static struct ICM40600Task mTask; #define DEC_INFO(name, type, axis, inter, samples) \ .sensorName = name, \ .sensorType = type, \ .numAxis = axis, \ .interrupt = inter, \ .minSamples = samples #define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \ DEC_INFO(name, type, axis, inter, samples), \ .supportedRates = rates #define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \ DEC_INFO(name, type, axis, inter, samples), \ .supportedRates = rates, \ .flags1 = SENSOR_INFO_FLAGS1_RAW, \ .rawType = raw, \ .rawScale = scale #define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \ DEC_INFO(name, type, axis, inter, samples), \ .supportedRates = rates, \ .flags1 = SENSOR_INFO_FLAGS1_BIAS, \ .biasType = bias #define DEC_INFO_RATE_RAW_BIAS(name, rates, type, axis, inter, samples, raw, scale, bias) \ DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale), \ .flags1 = SENSOR_INFO_FLAGS1_RAW | SENSOR_INFO_FLAGS1_BIAS, \ .biasType = bias typedef struct ICM40600Task _Task; #define TASK _Task* const _task // To get rid of static variables all task functions should have a task structure pointer input. // This is an intermediate step. #define TDECL() TASK = &mTask; (void)_task // Access task variables without explicitly specify the task structure pointer. #define T(v) (_task->v) // Atomic get state #define GET_STATE() (atomicReadByte(&(_task->state))) // Atomic set state, this set the state to arbitrary value, use with caution #define SET_STATE(s) do{\ DEBUG_PRINT_IF(DBG_STATE, "set state %" PRI_STATE "\n", getStateName(s));\ atomicWriteByte(&(_task->state), (s));\ }while(0) // Atomic switch state from IDLE to desired state. static bool trySwitchState_(TASK, enum SensorState newState) { #if DBG_STATE bool ret = atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState); uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE(); DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n", getStateName(prevState), getStateName(newState), ret ? "ok" : "failed"); return ret; #else return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState); #endif } // Short-hand #define trySwitchState(s) trySwitchState_(_task, (s)) static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] = { #ifdef ACCEL_CAL_ENABLED { DEC_INFO_RATE_RAW_BIAS("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0 / kScale_acc, SENS_TYPE_ACCEL_BIAS) }, #else { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0 / kScale_acc) }, #endif #ifdef GYRO_CAL_ENABLED { DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) }, #else { DEC_INFO_RATE("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20) }, #endif { DEC_INFO("Wake-on-Motion", SENS_TYPE_ANY_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) }, { DEC_INFO("No-Motion", SENS_TYPE_NO_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) }, }; static void time_init(TASK) { time_sync_init(&T(gSensorTime2RTC)); } static bool sensortime_to_rtc_time(TASK, uint64_t sensor_time, uint64_t *rtc_time_ns) { return time_sync_estimate_time1(&T(gSensorTime2RTC), sensor_time, rtc_time_ns); } static void map_sensortime_to_rtc_time(TASK, uint64_t sensor_time, uint64_t rtc_time_ns) { #if DBG_TIMESTAMP T(statistics_set).sync_count++; #endif time_sync_add(&T(gSensorTime2RTC), rtc_time_ns, sensor_time); } static void invalidate_sensortime_to_rtc_time(TASK) { #if DBG_TIMESTAMP T(statistics_set).sync_reset_count++; #endif time_sync_reset(&T(gSensorTime2RTC)); } static void dataEvtFree(void *ptr) { TDECL(); struct TripleAxisDataEvent *ev = (struct TripleAxisDataEvent *)ptr; slabAllocatorFree(T(mDataSlab), ev); } static void spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay) { TDECL(); if (T(spiInUse)) { ERROR_PRINT("SPI in use, cannot queue write\n"); return; } T(packets[T(mRegCnt)]).size = 2; T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]); T(packets[T(mRegCnt)]).rxBuf = &T(txrxBuffer[T(mWbufCnt)]); T(packets[T(mRegCnt)]).delay = delay * 1000; T(txrxBuffer[T(mWbufCnt++)]) = ICM40600_SPI_WRITE | addr; T(txrxBuffer[T(mWbufCnt++)]) = data; T(mRegCnt)++; } /* * need to be sure size of buf is larger than read size */ static void spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay) { TDECL(); if (T(spiInUse)) { ERROR_PRINT("SPI in use, cannot queue read %d %d\n", (int)addr, (int)size); return; } *buf = &T(txrxBuffer[T(mWbufCnt)]); T(packets[T(mRegCnt)]).size = size + 1; // first byte will not contain valid data T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]); T(packets[T(mRegCnt)]).rxBuf = *buf; T(packets[T(mRegCnt)]).delay = delay * 1000; T(txrxBuffer[T(mWbufCnt)++]) = ICM40600_SPI_READ | addr; T(mWbufCnt) += size; T(mRegCnt)++; } static void spiBatchTxRx(struct SpiMode *mode, SpiCbkF callback, void *cookie, const char * src) { TDECL(); if (T(mRegCnt) == 0) { callback(cookie, 0); return; } if (T(mWbufCnt) > SPI_BUF_SIZE) { ERROR_PRINT("NO enough SPI buffer space, dropping transaction.\n"); return; } if (T(mRegCnt) > SPI_PACKET_SIZE) { ERROR_PRINT("spiBatchTxRx too many packets!\n"); return; } T(spiInUse) = true; T(mWbufCnt) = 0; // Reset variables before issuing SPI transaction. // SPI may finish before spiMasterRxTx finish uint8_t regCount = T(mRegCnt); T(mRegCnt) = 0; if (spiMasterRxTx(T(spiDev), T(cs), T(packets), regCount, mode, callback, cookie) < 0) { ERROR_PRINT("spiMasterRxTx failed!\n"); } } static bool icm40600Isr1(struct ChainedIsr *isr) { TASK = container_of(isr, struct ICM40600Task, Isr1); if (!extiIsPendingGpio(T(Int1))) { return false; } /* better to use atomic read for the pending flag but not serious even not atomic */ if (!T(pending_int[0])) { osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid)); } extiClearPendingGpio(T(Int1)); return true; } static void sensorSpiCallback(void *cookie, int err) { TDECL(); T(spiInUse) = false; if (!osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, T(tid))) ERROR_PRINT("sensorSpiCallback: osEnqueuePrivateEvt() failed\n"); } static void noMotionCallback(uint32_t timerId, void *data) { const struct ICM40600Sensor *sensor = (const struct ICM40600Sensor *)data; if (!sensor->configed) return; osEnqueueEvt(EVT_SENSOR_NO_MOTION, NULL, NULL); } static void setOffsetReg(TASK) { uint8_t val; const int16_t * const acc_offset = T(sensors[ACC]).offset; const int16_t * const gyr_offset = T(sensors[GYR]).offset; DEBUG_PRINT("Set ACC hw offset %d %d %d\n", -acc_offset[0], -acc_offset[1], -acc_offset[2]); DEBUG_PRINT("Set GYR hw offset %d %d %d\n", -gyr_offset[0], -gyr_offset[1], -gyr_offset[2]); // GX_L val = (-gyr_offset[0]) & 0xff; SPI_WRITE(REG_GOS_USER0, val); // GY_H / GX_H val = (-gyr_offset[1] >> 4) & 0xf0; val |= ((-gyr_offset[0] >> 8) & 0x0f); SPI_WRITE(REG_GOS_USER1, val); // GY_L val = (-gyr_offset[1]) & 0xff; SPI_WRITE(REG_GOS_USER2, val); // GZ_L val = (-gyr_offset[2]) & 0xff; SPI_WRITE(REG_GOS_USER3, val); // AX_H / GZ_H val = (-acc_offset[0] >> 4) & 0xf0; val |= ((-gyr_offset[2] >> 8) & 0x0f); SPI_WRITE(REG_GOS_USER4, val); // AX_H / GZ_H val = (-acc_offset[0] >> 4) & 0xf0; val |= ((-gyr_offset[2] >> 8) & 0x0f); SPI_WRITE(REG_GOS_USER4, val); // AX_L val = (-acc_offset[0]) & 0xff; SPI_WRITE(REG_GOS_USER5, val); // AY_L val = (-acc_offset[1]) & 0xff; SPI_WRITE(REG_GOS_USER6, val); // AZ_H / AY_H val = (-acc_offset[2] >> 4) & 0xf0; val |= ((-acc_offset[1] >> 8) & 0x0f); SPI_WRITE(REG_GOS_USER7, val); // AZ_L val = (-acc_offset[2]) & 0xff; SPI_WRITE(REG_GOS_USER8, val); } static void resetOffsetReg(TASK, enum SensorIndex idx) { uint8_t val; DEBUG_PRINT("Reset offset registers sensor=%d\n", idx); if (idx == ACC) { val = (-T(sensors[GYR]).offset[2] >> 8) & 0x0f; SPI_WRITE(REG_GOS_USER4, val); // AX_H / GZ_H SPI_WRITE(REG_GOS_USER5, 0x00); // AX_L SPI_WRITE(REG_GOS_USER6, 0x00); // AY_L SPI_WRITE(REG_GOS_USER7, 0x00); // AZ_H / AY_H SPI_WRITE(REG_GOS_USER8, 0x00); // AZ_L } else if (idx == GYR) { SPI_WRITE(REG_GOS_USER0, 0x00); // GX_L SPI_WRITE(REG_GOS_USER1, 0x00); // GY_H / GX_H SPI_WRITE(REG_GOS_USER2, 0x00); // GY_L SPI_WRITE(REG_GOS_USER3, 0x00); // GZ_L val = (-T(sensors[ACC]).offset[0] >> 4) & 0xf0; SPI_WRITE(REG_GOS_USER4, val); // AX_H / GZ_H } } static bool saveCalibration(TASK) { if (trySwitchState(SENSOR_SAVE_CALIBRATION)) { setOffsetReg(_task); spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__); return true; } else { return false; } } static bool accCfgData(void *data, void *cookie) { TDECL(); struct ICM40600Sensor *sensor = &T(sensors[ACC]); struct CfgData { int32_t hw[3]; // chip frame (hw unit @FSR=factory calibration) float sw[3]; // body frame }; struct CfgData *values = data; int i; for (i = 0; i < 3; i++) { // offset register is 12bit if (values->hw[i] > 2047) { sensor->offset[i] = 2047; } else if (values->hw[i] < -2048) { sensor->offset[i] = -2048; } else { sensor->offset[i] = values->hw[i]; } } #ifdef ACCEL_CAL_ENABLED accelCalBiasSet(&T(accel_cal), values->sw[0], values->sw[1], values->sw[2]); #endif if (!saveCalibration(_task)) { T(pending_calibration_save) = true; } INFO_PRINT("accCfgData hw bias: data=%d, %d, %d\n", sensor->offset[0], sensor->offset[1], sensor->offset[2]); return true; } static bool gyrCfgData(void *data, void *cookie) { TDECL(); struct ICM40600Sensor *sensor = &T(sensors[GYR]); const struct AppToSensorHalDataPayload *p = data; int i; if (p->type == HALINTF_TYPE_GYRO_CAL_BIAS && p->size == sizeof(struct GyroCalBias)) { const struct GyroCalBias *bias = p->gyroCalBias; for (i = 0; i < 3; i++) { // offset register is 12bit if (bias->hardwareBias[i] > 2047) { sensor->offset[i] = 2047; } else if (bias->hardwareBias[i] < -2048) { sensor->offset[i] = -2048; } else { sensor->offset[i] = bias->hardwareBias[i]; } } #ifdef GYRO_CAL_ENABLED gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1], bias->softwareBias[2], sensorGetTime()); #endif if (!saveCalibration(_task)) { T(pending_calibration_save) = true; } INFO_PRINT("gyrCfgData hw bias: data=%d, %d, %d\n", sensor->offset[0], sensor->offset[1], sensor->offset[2]); } else if (p->type == HALINTF_TYPE_GYRO_OTC_DATA && p->size == sizeof(struct GyroOtcData)) { // Over-temperature gyro calibration not supported } else { ERROR_PRINT("Unknown gyro config data type 0x%04x, size %d\n", p->type, p->size); } return true; } static bool validateFifoData(const int16_t data[3]) { bool ret = true; if ((data[0] == -32768) && (data[1] == -32768) && (data[2] == -32768)) ret = false; return ret; } static void getFifoData(const uint8_t *data, int idx, struct FifoPacketData *out) { /* invalidate all flags */ out->valid_accel = false; out->valid_gyro = false; /* Header : 1 byte * Accel : 6 byte * Gyro : 6 byte * Temp : 1 byte * Timestamp : 2 byte */ if (data[idx] & BIT_FIFO_HEAD_ACCEL) { out->accel[0] = (data[idx + 2] << 8) | data[idx + 1]; out->accel[1] = (data[idx + 4] << 8) | data[idx + 3]; out->accel[2] = (data[idx + 6] << 8) | data[idx + 5]; out->valid_accel = validateFifoData(out->accel); } out->odr_accel = (data[idx] & BIT_FIFO_HEAD_ODR_ACCEL) ? true : false; if (data[idx] & BIT_FIFO_HEAD_GYRO) { out->gyro[0] = (data[idx + 8] << 8) | data[idx + 7]; out->gyro[1] = (data[idx + 10] << 8) | data[idx + 9]; out->gyro[2] = (data[idx + 12] << 8) | data[idx + 11]; out->valid_gyro = validateFifoData(out->gyro); } out->odr_gyro = (data[idx] & BIT_FIFO_HEAD_ODR_GYRO) ? true : false; out->temp = (int8_t)data[idx + 13]; out->timestamp = (data[idx + 15] << 8) | data[idx + 14]; } static bool calSelftestGetOneData(enum SensorIndex sensor_idx, const uint8_t *data, int idx, int16_t raw_data[3]) { struct FifoPacketData fifo_data; bool ret = false; getFifoData(data, idx, &fifo_data); switch (sensor_idx) { case ACC: if (fifo_data.valid_accel) { raw_data[0] = fifo_data.accel[0]; raw_data[1] = fifo_data.accel[1]; raw_data[2] = fifo_data.accel[2]; ret = true; } break; case GYR: if (fifo_data.valid_gyro) { raw_data[0] = fifo_data.gyro[0]; raw_data[1] = fifo_data.gyro[1]; raw_data[2] = fifo_data.gyro[2]; ret = true; } break; default: break; } return ret; } static void calSelftestFifoEnable(TASK, enum SensorIndex idx, bool en, uint32_t delay) { uint8_t val; if (en) { // enable FIFO output T(config).fifo_sample_size = FIFO_PACKET_SIZE; val = BIT_FIFO_ACCEL_EN | BIT_FIFO_GYRO_EN | BIT_FIFO_TEMP_EN | BIT_FIFO_TMST_FSYNC_EN; SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_STREAM); SPI_WRITE(REG_FIFO_CONFIG1, val, delay); // with wait } else { // disable FIFO output T(config).fifo_sample_size = 0; SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_BYPASS); SPI_WRITE(REG_FIFO_CONFIG1, 0); } } /* * Factory calibration */ static void sendCalibrationResult(uint8_t status, uint8_t sensorType, int32_t xBias, int32_t yBias, int32_t zBias) { struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData)); if (!data) { ERROR_PRINT("Couldn't alloc cal result pkt"); return; } data->header.appId = ICM40600_APP_ID; data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket)); data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT; data->data_header.sensorType = sensorType; data->data_header.status = status; data->xBias = xBias; data->yBias = yBias; data->zBias = zBias; if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) ERROR_PRINT("Couldn't send cal result evt"); } static void calibrationInit(TASK, enum SensorIndex idx) { uint8_t val; // Disable Interrupt SPI_WRITE(REG_INT_SOURCE0, 0); SPI_WRITE(REG_INT_SOURCE1, 0); // reset offset registers resetOffsetReg(_task, idx); // stop FIFO calSelftestFifoEnable(_task, idx, false, 0); // set rate SPI_WRITE(REG_GYRO_CONFIG0, (CALIBRATION_GYR_FS << SHIFT_GYRO_FS_SEL) | (CALIBRATION_ODR << SHIFT_ODR_CONF)); SPI_WRITE(REG_ACCEL_CONFIG0, (CALIBRATION_ACC_FS << SHIFT_ACCEL_FS_SEL) | (CALIBRATION_ODR << SHIFT_ODR_CONF)); // set filter SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, CALIBRATION_ACC_BW_IND | CALIBRATION_GYR_BW_IND); // turn on sensors switch (idx) { case ACC: val = BIT_ACCEL_MODE_LN; break; case GYR: val = BIT_GYRO_MODE_LN; break; default: val = 0; break; } SPI_WRITE(REG_PWR_MGMT_0, val, 200 * 1000); calSelftestFifoEnable(_task, idx, true, CALIBRATION_READ_INTERVAL_US); } static void calibrationDeinit(TASK, enum SensorIndex idx) { calSelftestFifoEnable(_task, idx, false, 0); SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND); SPI_WRITE(REG_PWR_MGMT_0, 0, 200); // 9136 // make register accesses happen when sensor is enabled next time T(config).gyro_rate = 0; T(config).accel_rate = 0; T(config).fifo_rate = 0; T(config).fifo_watermark = 0; T(config).accel_on = false; T(config).gyro_on = false; T(config).wom_on = false; } static void calibrationHandling(TASK, enum SensorIndex idx) { const uint8_t *buf, *data; uint8_t int_status; uint16_t fifo_count; int i, t; bool r; int16_t raw_data[3] = { 0, 0, 0 }; if (idx != ACC && idx != GYR) { ERROR_PRINT("Invalid sensor index\n"); return; } switch (T(calibration_state)) { case CALIBRATION_START: T(mRetryLeft) = RETRY_CNT_CALIBRATION; calibrationInit(_task, idx); for (i = 0; i < 3; i++) { T(factory_cal).data[i] = 0; } SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0])); SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1])); T(calibration_state) = CALIBRATION_READ_STATUS; T(factory_cal).data_count = CALIBRATION_SAMPLE_NB; spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; case CALIBRATION_READ_STATUS: buf = T(dataBuffer[0]); int_status = buf[1]; buf = T(dataBuffer[1]); fifo_count = buf[2] << 8 | buf[1]; fifo_count = fifo_count - fifo_count % T(config).fifo_sample_size; T(fifo_count) = fifo_count; DEBUG_PRINT("fifo_count = %d\n", fifo_count); if (int_status & BIT_INT_STATUS_FIFO_FULL) { ERROR_PRINT("fifo overflow\n"); calibrationDeinit(_task, idx); T(calibration_state) = CALIBRATION_DONE; sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0); } else { T(calibration_state) = CALIBRATION_READ_DATA; SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0])); } spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; case CALIBRATION_READ_DATA: buf = T(dataBuffer[0]); data = &buf[1]; for (i = 0; i < T(fifo_count); i += T(config).fifo_sample_size) { r = calSelftestGetOneData(idx, data, i, raw_data); if (r == false) { ERROR_PRINT("invalid data packet\n"); calibrationDeinit(_task, idx); T(calibration_state) = CALIBRATION_DONE; sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0); break; } for (t = 0; t < 3; t++) { T(factory_cal).data[t] += raw_data[t]; } T(factory_cal).data_count--; if (T(factory_cal).data_count == 0) { break; } } if (T(factory_cal).data_count > 0) { if (--T(mRetryLeft) == 0) { ERROR_PRINT("calibration timeout\n"); calibrationDeinit(_task, idx); T(calibration_state) = CALIBRATION_DONE; sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0); } else { calSelftestFifoEnable(_task, idx, false, 0); // toggle FIFO to reset calSelftestFifoEnable(_task, idx, true, CALIBRATION_READ_INTERVAL_US); SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0])); SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1])); T(calibration_state) = CALIBRATION_READ_STATUS; } spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; } T(calibration_state) = CALIBRATION_SET_OFFSET; // fall-through case CALIBRATION_SET_OFFSET: DEBUG_PRINT("calibration total: %ld, %ld, %ld, data count=%d\n", T(factory_cal).data[0], T(factory_cal).data[1], T(factory_cal).data[2], CALIBRATION_SAMPLE_NB); for (i = 0; i < 3; i++) { T(factory_cal).data[i] /= CALIBRATION_SAMPLE_NB; } DEBUG_PRINT("average: %ld, %ld, %ld\n", T(factory_cal).data[0], T(factory_cal).data[1], T(factory_cal).data[2]); if (idx == ACC) { // assume the largest data axis shows +1 or -1 gee t = 0; for (i = 0; i < 3; i++) { if (abs(T(factory_cal).data[i]) > abs(T(factory_cal).data[t])) t = i; } if (T(factory_cal).data[t] > 0) { DEBUG_PRINT("assume axis %d is %d\n", t, CALIBRATION_ACC_1G); T(factory_cal).data[t] -= CALIBRATION_ACC_1G; } else { DEBUG_PRINT("assume axis %d is %d\n", t, -CALIBRATION_ACC_1G); T(factory_cal).data[t] += CALIBRATION_ACC_1G; } } // set bias to offset registers for (i = 0; i < 3; i++) { T(sensors[idx]).offset[i] = T(factory_cal).data[i]; } setOffsetReg(_task); calibrationDeinit(_task, idx); sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, mSensorInfo[idx].sensorType, T(factory_cal).data[0], T(factory_cal).data[1], T(factory_cal).data[2]); INFO_PRINT("reported: %ld, %ld, %ld\n", T(factory_cal).data[0], T(factory_cal).data[1], T(factory_cal).data[2]); T(calibration_state) = CALIBRATION_DONE; spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; default: break; } } static bool accCalibration(void *cookie) { TDECL(); INFO_PRINT("Accel Calibration\n"); if (!T(powered) && trySwitchState(SENSOR_CALIBRATING)) { T(calibration_state) = CALIBRATION_START; calibrationHandling(_task, ACC); return true; } else { ERROR_PRINT("Cannot run calibration because sensor is busy\n"); sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0); return false; } } static bool gyrCalibration(void *cookie) { TDECL(); INFO_PRINT("Gyro Calibration\n"); if (!T(powered) && trySwitchState(SENSOR_CALIBRATING)) { T(calibration_state) = CALIBRATION_START; calibrationHandling(_task, GYR); return true; } else { ERROR_PRINT("Cannot run calibration because sensor is busy\n"); sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO, 0, 0, 0); return false; } } /* * Selftest */ static void sendTestResult(uint8_t status, uint8_t sensorType) { struct TestResultData *data = heapAlloc(sizeof(struct TestResultData)); if (!data) { ERROR_PRINT("Couldn't alloc test result packet"); return; } data->header.appId = ICM40600_APP_ID; data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket)); data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT; data->data_header.sensorType = sensorType; data->data_header.status = status; if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) ERROR_PRINT("Couldn't send test result packet"); } static const uint16_t SelfTestEquation[256] = { 2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808, 2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041, 3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293, 3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566, 3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862, 3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182, 4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528, 4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903, 4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310, 5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750, 5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226, 6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742, 6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301, 7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906, 7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561, 8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270, 9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038, 10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870, 10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771, 11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746, 12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802, 13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946, 15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184, 16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526, 17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978, 19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550, 20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253, 22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097, 24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093, 26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255, 28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597, 30903, 31212, 31524, 31839, 32157, 32479, 32804 }; static void selfTestInit(TASK, enum SensorIndex idx) { uint8_t val; // Disable Interrupt SPI_WRITE(REG_INT_SOURCE0, 0); SPI_WRITE(REG_INT_SOURCE1, 0); // reset offset registers resetOffsetReg(_task, idx); // stop FIFO calSelftestFifoEnable(_task, idx, false, 0); // self-test mode set val = 0; if (T(self_test).st_mode) { if (idx == ACC) { val |= (BIT_TEST_AX_EN | BIT_TEST_AY_EN | BIT_TEST_AZ_EN); val |= BIT_SELF_TEST_REGULATOR_EN; } else if (idx == GYR) { val |= (BIT_TEST_GX_EN | BIT_TEST_GY_EN | BIT_TEST_GZ_EN); } } SPI_WRITE(REG_SELF_TEST_CONFIG, val); // set rate SPI_WRITE(REG_GYRO_CONFIG0, (SELF_TEST_GYR_FS << SHIFT_GYRO_FS_SEL) | (SELF_TEST_ODR << SHIFT_ODR_CONF)); SPI_WRITE(REG_ACCEL_CONFIG0, (SELF_TEST_ACC_FS << SHIFT_ACCEL_FS_SEL) | (SELF_TEST_ODR << SHIFT_ODR_CONF)); // set filter val = 0; if (idx == ACC) { val |= SELF_TEST_ACC_BW_IND; } else if (idx == GYR) { val |= SELF_TEST_GYR_BW_IND; } SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, val); // turn on sensors val = 0; if (idx == ACC) { val |= BIT_ACCEL_MODE_LN; } else if (idx == GYR) { val |= BIT_GYRO_MODE_LN; } SPI_WRITE(REG_PWR_MGMT_0, val, 200 * 1000); calSelftestFifoEnable(_task, idx, true, SELF_TEST_READ_INTERVAL_US); } static void selfTestDeinit(TASK, enum SensorIndex idx) { calSelftestFifoEnable(_task, idx, false, 0); SPI_WRITE(REG_SELF_TEST_CONFIG, 0); SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND); SPI_WRITE(REG_PWR_MGMT_0, 0, 200); // 9136 // make register accesses happen when sensor is enabled next time T(config).gyro_rate = 0; T(config).accel_rate = 0; T(config).fifo_rate = 0; T(config).fifo_watermark = 0; T(config).accel_on = false; T(config).gyro_on = false; T(config).wom_on = false; } static bool checkAccelSelftest(TASK) { int32_t st_res; uint32_t st_otp[3]; int i; int32_t ratio; bool pass = true; bool otp_value_zero = false; /* calculate ST_OTP */ for (i = 0; i < 3; i++) { if (T(self_test.otp_st_data_accel[i] != 0)) st_otp[i] = SelfTestEquation[T(self_test).otp_st_data_accel[i] - 1]; else otp_value_zero = true; } if (!otp_value_zero) { /* criteria a */ for (i = 0; i < 3; i++) { st_res = T(self_test).data_st_on[i] - T(self_test).data_st_off[i]; ratio = abs(st_res / st_otp[i] - SELF_TEST_PRECISION); if (ratio >= SELF_TEST_ACC_SHIFT_DELTA) { INFO_PRINT("error accel[%d] : st_res = %ld, st_otp = %ld\n", i, st_res, st_otp[i]); pass = false; } } } else { /* criteria b */ for (i = 0; i < 3; i++) { st_res = abs(T(self_test).data_st_on[i] - T(self_test).data_st_off[i]); if (st_res < SELF_TEST_MIN_ACC || st_res > SELF_TEST_MAX_ACC) { INFO_PRINT("error accel[%d] : st_res = %ld, min = %d, max = %d\n", i, st_res, SELF_TEST_MIN_ACC, SELF_TEST_MAX_ACC); pass = false; } } } return pass; } static bool checkGyroSelftest(TASK) { int32_t st_res; uint32_t st_otp[3]; int i; bool pass = true; bool otp_value_zero = false; /* calculate ST_OTP */ for (i = 0; i < 3; i++) { if (T(self_test).otp_st_data_gyro[i] != 0) st_otp[i] = SelfTestEquation[T(self_test).otp_st_data_gyro[i] - 1]; else otp_value_zero = true; } if (!otp_value_zero) { /* criteria a */ for (i = 0; i < 3; i++) { st_res = T(self_test).data_st_on[i] - T(self_test).data_st_off[i]; if (st_res <= st_otp[i] * SELF_TEST_GYR_SHIFT_DELTA) { INFO_PRINT("error gyro[%d] : st_res = %ld, st_otp = %ld\n", i, st_res, st_otp[i]); pass = false; } } } else { /* criteria b */ for (i = 0; i < 3; i++) { st_res = abs(T(self_test).data_st_on[i] - T(self_test).data_st_off[i]); if (st_res < SELF_TEST_MIN_GYR) { INFO_PRINT("error gyro[%d] : st_res = %ld, min = %d\n", i, st_res, SELF_TEST_MIN_GYR); pass = false; } } } if (pass) { /* criteria c */ for (i = 0; i < 3; i++) { if (abs(T(self_test).data_st_off[i]) > SELF_TEST_MAX_GYR_OFFSET) { INFO_PRINT("error gyro[%d] = %d, max = %d\n", i, abs(T(self_test).data_st_off[i]), SELF_TEST_MAX_GYR_OFFSET); pass = false; } } } return pass; } static void selfTestHandling(TASK, enum SensorIndex idx) { const uint8_t *buf, *data; uint8_t int_status; uint16_t fifo_count; int i, t; bool r; int16_t raw_data[3] = { 0, 0, 0 }; bool pass; if (idx != ACC && idx != GYR) { ERROR_PRINT("Invalid sensor index\n"); return; } switch (T(selftest_state)) { case TEST_START: T(mRetryLeft) = RETRY_CNT_SELF_TEST; selfTestInit(_task, idx); for (i = 0; i < 3; i++) { T(self_test).data[i] = 0; } SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0])); SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1])); T(selftest_state) = TEST_READ_STATUS; T(self_test).data_count = SELF_TEST_SAMPLE_NB; spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; case TEST_READ_STATUS: buf = T(dataBuffer[0]); int_status = buf[1]; buf = T(dataBuffer[1]); fifo_count = buf[2] << 8 | buf[1]; fifo_count = fifo_count - fifo_count % T(config).fifo_sample_size; T(fifo_count) = fifo_count; DEBUG_PRINT("fifo_count = %d\n", fifo_count); if (int_status & BIT_INT_STATUS_FIFO_FULL) { ERROR_PRINT("fifo overflow\n"); selfTestDeinit(_task, idx); T(selftest_state) = TEST_DONE; sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType); } else { T(selftest_state) = TEST_READ_DATA; SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0])); } spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; case TEST_READ_DATA: buf = T(dataBuffer[0]); data = &buf[1]; for (i = 0; i < T(fifo_count); i += T(config).fifo_sample_size) { r = calSelftestGetOneData(idx, data, i, raw_data); if (r == false) { ERROR_PRINT("invalid data packet\n"); selfTestDeinit(_task, idx); T(selftest_state) = TEST_DONE; sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType); break; } for (t = 0; t < 3; t++) { T(self_test).data[t] += raw_data[t]; } T(self_test).data_count--; if (T(self_test).data_count == 0) { break; } } if (T(self_test).data_count > 0) { if (--T(mRetryLeft) == 0) { ERROR_PRINT("selftest timeout\n"); selfTestDeinit(_task, idx); T(selftest_state) = TEST_DONE; sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType); } else { calSelftestFifoEnable(_task, idx, false, 0); // toggle FIFO to reset calSelftestFifoEnable(_task, idx, true, SELF_TEST_READ_INTERVAL_US); SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0])); SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1])); T(selftest_state) = TEST_READ_STATUS; } spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; } T(selftest_state) = TEST_READ_OTP; // fall-through case TEST_READ_OTP: for (i = 0; i < 3; i++) { T(self_test).data[i] = T(self_test).data[i] / SELF_TEST_SAMPLE_NB * SELF_TEST_PRECISION; } INFO_PRINT("st_mode=%d average (scaled) : %ld, %ld, %ld\n", T(self_test).st_mode, T(self_test).data[0], T(self_test).data[1], T(self_test).data[2]); selfTestDeinit(_task, idx); for (i = 0; i < 3; i++) { if (T(self_test).st_mode) { T(self_test).data_st_on[i] = T(self_test).data[i]; } else { T(self_test).data_st_off[i] = T(self_test).data[i]; } } // Run again with self-test mode on if (!T(self_test).st_mode) { T(self_test).st_mode = true; T(selftest_state) = TEST_START; } else { INFO_PRINT("st mode on : %ld %ld %ld\n", T(self_test).data_st_on[0], T(self_test).data_st_on[1], T(self_test).data_st_on[2]); INFO_PRINT("st mode off : %ld %ld %ld\n", T(self_test).data_st_off[0], T(self_test).data_st_off[1], T(self_test).data_st_off[2]); // read OTP if (idx == ACC) { SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_2); SPI_READ(REG_XA_ST_DATA, 3, &T(dataBuffer[0])); } else if (idx == GYR) { SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_1); SPI_READ(REG_XG_ST_DATA, 3, &T(dataBuffer[0])); } SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_0); T(selftest_state) = TEST_REPORT; } spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; case TEST_REPORT: buf = T(dataBuffer[0]); if (idx == ACC) { T(self_test).otp_st_data_accel[0] = buf[1]; T(self_test).otp_st_data_accel[1] = buf[2]; T(self_test).otp_st_data_accel[2] = buf[3]; INFO_PRINT("otp accel : %d %d %d\n", T(self_test).otp_st_data_accel[0], T(self_test).otp_st_data_accel[1], T(self_test).otp_st_data_accel[2]); } else if (idx == GYR) { T(self_test).otp_st_data_gyro[0] = buf[1]; T(self_test).otp_st_data_gyro[1] = buf[2]; T(self_test).otp_st_data_gyro[2] = buf[3]; INFO_PRINT("otp gyro : %d %d %d\n", T(self_test).otp_st_data_gyro[0], T(self_test).otp_st_data_gyro[1], T(self_test).otp_st_data_gyro[2]); } pass = false; if (idx == ACC) { pass = checkAccelSelftest(_task); } else if (idx == GYR) { pass = checkGyroSelftest(_task); } if (pass) { sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, mSensorInfo[idx].sensorType); } else { sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType); } T(selftest_state) = TEST_DONE; selfTestDeinit(_task, idx); spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__); break; default: break; } } static bool accSelfTest(void *cookie) { TDECL(); INFO_PRINT("Accel Selftest\n"); if (!T(powered) && trySwitchState(SENSOR_TESTING)) { T(self_test).st_mode = false; T(selftest_state) = TEST_START; selfTestHandling(_task, ACC); return true; } else { ERROR_PRINT("cannot test accel because sensor is busy\n"); sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL); return false; } } static bool gyrSelfTest(void *cookie) { TDECL(); INFO_PRINT("Gyro Selftest\n"); if (!T(powered) && trySwitchState(SENSOR_TESTING)) { T(self_test).st_mode = false; T(selftest_state) = TEST_START; selfTestHandling(_task, GYR); return true; } else { ERROR_PRINT("cannot test accel because sensor is busy\n"); sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO); return false; } } static bool sensorFirmwareUpload(void *cookie) { TDECL(); int i = (long int)cookie; sensorSignalInternalEvt(T(sensors[i]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0); return true; } static bool enableInterrupt(struct Gpio *pin, IRQn_Type irq, struct ChainedIsr *isr) { gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE); syscfgSetExtiPort(pin); extiEnableIntGpio(pin, EXTI_TRIGGER_RISING); extiChainIsr(irq, isr); return true; } static bool disableInterrupt(struct Gpio *pin, IRQn_Type irq, struct ChainedIsr *isr) { extiUnchainIsr(irq, isr); extiDisableIntGpio(pin); return true; } static void configInt1(TASK, bool on) { enum SensorIndex i; bool powered = false; if (on && !T(Int1_EN)) { enableInterrupt(T(Int1), T(Irq1), &T(Isr1)); T(Int1_EN) = true; } else if (!on && T(Int1_EN)) { for (i = 0; i < NUM_OF_SENSOR; i++) { if (T(sensors[i]).powered) { powered = true; break; } } if (!powered) { disableInterrupt(T(Int1), T(Irq1), &T(Isr1)); T(Int1_EN) = false; } } } static uint8_t computeOdrConf(uint32_t rate) { switch (rate) { case SENSOR_HZ(1000.0f): return 6; case SENSOR_HZ(200.0f): return 7; case SENSOR_HZ(100.0f): return 8; case SENSOR_HZ(50.0f): return 9; case SENSOR_HZ(25.0f): default: return 10; } } static void computeConfig(TASK, struct ICM40600Config *config) { uint64_t latency; uint64_t val; uint32_t latency_ms; int i; // copy current parameters *config = T(config); // compute sensors on if (T(sensors[ACC]).configed || T(sensors[WOM]).configed || T(sensors[NOMO]).configed) { config->accel_on = true; } else { config->accel_on = false; } config->gyro_on = T(sensors[GYR]).configed; if (T(sensors[WOM]).configed || T(sensors[NOMO]).configed) { config->wom_on = true; } else { config->wom_on = false; } // compute accel and gyro rates if (config->accel_on) { if (T(sensors[ACC]).configed) { if (T(sensors[ACC]).rate > NO_DECIMATION_MAX_RATE) { config->accel_rate = DECIMATION_HIGH_RATE; } else if (T(sensors[ACC]).rate < NO_DECIMATION_MIN_RATE) { config->accel_rate = DECIMATION_LOW_RATE; } else { config->accel_rate = T(sensors[ACC]).rate; } } else { config->accel_rate = NO_DECIMATION_MIN_RATE; } } if (config->gyro_on) { if (T(sensors[GYR]).configed) { if (T(sensors[GYR]).rate > NO_DECIMATION_MAX_RATE) { config->gyro_rate = DECIMATION_HIGH_RATE; } else if (T(sensors[GYR]).rate < NO_DECIMATION_MIN_RATE) { config->gyro_rate = DECIMATION_LOW_RATE; } else { config->gyro_rate = T(sensors[GYR]).rate; } } else { config->gyro_rate = NO_DECIMATION_MIN_RATE; } } // compute wom threshold if (config->wom_on) { config->wom_threshold = ICM40600_WOM_THRESHOLD_MG / (config->accel_rate / NO_DECIMATION_MIN_RATE); } // compute fifo configuration if (T(sensors[ACC]).configed || T(sensors[GYR]).configed) { config->fifo_sample_size = FIFO_PACKET_SIZE; } else { config->fifo_sample_size = 0; } // compute fifo rate and latency/watermark config->fifo_rate = 0; latency = SENSOR_LATENCY_NODATA; for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) { if (T(sensors[i]).configed) { // look for the highest rate if (T(sensors[i]).rate > config->fifo_rate) { config->fifo_rate = T(sensors[i]).rate; } // look for the shortest latency if (T(sensors[i]).latency < latency) { latency = T(sensors[i]).latency; } } } if (config->fifo_rate > NO_DECIMATION_MAX_RATE) { config->fifo_rate = DECIMATION_HIGH_RATE; } else if (config->fifo_rate < NO_DECIMATION_MIN_RATE) { config->fifo_rate = DECIMATION_LOW_RATE; } // add 0.5ms for rounding latency_ms = cpuMathU64DivByU16(cpuMathU64DivByU16(latency + 500000, 1000), 1000); val = (uint64_t)latency_ms * (uint64_t)config->fifo_rate; config->fifo_watermark = cpuMathU64DivByU16(cpuMathU64DivByU16(val, 1000), RATE_TO_HZ); config->fifo_watermark *= config->fifo_sample_size; if (config->fifo_watermark < config->fifo_sample_size) { config->fifo_watermark = config->fifo_sample_size; } if (config->fifo_watermark > MAX_BATCH_SIZE) { config->fifo_watermark = MAX_BATCH_SIZE - (MAX_BATCH_SIZE % config->fifo_sample_size); } } static void updateConfig(TASK, const struct ICM40600Config *config) { uint32_t delay_us; uint8_t val, val2; // Disable Interrupt SPI_WRITE(REG_INT_SOURCE0, 0); SPI_WRITE(REG_INT_SOURCE1, 0); // set rate and WoM threshold if (config->gyro_rate != T(config).gyro_rate) { val = computeOdrConf(config->gyro_rate); SPI_WRITE(REG_GYRO_CONFIG0, (ICM40600_GYR_FS << SHIFT_GYRO_FS_SEL) | (val << SHIFT_ODR_CONF)); } if (config->accel_rate != T(config).accel_rate) { val = computeOdrConf(config->accel_rate); SPI_WRITE(REG_ACCEL_CONFIG0, (ICM40600_ACC_FS << SHIFT_ACCEL_FS_SEL) | (val << SHIFT_ODR_CONF)); } if (config->wom_threshold != T(config).wom_threshold) { val = ICM40600_WOM_COMPUTE(config->wom_threshold); SPI_WRITE(REG_ACCEL_WOM_X_THR, val); SPI_WRITE(REG_ACCEL_WOM_Y_THR, val); SPI_WRITE(REG_ACCEL_WOM_Z_THR, val); } // set WM if (config->fifo_watermark != T(config).fifo_watermark) { SPI_WRITE(REG_FIFO_CONFIG2, config->fifo_watermark & 0xFF); SPI_WRITE(REG_FIFO_CONFIG3, (config->fifo_watermark >> 8) & 0xFF); } // turn on/off accel and gyro if any change if (config->gyro_on != T(config).gyro_on || config->accel_on != T(config).accel_on) { val = 0; if (config->gyro_on) { val |= BIT_GYRO_MODE_LN; } if (config->accel_on) { val |= BIT_ACCEL_MODE_LN; } // delay needed if gyro or accel turning on if ((config->gyro_on && !T(config).gyro_on) || (config->accel_on && !T(config).accel_on)) { delay_us = 200; // 9136 } else { delay_us = 0; } SPI_WRITE(REG_PWR_MGMT_0, val, delay_us); } // turn on/off WOM if (config->wom_on != T(config).wom_on) { if (config->wom_on) { val = BIT_WOM_INT_MODE_OR | BIT_WOM_MODE_PREV | BIT_SMD_MODE_OLD; } else { val = 0; } SPI_WRITE(REG_SMD_CONFIG, val); } // turn on/off FIFO if (config->fifo_sample_size != T(config).fifo_sample_size) { if (config->fifo_sample_size != 0) { // enabling FIFO val = BIT_FIFO_MODE_STREAM; val2 = BIT_FIFO_ACCEL_EN | BIT_FIFO_GYRO_EN | BIT_FIFO_TEMP_EN | BIT_FIFO_TMST_FSYNC_EN | BIT_FIFO_WM_TH; // reset chip time T(chip_time_us) = 0; T(chip_timestamp) = 0; T(fifo_start_sync) = true; invalidate_sensortime_to_rtc_time(_task); } else { // disabling FIFO val = BIT_FIFO_MODE_BYPASS; val2 = 0; } SPI_WRITE(REG_FIFO_CONFIG, val); SPI_WRITE(REG_FIFO_CONFIG1, val2); if (val == BIT_FIFO_MODE_BYPASS) { SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[0])); // 9052 } } // enable/disable FIFO data interrupt if (config->fifo_sample_size != 0) { val = BIT_INT_FIFO_THS_INT1_EN; } else { val = 0; } SPI_WRITE(REG_INT_SOURCE0, val); // enable/disable WOM interrupt (only when FIFO disabled or batch mode) if (config->wom_on && (config->fifo_sample_size == 0 || config->fifo_watermark > config->fifo_sample_size)) { val = BIT_INT_WOM_XYZ_INT1_EN; } else { val = 0; } SPI_WRITE(REG_INT_SOURCE1, val); } static void applyConfig(TASK, const struct ICM40600Config *config) { uint32_t duration_us; uint32_t decimator; int i; // setup wait for odr change if (config->accel_rate != T(config).accel_rate) { T(sensors[ACC]).wait_for_odr = true; } if (config->gyro_rate != T(config).gyro_rate) { T(sensors[GYR]).wait_for_odr = true; } // setup first samples skip if (config->gyro_on && !T(config).gyro_on) { // gyro turning on duration_us = (1000000 * RATE_TO_HZ) / config->gyro_rate; if (duration_us * GYR_SKIP_SAMPLE_NB > ICM40600_GYRO_START_TIME_MS * 1000) { T(sensors[GYR]).skip_sample_cnt = GYR_SKIP_SAMPLE_NB; } else { T(sensors[GYR]).skip_sample_cnt = (ICM40600_GYRO_START_TIME_MS * 1000) / duration_us; if ((ICM40600_GYRO_START_TIME_MS * 1000) % duration_us) { T(sensors[GYR]).skip_sample_cnt++; } } } if (config->accel_on && !T(config).accel_on) { // accel turning on duration_us = (1000000 * RATE_TO_HZ) / config->accel_rate; if (duration_us * ACC_SKIP_SAMPLE_NB > ICM40600_ACCEL_START_TIME_MS * 1000) { T(sensors[ACC]).skip_sample_cnt = ACC_SKIP_SAMPLE_NB; } else { T(sensors[ACC]).skip_sample_cnt = (ICM40600_ACCEL_START_TIME_MS * 1000) / duration_us; if ((ICM40600_ACCEL_START_TIME_MS * 1000) % duration_us) { T(sensors[ACC]).skip_sample_cnt++; } } } // update all sensors decimators for (i = 0; i <= GYR; i++) { if (!T(sensors[i]).configed || T(sensors[i]).rate == 0) { decimator = 1; } else { if (i == ACC) { decimator = config->accel_rate / T(sensors[i]).rate; } else if (i == GYR) { decimator = config->gyro_rate / T(sensors[i]).rate; } } if (decimator != T(sensors[i]).decimator) { T(sensors[i]).decimator = decimator; T(sensors[i]).data_cnt = 0; } } // setup NOMO timer if (T(sensors[NOMO]).configed && !T(noMotionTimerHandle)) { T(noMotionTimerHandle) = timTimerSet(ICM40600_NOM_DURATION_NS, 0, 100, noMotionCallback, &T(sensors[NOMO]), false); } else if (!T(sensors[NOMO]).configed && T(noMotionTimerHandle)) { timTimerCancel(T(noMotionTimerHandle)); T(noMotionTimerHandle) = 0; } // update config T(config) = *config; DEBUG_PRINT("config: accel(%d, %luHz/%u), gyro(%d, %luHz/%u), wom(%d, %lu), " "fifo(%u/%u, %luHz)\n", T(config).accel_on, T(config).accel_rate / RATE_TO_HZ, T(sensors[ACC]).decimator, T(config).gyro_on, T(config).gyro_rate / RATE_TO_HZ, T(sensors[GYR]).decimator, T(config).wom_on, T(config).wom_threshold, T(config).fifo_sample_size, T(config).fifo_watermark, T(config).fifo_rate / RATE_TO_HZ); } static void configSensor(TASK) { struct ICM40600Config config; computeConfig(_task, &config); updateConfig(_task, &config); applyConfig(_task, &config); } static void sensorTurnOn(TASK) { /* enable chip timestamp */ SPI_WRITE(REG_TMST_CONFIG, BIT_EN_DREG_FIFO_D2A | BIT_TMST_EN); T(powered) = true; #if DBG_TIMESTAMP memset(&T(statistics_set), 0, sizeof(struct StatisticsSet)); #endif DEBUG_PRINT("chip on\n"); } static void sensorTurnOff(TASK) { /* disable chip timestamp */ SPI_WRITE(REG_TMST_CONFIG, BIT_EN_DREG_FIFO_D2A); T(powered) = false; #if DBG_TIMESTAMP DEBUG_PRINT("time sync stats: reset: %ld, sync: %ld, adjust+: %ld, adjust-: %ld, truncate: %ld\n", T(statistics_set).sync_reset_count, T(statistics_set).sync_count, T(statistics_set).sync_adjust_plus, T(statistics_set).sync_adjust_minus, T(statistics_set).sync_truncate); #endif DEBUG_PRINT("chip off\n"); } static void sensorPower(TASK, int sensorType, bool on) { bool chip_on; int i; if (on) { // turn on chip if needed if (!T(powered)) { sensorTurnOn(_task); } } else { // change chip configuration configSensor(_task); // turn chip off if needed chip_on = false; for (i = 0; i < NUM_OF_SENSOR; ++i) { if (T(sensors[i]).powered) { chip_on = true; break; } } if (!chip_on) { sensorTurnOff(_task); } } } static bool sensorSetPower(bool on, void *cookie) { TDECL(); int sensor_type = (int)cookie; struct ICM40600Sensor *sensor = &T(sensors[sensor_type]); DEBUG_PRINT("%s: on=%d, state=%" PRI_STATE "\n", mSensorInfo[sensor_type].sensorName, on, getStateName(GET_STATE())); if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) { sensor->powered = on; if (!on) { sensor->configed = false; } configInt1(_task, on); sensorPower(_task, sensor_type, on); spiBatchTxRx(&T(mode), sensorSpiCallback, sensor, __FUNCTION__); } else { T(pending_config[sensor_type]) = true; sensor->pConfig.enable = on; } return true; } static bool sensorSetRate(uint32_t rate, uint64_t latency, void *cookie) { TDECL(); int sensor_type = (int)cookie; struct ICM40600Sensor *sensor = &T(sensors[sensor_type]); DEBUG_PRINT("%s: rate=%lu, latency=%llu, state=%" PRI_STATE "\n", mSensorInfo[sensor_type].sensorName, rate, latency, getStateName(GET_STATE())); if (trySwitchState(SENSOR_CONFIG_CHANGING)) { sensor->rate = rate; sensor->latency = latency; sensor->configed = true; configSensor(_task); spiBatchTxRx(&T(mode), sensorSpiCallback, sensor, __FUNCTION__); } else { T(pending_config[sensor_type]) = true; sensor->pConfig.enable = sensor->powered; sensor->pConfig.rate = rate; sensor->pConfig.latency = latency; } return true; } static void sendFlushEvt(void) { TDECL(); uint32_t evtType = 0; int i; for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) { while (T(sensors[i]).flush > 0) { evtType = sensorGetMyEventType(mSensorInfo[i].sensorType); osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); T(sensors[i]).flush--; } } } static void int1Evt(TASK, bool flush); static bool flushSensor(void *cookie) { TDECL(); int sensor_idx = (int)cookie; uint32_t evtType; DEBUG_PRINT("%s flush\n", mSensorInfo[sensor_idx].sensorName); if (sensor_idx >= FIRST_CONT_SENSOR && sensor_idx < NUM_CONT_SENSOR) { T(sensors[sensor_idx]).flush++; int1Evt(_task, true); return true; } if (sensor_idx >= FIRST_ONESHOT_SENSOR && sensor_idx < NUM_OF_SENSOR) { evtType = sensorGetMyEventType(mSensorInfo[sensor_idx].sensorType); osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL); return true; } return false; } static bool flushData(struct ICM40600Sensor *sensor, uint32_t eventId) { bool success = false; if (sensor->data_evt) { success = osEnqueueEvtOrFree(eventId, sensor->data_evt, dataEvtFree); sensor->data_evt = NULL; } return success; } static void flushAllData(void) { TDECL(); int i; for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) { flushData(&T(sensors[i]), EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType)); } } static bool allocateDataEvt(struct ICM40600Sensor *mSensor, uint64_t rtc_time) { TDECL(); mSensor->data_evt = slabAllocatorAlloc(T(mDataSlab)); if (mSensor->data_evt == NULL) { // slab allocation failed ERROR_PRINT("slabAllocatorAlloc() failed\n"); return false; } // delta time for the first sample is sample count memset(&mSensor->data_evt->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample)); mSensor->data_evt->referenceTime = rtc_time; mSensor->prev_rtc_time = rtc_time; return true; } static void parseOnePacket(TASK, const uint8_t *data, int idx) { struct FifoPacketData fifo_data; uint16_t diff; getFifoData(data, idx, &fifo_data); /* wait for odr */ if (T(sensors[ACC]).wait_for_odr) { if (fifo_data.odr_accel) { T(sensors[ACC]).wait_for_odr = false; } else { fifo_data.valid_accel = false; } } if (T(sensors[GYR]).wait_for_odr) { if (fifo_data.odr_gyro) { T(sensors[GYR]).wait_for_odr = false; } else { fifo_data.valid_gyro = false; } } /* drop first some samples */ if (fifo_data.valid_accel) { if (T(sensors[ACC]).skip_sample_cnt > 0) { fifo_data.valid_accel = false; T(sensors[ACC]).skip_sample_cnt--; } } if (fifo_data.valid_gyro) { if (T(sensors[GYR]).skip_sample_cnt > 0) { fifo_data.valid_gyro = false; T(sensors[GYR]).skip_sample_cnt--; } } /* update sensors data */ if (fifo_data.valid_accel) { ICM40600_TO_ANDROID_COORDINATE(fifo_data.accel[0], fifo_data.accel[1], fifo_data.accel[2]); if (T(sensors[ACC]).configed) { T(sensors[ACC]).data[0] = fifo_data.accel[0]; T(sensors[ACC]).data[1] = fifo_data.accel[1]; T(sensors[ACC]).data[2] = fifo_data.accel[2]; T(sensors[ACC]).updated = true; T(sensors[ACC]).data_cnt++; } } if (fifo_data.valid_gyro) { ICM40600_TO_ANDROID_COORDINATE(fifo_data.gyro[0], fifo_data.gyro[1], fifo_data.gyro[2]); if (T(sensors[GYR]).configed) { T(sensors[GYR]).data[0] = fifo_data.gyro[0]; T(sensors[GYR]).data[1] = fifo_data.gyro[1]; T(sensors[GYR]).data[2] = fifo_data.gyro[2]; T(sensors[GYR]).updated = true; T(sensors[GYR]).data_cnt++; } } // update temperature T(chip_temperature) = fifo_data.temp * TEMP_SCALE + TEMP_OFFSET; // count up chip time if (T(chip_time_us) == 0) { T(chip_time_us) = (uint64_t)fifo_data.timestamp * CHIP_TIME_RES_US + CHIP_TIME_OFFSET_US; } else { // unsigned difference handle counter roll-up diff = fifo_data.timestamp - T(chip_timestamp); T(chip_time_us) += diff * CHIP_TIME_RES_US; } T(chip_timestamp) = fifo_data.timestamp; } static void pushSensorData(TASK, struct ICM40600Sensor *mSensor, uint64_t rtc_time) { float x, y, z; float offset[3]; bool new_offset_update = false; struct TripleAxisDataPoint *sample; uint32_t delta_time; switch (mSensor->idx) { case ACC: // scale data to android units x = mSensor->data[0] * kScale_acc; y = mSensor->data[1] * kScale_acc; z = mSensor->data[2] * kScale_acc; // run and apply calibration on sensor data #ifdef ACCEL_CAL_ENABLED accelCalRun(&T(accel_cal), rtc_time, x, y, z, T(chip_temperature)); accelCalBiasRemove(&T(accel_cal), &x, &y, &z); # ifdef ACCEL_CAL_DBG_ENABLED accelCalDebPrint(&T(accel_cal), T(chip_temperature)); # endif #endif #ifdef GYRO_CAL_ENABLED gyroCalUpdateAccel(&T(gyro_cal), rtc_time, x, y, z); #endif break; case GYR: // scale data to android units x = mSensor->data[0] * kScale_gyr; y = mSensor->data[1] * kScale_gyr; z = mSensor->data[2] * kScale_gyr; // run and apply calibration on sensor data #ifdef GYRO_CAL_ENABLED gyroCalUpdateGyro(&T(gyro_cal), rtc_time, x, y, z, T(chip_temperature)); gyroCalRemoveBias(&T(gyro_cal), x, y, z, &x, &y, &z); new_offset_update = gyroCalNewBiasAvailable(&T(gyro_cal)); if (new_offset_update) { float gyro_offset_temperature_celsius; gyroCalGetBias(&T(gyro_cal), &offset[0], &offset[1], &offset[2], &gyro_offset_temperature_celsius); } #endif break; default: return; } if (mSensor->data_evt == NULL) { if (!allocateDataEvt(mSensor, rtc_time)) return; } if (mSensor->data_evt->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) { ERROR_PRINT("samples bad index\n"); return; } // handle bias sending if (new_offset_update) { if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) { // flush existing samples so the bias appears after them. flushData(mSensor, EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[mSensor->idx].sensorType)); if (!allocateDataEvt(mSensor, rtc_time)) { return; } } mSensor->data_evt->samples[0].firstSample.biasCurrent = true; mSensor->data_evt->samples[0].firstSample.biasPresent = 1; mSensor->data_evt->samples[0].firstSample.biasSample = mSensor->data_evt->samples[0].firstSample.numSamples; sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; // Updates the offset in HAL. sample->x = offset[0]; sample->y = offset[1]; sample->z = offset[2]; flushData(mSensor, sensorGetMyEventType(mSensorInfo[mSensor->idx].biasType)); DEBUG_PRINT("send new gyro bias\n"); if (!allocateDataEvt(mSensor, rtc_time)) { return; } } sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; // the first deltatime is for sample size if (mSensor->data_evt->samples[0].firstSample.numSamples > 1) { delta_time = rtc_time - mSensor->prev_rtc_time; delta_time = delta_time < 0 ? 0 : delta_time; sample->deltaTime = delta_time; mSensor->prev_rtc_time = rtc_time; } sample->x = x; sample->y = y; sample->z = z; if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) { flushAllData(); } } static void computeTimeSync(TASK, const uint8_t *data, uint32_t count, uint64_t data_timestamp) { const uint64_t now = sensorGetTime(); uint64_t chip_time_us; uint32_t sample_index; uint32_t duration_us; struct FifoPacketData packet; uint16_t sample_nb; uint16_t chip_timestamp; uint16_t diff; bool first_sync = false; bool sync = false; /* scan fifo data for the latest chip timestamp */ chip_time_us = T(chip_time_us); chip_timestamp = T(chip_timestamp); sample_index = 0; sample_nb = 0; while (count >= T(config).fifo_sample_size) { sample_nb++; getFifoData(data, sample_index, &packet); sample_index += T(config).fifo_sample_size; count -= T(config).fifo_sample_size; // count up chip time if (chip_time_us == 0) { // first chip timestamp chip_time_us = (uint64_t)packet.timestamp * CHIP_TIME_RES_US + CHIP_TIME_OFFSET_US; } else { // unsigned difference handle counter roll-up diff = packet.timestamp - chip_timestamp; chip_time_us += diff * CHIP_TIME_RES_US; } chip_timestamp = packet.timestamp; } /* first time sync after FIFO enable */ if (T(fifo_start_sync)) { if (T(config).fifo_rate != 0) { duration_us = (1000000 * RATE_TO_HZ) / T(config).fifo_rate; } else { duration_us = 0; } // use estimated duration map_sensortime_to_rtc_time(_task, chip_time_us - duration_us * sample_nb, data_timestamp - (uint64_t)duration_us * 1000ULL * sample_nb); T(fifo_start_sync) = false; T(last_sync_time) = now; first_sync = true; } /* periodical time sync */ if ((now - T(last_sync_time)) > MSEC_TO_NANOS(100)) { // every 100ms uint64_t estimated_rtc; uint64_t min_rtc, max_rtc; uint64_t limit_ns, adjust_ns; uint64_t updated_data_timestamp = data_timestamp; bool valid = sensortime_to_rtc_time(_task, chip_time_us, &estimated_rtc); // check if appropriate to inject to time sync algorithm // adjust rtc time if necessary not to make large jitters if (valid) { limit_ns = U64_DIV_BY_CONST_U16((data_timestamp - T(last_sync_data_ts)) * 10, 1000); // 1% (100ms * 1% = 1ms) adjust_ns = U64_DIV_BY_CONST_U16((data_timestamp - T(last_sync_data_ts)) * 1, 10000); // 0.01% (100ms * 0.01% = 10us) min_rtc = data_timestamp - limit_ns; // actual ts - x max_rtc = data_timestamp + limit_ns; // actual ts + x if ((estimated_rtc >= min_rtc) && (estimated_rtc <= max_rtc)) { sync = true; } else if (estimated_rtc < min_rtc) { sync = true; updated_data_timestamp = updated_data_timestamp - adjust_ns; #if DBG_TIMESTAMP T(statistics_set).sync_adjust_minus++; #endif } else if (estimated_rtc > max_rtc) { sync = true; updated_data_timestamp = updated_data_timestamp + adjust_ns; #if DBG_TIMESTAMP T(statistics_set).sync_adjust_plus++; #endif } // limit the adjustment if (sync) { if (updated_data_timestamp > (data_timestamp + MSEC_TO_NANOS(1))) { updated_data_timestamp = data_timestamp + MSEC_TO_NANOS(1); } } } if (sync) { data_timestamp = updated_data_timestamp; } } /* do time sync */ if (first_sync || sync) { map_sensortime_to_rtc_time(_task, chip_time_us, data_timestamp); T(last_sync_time) = now; T(last_sync_data_ts) = data_timestamp; } } static void dispatchData(TASK, const uint8_t *data, uint32_t count) { uint64_t ts = 0; uint32_t sample_index = 0; int i; if (count == 0) { return; } /* do time sync if no flush or fist time starting FIFO */ if (T(fifo_start_sync) || !T(flush)) { computeTimeSync(_task, data, count, T(data_timestamp)); } /* process FIFO data */ while (count >= T(config).fifo_sample_size) { for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) { T(sensors[i]).updated = false; } parseOnePacket(_task, data, sample_index); sample_index += T(config).fifo_sample_size; count -= T(config).fifo_sample_size; // compute timestamp if (!sensortime_to_rtc_time(_task, T(chip_time_us), &ts)) { continue; } // add data for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) { if (!T(sensors[i]).configed || !T(sensors[i]).updated) { continue; } if (T(sensors[i]).data_cnt % T(sensors[i]).decimator == 0) { if (ts <= T(sensors[i]).prev_rtc_time) { ts = T(sensors[i]).prev_rtc_time + MIN_INCREMENT_TIME_NS; #if DBG_TIMESTAMP T(statistics_set).sync_truncate++; #endif } pushSensorData(_task, &T(sensors[i]), ts); } } } flushAllData(); } static void int1Handling(TASK) { uint8_t int_status; uint8_t int_status2; uint16_t fifo_count = 0; union EmbeddedDataPoint trigger_axies; const uint8_t *buf; switch (T(int_state)) { case INT_READ_STATUS: T(int_state) = INT_PROCESS_STATUS; // read INT status1, status2 and fifo_count registers SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0])); SPI_READ(REG_INT_STATUS2, 1, &T(dataBuffer[1])); SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[2])); T(data_timestamp) = sensorGetTime(); spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__); break; case INT_PROCESS_STATUS: buf = T(dataBuffer[0]); int_status = buf[1]; buf = T(dataBuffer[1]); int_status2 = buf[1]; buf = T(dataBuffer[2]); fifo_count = buf[2] << 8 | buf[1]; // INT status handling if (int_status2 & BIT_INT_STATUS_WOM_XYZ) { if (T(sensors[WOM]).configed) { trigger_axies.idata = ((int_status2 & BIT_INT_STATUS_WOM_X) >> 0) | ((int_status2 & BIT_INT_STATUS_WOM_Y) >> 1) | ((int_status2 & BIT_INT_STATUS_WOM_Z) >> 2); osEnqueueEvt(EVT_SENSOR_ANY_MOTION, trigger_axies.vptr, NULL); } if (T(sensors[NOMO]).configed && T(noMotionTimerHandle)) { timTimerCancel(T(noMotionTimerHandle)); T(noMotionTimerHandle) = timTimerSet(ICM40600_NOM_DURATION_NS, 0, 100, noMotionCallback, (void *)&T(sensors[NOMO]), false); } } // FIFO overflow if (int_status & BIT_INT_STATUS_FIFO_FULL) { ERROR_PRINT("fifo overflow\n"); } // FIFO WM status handling if ((int_status & BIT_INT_STATUS_FIFO_THS) || T(flush)) { T(int_state) = INT_READ_FIFO_DATA; } else { T(int_state) = INT_DONE; T(fifo_count) = 0; sensorSpiCallback(_task, 0); break; } // fall-through case INT_READ_FIFO_DATA: T(int_state) = INT_DONE; // compute fifo count and delete partial sample fifo_count -= fifo_count % T(config).fifo_sample_size; // read FIFO data T(fifo_count) = fifo_count; if (fifo_count > 0) { SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0])); spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__); } else { sensorSpiCallback(_task, 0); } break; default: T(int_state) = INT_DONE; T(fifo_count) = 0; sensorSpiCallback(_task, 0); break; } } static void int1Evt(TASK, bool flush) { if (trySwitchState(SENSOR_INT_1_HANDLING)) { T(flush) = flush; T(int_state) = INT_READ_STATUS; int1Handling(_task); } else { if (flush) { T(pending_flush) = true; } else { T(pending_int[0]) = true; } } } #define DEC_OPS(power, firmware, rate, flush) \ .sensorPower = power, \ .sensorFirmwareUpload = firmware, \ .sensorSetRate = rate, \ .sensorFlush = flush #define DEC_OPS_CAL_CFG_TEST(power, firmware, rate, flush, cal, cfg, test) \ DEC_OPS(power, firmware, rate, flush), \ .sensorCalibrate = cal, \ .sensorCfgData = cfg, \ .sensorSelfTest = test, #define DEC_OPS_CFG(power, firmware, rate, flush, cfg) \ DEC_OPS(power, firmware, rate, flush), \ .sensorCfgData = cfg static const struct SensorOps mSensorOps[NUM_OF_SENSOR] = { { DEC_OPS_CAL_CFG_TEST(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor, accCalibration, accCfgData, accSelfTest) }, { DEC_OPS_CAL_CFG_TEST(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor, gyrCalibration, gyrCfgData, gyrSelfTest) }, { DEC_OPS(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor) }, { DEC_OPS(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor) }, }; static void configEvent(struct ICM40600Sensor *mSensor, struct ConfigStat *ConfigData) { TDECL(); int i; for (i = 0; &T(sensors[i]) != mSensor; i++) ; if (ConfigData->enable == 0 && mSensor->powered) { mSensorOps[i].sensorPower(false, (void *)i); } else if (ConfigData->enable == 1 && !mSensor->powered) { mSensorOps[i].sensorPower(true, (void *)i); } else { mSensorOps[i].sensorSetRate(ConfigData->rate, ConfigData->latency, (void *)i); } } static void processPendingEvt(TASK) { enum SensorIndex i; if (T(pending_int[0])) { T(pending_int[0]) = false; int1Evt(_task, false); return; } if (T(pending_flush)) { T(pending_flush) = false; int1Evt(_task, true); return; } for (i = 0; i < NUM_OF_SENSOR; i++) { if (T(pending_config[i])) { T(pending_config[i]) = false; configEvent(&T(sensors[i]), &T(sensors[i]).pConfig); return; } } if (T(pending_calibration_save)) { T(pending_calibration_save) = !saveCalibration(_task); return; } } static void sensorInit(TASK) { const uint8_t *buf; switch (T(init_state)) { case RESET_ICM40600: // reset chip and turn on SPI_WRITE(REG_DEVICE_CONFIG, BIT_SOFT_RESET, 100 * 1000); SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0])); T(powered) = false; sensorTurnOn(_task); // set SPI speed register SPI_WRITE(REG_SPI_SPEED, ICM40600_SPI_SPEED_REG_VALUE); T(init_state) = INIT_ICM40600; spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__); break; case INIT_ICM40600: buf = T(dataBuffer[0]); if (!(buf[1] & BIT_INT_STATUS_RESET_DONE)) { ERROR_PRINT("chip reset failed!\n"); } // reconfigure SPI speed T(mode).speed = ICM40600_SPI_SPEED_HZ; // configure interface // i2c disabled // fifo_count: little endian // sensor data: little endian SPI_WRITE(REG_INTF_CONFIG0, BIT_UI_SIFS_DISABLE_I2C); // configure interrupt SPI_WRITE(REG_INT_CONFIG, (INT1_POLARITY << SHIFT_INT1_POLARITY) | (INT1_DRIVE_CIRCUIT << SHIFT_INT1_DRIVE_CIRCUIT) | (INT1_MODE << SHIFT_INT1_MODE)); SPI_WRITE(REG_INT_CONFIG1, BIT_INT_ASY_RST_DISABLE); // 9139 // static config of sensors SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND); SPI_WRITE(REG_GYRO_CONFIG1, 0x1A); // default reset value SPI_WRITE(REG_ACCEL_CONFIG1, 0x15); // default reset value // turn off FIFO and sensors SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_BYPASS); SPI_WRITE(REG_FIFO_CONFIG1, 0); SPI_WRITE(REG_FIFO_CONFIG2, 0); SPI_WRITE(REG_FIFO_CONFIG3, 0); SPI_WRITE(REG_SMD_CONFIG, BIT_SMD_MODE_OFF); SPI_WRITE(REG_PWR_MGMT_0, BIT_GYRO_MODE_OFF | BIT_ACCEL_MODE_OFF, 200); // 9136 sensorTurnOff(_task); T(init_state) = INIT_DONE; spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__); break; default: break; } } static void handleSpiDoneEvt(const void* evtData) { TDECL(); struct ICM40600Sensor *mSensor; bool returnIdle = false; uint32_t i; const uint8_t *buf; #ifdef ACCEL_CAL_ENABLED bool accelCalNewBiasAvailable; struct TripleAxisDataPoint *sample; float accelCalBiasX, accelCalBiasY, accelCalBiasZ; bool fallThrough; #endif switch (GET_STATE()) { case SENSOR_BOOT: SET_STATE(SENSOR_VERIFY_ID); SPI_READ(REG_WHO_AM_I, 1, &T(dataBuffer[0])); spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__); break; case SENSOR_VERIFY_ID: buf = T(dataBuffer[0]); if (buf[1] != WHO_AM_I_ICM40604 && buf[1] != WHO_AM_I_ICM40605) { ERROR_PRINT("chip not found (id=0x%x)\n", buf[1]); break; } INFO_PRINT("chip found (id=0x%x)\n", buf[1]); SET_STATE(SENSOR_INITIALIZING); T(init_state) = RESET_ICM40600; sensorInit(_task); break; case SENSOR_INITIALIZING: if (T(init_state) == INIT_DONE) { for (i = 0; i < NUM_OF_SENSOR; i++) { sensorRegisterInitComplete(T(sensors[i]).handle); } returnIdle = true; } else { sensorInit(_task); } break; case SENSOR_POWERING_UP: mSensor = (struct ICM40600Sensor *)evtData; sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0); returnIdle = true; break; case SENSOR_POWERING_DOWN: mSensor = (struct ICM40600Sensor *)evtData; #ifdef ACCEL_CAL_ENABLED if (mSensor->idx == ACC) { // https://source.android.com/devices/sensors/sensor-types.html // "The bias and scale calibration must only be updated while the sensor is deactivated, // so as to avoid causing jumps in values during streaming." accelCalNewBiasAvailable = accelCalUpdateBias(&T(accel_cal), &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ); // notify HAL about new accel bias calibration if (accelCalNewBiasAvailable) { fallThrough = true; if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) { // flush existing samples so the bias appears after them flushData(mSensor, EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[ACC].sensorType)); // try to allocate another data event and break if unsuccessful if (!allocateDataEvt(mSensor, sensorGetTime())) { fallThrough = false; } } if (fallThrough) { mSensor->data_evt->samples[0].firstSample.biasCurrent = true; mSensor->data_evt->samples[0].firstSample.biasPresent = 1; mSensor->data_evt->samples[0].firstSample.biasSample = mSensor->data_evt->samples[0].firstSample.numSamples; sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++]; sample->x = accelCalBiasX; sample->y = accelCalBiasY; sample->z = accelCalBiasZ; flushData(mSensor, sensorGetMyEventType(mSensorInfo[ACC].biasType)); DEBUG_PRINT("send new accel bias\n"); allocateDataEvt(mSensor, sensorGetTime()); } } } #endif sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0); returnIdle = true; break; case SENSOR_INT_1_HANDLING: if (T(int_state) == INT_DONE) { buf = T(dataBuffer[0]); dispatchData(_task, &buf[1], T(fifo_count)); if (T(flush)) { sendFlushEvt(); } returnIdle = true; } else { int1Handling(_task); } break; case SENSOR_CONFIG_CHANGING: mSensor = (struct ICM40600Sensor *)evtData; sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency); returnIdle = true; break; case SENSOR_CALIBRATING: mSensor = (struct ICM40600Sensor *)evtData; if (mSensor->idx == ACC) { if (T(calibration_state) == CALIBRATION_DONE) { INFO_PRINT("Accel Calibration done\n"); returnIdle = true; } else { calibrationHandling(_task, ACC); } } else if (mSensor->idx == GYR) { if (T(calibration_state) == CALIBRATION_DONE) { INFO_PRINT("Gyro Calibration done\n"); returnIdle = true; } else { calibrationHandling(_task, GYR); } } break; case SENSOR_TESTING: mSensor = (struct ICM40600Sensor *)evtData; if (mSensor->idx == ACC) { if (T(selftest_state) == TEST_DONE) { INFO_PRINT("Accel Selftest done\n"); returnIdle = true; } else { selfTestHandling(_task, ACC); } } else if (mSensor->idx == GYR) { if (T(selftest_state) == TEST_DONE) { INFO_PRINT("Gyro Selftest done\n"); returnIdle = true; } else { selfTestHandling(_task, GYR); } } break; case SENSOR_SAVE_CALIBRATION: returnIdle = true; break; default: break; } if (returnIdle) { SET_STATE(SENSOR_IDLE); processPendingEvt(_task); } } #ifdef GYRO_CAL_ENABLED static void processMagEvents(TASK, const struct TripleAxisDataEvent *evtData) { const struct SensorFirstSample * const first = &evtData->samples[0].firstSample; const struct TripleAxisDataPoint *sample; uint64_t ts = evtData->referenceTime; unsigned int i; for (i = 0; i < first->numSamples; ++i) { sample = &evtData->samples[i]; if (i > 0) { ts += sample->deltaTime; } gyroCalUpdateMag(&T(gyro_cal), ts, sample->x, sample->y, sample->z); } } #endif static void handleEvent(uint32_t evtType, const void* evtData) { TDECL(); switch (evtType) { case EVT_APP_START: SET_STATE(SENSOR_BOOT); osEventUnsubscribe(T(tid), EVT_APP_START); #ifdef GYRO_CAL_ENABLED osEventSubscribe(T(tid), EVT_SENSOR_MAG_DATA_RDY); #endif // fall through case EVT_SPI_DONE: handleSpiDoneEvt(evtData); break; case EVT_SENSOR_INTERRUPT_1: int1Evt(_task, false); break; #ifdef GYRO_CAL_ENABLED case EVT_SENSOR_MAG_DATA_RDY: if (evtData != SENSOR_DATA_EVENT_FLUSH) { processMagEvents(_task, (const struct TripleAxisDataEvent *)evtData); } break; #endif default: break; } } static void initSensorStruct(struct ICM40600Sensor *sensor, enum SensorIndex idx) { sensor->data_evt = NULL; sensor->rate = 0; sensor->latency = 0; sensor->decimator = 1; sensor->data_cnt = 0; sensor->prev_rtc_time = 0; sensor->skip_sample_cnt = 0; sensor->data[0] = 0; sensor->data[1] = 0; sensor->data[2] = 0; sensor->offset[0] = 0; sensor->offset[1] = 0; sensor->offset[2] = 0; sensor->updated = false; sensor->powered = false; sensor->configed = false; sensor->wait_for_odr = false; sensor->idx = idx; sensor->flush = 0; } static bool startTask(uint32_t task_id) { TDECL(); enum SensorIndex i; size_t slabSize; time_init(_task); T(tid) = task_id; T(Int1) = gpioRequest(ICM40600_INT1_PIN); T(Irq1) = ICM40600_INT1_IRQ; T(Isr1).func = icm40600Isr1; T(Int1_EN) = false; T(pending_int[0]) = false; T(pending_flush) = false; T(pending_calibration_save) = false; T(mode).speed = ICM40600_SPI_DEFAULT_SPEED_HZ; T(mode).bitsPerWord = 8; T(mode).cpol = SPI_CPOL_IDLE_HI; T(mode).cpha = SPI_CPHA_TRAILING_EDGE; T(mode).nssChange = true; T(mode).format = SPI_FORMAT_MSB_FIRST; T(cs) = GPIO_PB(12); T(config).accel_rate = 0; T(config).gyro_rate = 0; T(config).fifo_rate = 0; T(config).wom_threshold = 0; T(config).fifo_watermark = 0; T(config).fifo_sample_size = 0; T(config).accel_on = false; T(config).gyro_on = false; T(config).wom_on = false; T(noMotionTimerHandle) = 0; T(fifo_count) = 0; T(powered) = false; T(flush) = false; T(data_timestamp) = 0; T(chip_time_us) = 0; T(last_sync_time) = 0; T(last_sync_data_ts) = 0; T(chip_timestamp) = 0; T(fifo_start_sync) = false; T(chip_temperature) = TEMP_OFFSET; spiMasterRequest(ICM40600_SPI_BUS_ID, &T(spiDev)); for (i = 0; i < NUM_OF_SENSOR; i++) { initSensorStruct(&T(sensors[i]), i); T(sensors[i]).handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], (void *)i, false); T(pending_config[i]) = false; } osEventSubscribe(T(tid), EVT_APP_START); #ifdef ACCEL_CAL_ENABLED // Init Accel Cal accelCalInit(&T(accel_cal), 800000000, // Stillness Time in ns (0.8s) 5, // Minimum Sample Number 0.00025, // Threshold 15, // nx bucket count 15, // nxb bucket count 15, // ny bucket count 15, // nyb bucket count 15, // nz bucket count 15, // nzb bucket count 15); // nle bucket count #endif #ifdef GYRO_CAL_ENABLED // Init Gyro Cal gyroCalInit(&T(gyro_cal), SEC_TO_NANOS(5.0f), // Min stillness period = 5.0 seconds SEC_TO_NANOS(5.9f), // Max stillness period = 6.0 seconds (NOTE 1) 0, 0, 0, // Initial bias offset calibration 0, // Time stamp of initial bias calibration SEC_TO_NANOS(1.5f), // Analysis window length = 1.5 seconds 7.5e-5f, // Gyroscope variance threshold [rad/sec]^2 1.5e-5f, // Gyroscope confidence delta [rad/sec]^2 4.5e-3f, // Accelerometer variance threshold [m/sec^2]^2 9.0e-4f, // Accelerometer confidence delta [m/sec^2]^2 5.0f, // Magnetometer variance threshold [uT]^2 1.0f, // Magnetometer confidence delta [uT]^2 0.95f, // Stillness threshold [0,1] 40.0f * MDEG_TO_RAD, // Stillness mean variation limit [rad/sec] 1.5f, // Max temperature delta during stillness [C] true); // Gyro calibration enable // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness // period of 6.0 seconds and avoid buffer boundary conditions that could push // the max stillness to the next multiple of the analysis window length // (i.e., 7.5 seconds). #endif slabSize = sizeof(struct TripleAxisDataEvent) + MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint); // TODO: need to investigate slab items number T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20); if (!T(mDataSlab)) { ERROR_PRINT("slabAllocatorNew() failed\n"); return false; } T(mWbufCnt) = 0; T(mRegCnt) = 0; T(spiInUse) = false; return true; } static void endTask(void) { TDECL(); #ifdef ACCEL_CAL_ENABLED accelCalDestroy(&T(accel_cal)); #endif slabAllocatorDestroy(T(mDataSlab)); spiMasterRelease(T(spiDev)); // disable and release interrupt. disableInterrupt(T(Int1), T(Irq1), &T(Isr1)); gpioRelease(T(Int1)); } INTERNAL_APP_INIT(ICM40600_APP_ID, ICM40600_APP_VERSION, startTask, endTask, handleEvent);