/* Includes ------------------------------------------------------------------*/ #include "lsm6ds3tr_c.h" /* Private macro -------------------------------------------------------------*/ #define BOOT_TIME 15 //ms #define WAIT_TIME_A 100 //ms #define WAIT_TIME_G_01 150 //ms #define WAIT_TIME_G_02 50 //ms /* Self test limits. */ #define MIN_ST_LIMIT_mg 90.0f #define MAX_ST_LIMIT_mg 1700.0f #define MIN_ST_LIMIT_mdps 150000.0f #define MAX_ST_LIMIT_mdps 700000.0f /* Self test results. */ #define ST_PASS 1U #define ST_FAIL 0U #define MIN_ODR(x, y) (x < y ? x : y) #define MAX_ODR(x, y) (x > y ? x : y) #define MAX_PATTERN_NUM FIFO_THRESHOLD / 6 #define LSM6DS3_ODR_LSB_TO_HZ(_odr) (_odr ? (13 << (_odr - 1)) : 0) /* static variable ------------------------------------------------------------------*/ static uint8_t whoamI, rst; static uint16_t pattern_len; static stmdev_ctx_t dev_ctx; static axis3bit16_t data_raw_acceleration; static axis3bit16_t data_raw_angular_rate; static int16_t data_raw_temperature; /* 6ds3 Accelerometer test parameters */ static sensor_lsm6ds3_xl test_6ds3_xl; /* 6ds3 Gyroscope test parameters */ static sensor_lsm6ds3_gy test_6ds3_gyro; //static float acceleration_mg[3]; //static float angular_rate_mdps[3]; //static float temperature_degC; /* static fuction ------------------------------------------------------------------*/ /** Please note that is MANDATORY: return 0 -> no Error.**/ static int32_t platform_write(void *handle, uint8_t reg, const uint8_t *bufp, uint16_t len) { int32_t ierror = 0; if(SPI_OnlyWriteReg(BOARD_SPI0_CS0_IO, reg, (uint8_t *)bufp, len)) { ierror = -1; } return ierror; } static int32_t platform_read(void *handle, uint8_t reg, uint8_t *bufp, uint16_t len) { int32_t ierror = 0; if(SPI_OnlyReadReg(BOARD_SPI0_CS0_IO, reg, bufp, len)) { ierror = -1; } return ierror; } static void platform_delay(uint32_t ms) { nrf_delay_ms(ms); } /* Following routine calculate the FIFO pattern composition based * on gyro and acc enable state and ODR freq */ static uint16_t LSM6DS3TR_C_Calculate_FIFO_Pattern(uint16_t *min_odr, uint16_t *max_odr) { uint16_t fifo_samples_tot_num = 0; /* Calculate min_odr and max_odr for current configuration */ if (test_6ds3_gyro.enable) { test_6ds3_gyro.odr_hz_val = LSM6DS3_ODR_LSB_TO_HZ(test_6ds3_gyro.odr); *max_odr = MAX_ODR(*max_odr, test_6ds3_gyro.odr_hz_val); *min_odr = MIN_ODR(*min_odr, test_6ds3_gyro.odr_hz_val); } if (test_6ds3_xl.enable) { test_6ds3_xl.odr_hz_val = LSM6DS3_ODR_LSB_TO_HZ(test_6ds3_xl.odr); *max_odr = MAX_ODR(*max_odr, test_6ds3_xl.odr_hz_val); *min_odr = MIN_ODR(*min_odr, test_6ds3_xl.odr_hz_val); } /* Calculate how many samples for each sensor are in current FIFO pattern */ if (test_6ds3_gyro.enable) { test_6ds3_gyro.samples_num_in_pattern = test_6ds3_gyro.odr_hz_val / *min_odr; test_6ds3_gyro.decimation = *max_odr / test_6ds3_gyro.odr_hz_val; fifo_samples_tot_num += test_6ds3_gyro.samples_num_in_pattern; } if (test_6ds3_xl.enable) { test_6ds3_xl.samples_num_in_pattern = test_6ds3_xl.odr_hz_val / *min_odr; test_6ds3_xl.decimation = *max_odr / test_6ds3_xl.odr_hz_val; fifo_samples_tot_num += test_6ds3_xl.samples_num_in_pattern; } /* Return the total number of 16-bit samples in the pattern */ return(6 * fifo_samples_tot_num); } /* Following routine read a pattern from FIFO */ static void LSM6DS3TR_C_Read_FIFO_Pattern(axis3bit16_t *acc, uint16_t *acc_num, axis3bit16_t *gry, uint16_t *gry_num) { uint8_t gy_num = test_6ds3_gyro.samples_num_in_pattern; uint8_t xl_num = test_6ds3_xl.samples_num_in_pattern; /* FIFO pattern is composed by gy_num gyroscope triplets and * xl_num accelerometer triplets. The sequence has always following order: * gyro first, accelerometer second */ while(gy_num > 0 || xl_num > 0) { /* Read gyro samples */ if (test_6ds3_gyro.enable && gy_num > 0) { lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_angular_rate.u8bit, 3 * sizeof(int16_t)); gry[*gry_num].i16bit[0] = data_raw_angular_rate.i16bit[0]; gry[*gry_num].i16bit[1] = data_raw_angular_rate.i16bit[1]; gry[*gry_num].i16bit[2] = data_raw_angular_rate.i16bit[2]; (*gry_num)++; // angular_rate_mdps[0] = // lsm6ds3_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[0]); // angular_rate_mdps[1] = // lsm6ds3_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[1]); // angular_rate_mdps[2] = // lsm6ds3_from_fs2000dps_to_mdps(data_raw_angular_rate.i16bit[2]); // sprintf((char*)tx_buffer, "Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n", // angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]); // tx_com( tx_buffer, strlen( (char const*)tx_buffer ) ); gy_num--; } /* Read XL samples */ if (test_6ds3_xl.enable && xl_num > 0) { lsm6ds3tr_c_fifo_raw_data_get(&dev_ctx, data_raw_acceleration.u8bit, 3 * sizeof(int16_t)); acc[*acc_num].i16bit[0] = data_raw_acceleration.i16bit[0]; acc[*acc_num].i16bit[1] = data_raw_acceleration.i16bit[1]; acc[*acc_num].i16bit[2] = data_raw_acceleration.i16bit[2]; (*acc_num)++; // acceleration_mg[0] = // lsm6ds3_from_fs2g_to_mg(data_raw_acceleration.i16bit[0]); // acceleration_mg[1] = // lsm6ds3_from_fs2g_to_mg(data_raw_acceleration.i16bit[1]); // acceleration_mg[2] = // lsm6ds3_from_fs2g_to_mg(data_raw_acceleration.i16bit[2]); // sprintf((char*)tx_buffer, "Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n", // acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]); // tx_com( tx_buffer, strlen( (char const*)tx_buffer ) ); xl_num--; } } } /* API ------------------------------------------------------------------*/ void lsm6ds3tr_c_init(void) { int16_t data_raw[3]; float val_st_off[3]; float val_st_on[3]; float test_val[3]; uint8_t st_result; uint8_t drdy; uint8_t i; uint8_t j; SPI_Init(); /* Initialize mems driver interface */ dev_ctx.write_reg = platform_write; dev_ctx.read_reg = platform_read; dev_ctx.handle = NULL; /* Wait sensor boot time */ platform_delay(BOOT_TIME); /* Check device ID */ lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI); if(whoamI != LSM6DS3TR_C_ID) { SEGGER_RTT_printf(0,"lsm6ds3tr_c_init error!!!\r\n"); return; } /* Restore default configuration */ lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE); do { lsm6ds3tr_c_reset_get(&dev_ctx, &rst); } while(rst); /* Enable Block Data Update */ lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); /* * Accelerometer Self Test */ /* Set Output Data Rate */ lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_52Hz); /* Set full scale */ lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, LSM6DS3TR_C_4g); /* Wait stable output */ platform_delay(WAIT_TIME_A); /* Check if new value available */ do { lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy); } while (!drdy); /* Read dummy data and discard it */ lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw); /* Read 5 sample and get the average vale for each axis */ memset(val_st_off, 0x00, 3 * sizeof(float)); for (i = 0; i < 5; i++) { /* Check if new value available */ do { lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy); } while (!drdy); /* Read data and accumulate the mg value */ lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw); for (j = 0; j < 3; j++) { val_st_off[j] += lsm6ds3tr_c_from_fs4g_to_mg(data_raw[j]); } } /* Calculate the mg average values */ for (i = 0; i < 3; i++) { val_st_off[i] /= 5.0f; } /* Enable Self Test positive (or negative) */ lsm6ds3tr_c_xl_self_test_set(&dev_ctx, LSM6DS3TR_C_XL_ST_NEGATIVE); //lsm6ds3tr_c_xl_self_test_set(&dev_ctx, LSM6DS3TR_C_XL_ST_POSITIVE); /* Wait stable output */ platform_delay(WAIT_TIME_A); /* Check if new value available */ do { lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy); } while (!drdy); /* Read dummy data and discard it */ lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw); /* Read 5 sample and get the average vale for each axis */ memset(val_st_on, 0x00, 3 * sizeof(float)); for (i = 0; i < 5; i++) { /* Check if new value available */ do { lsm6ds3tr_c_xl_flag_data_ready_get(&dev_ctx, &drdy); } while (!drdy); /* Read data and accumulate the mg value */ lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw); for (j = 0; j < 3; j++) { val_st_on[j] += lsm6ds3tr_c_from_fs4g_to_mg(data_raw[j]); } } /* Calculate the mg average values */ for (i = 0; i < 3; i++) { val_st_on[i] /= 5.0f; } /* Calculate the mg values for self test */ for (i = 0; i < 3; i++) { test_val[i] = fabs((val_st_on[i] - val_st_off[i])); } /* Check self test limit */ st_result = ST_PASS; for (i = 0; i < 3; i++) { if (( MIN_ST_LIMIT_mg > test_val[i] ) || ( test_val[i] > MAX_ST_LIMIT_mg)) { st_result = ST_FAIL; } } /* Disable Self Test */ lsm6ds3tr_c_xl_self_test_set(&dev_ctx, LSM6DS3TR_C_XL_ST_DISABLE); /* Disable sensor. */ lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, LSM6DS3TR_C_XL_ODR_OFF); /* * Gyroscope Self Test */ /* Set Output Data Rate */ lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_208Hz); /* Set full scale */ lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, LSM6DS3TR_C_2000dps); /* Wait stable output */ platform_delay(WAIT_TIME_G_01); /* Check if new value available */ do { lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, &drdy); } while (!drdy); /* Read dummy data and discard it */ lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw); /* Read 5 sample and get the average vale for each axis */ memset(val_st_off, 0x00, 3 * sizeof(float)); for (i = 0; i < 5; i++) { /* Check if new value available */ do { lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, &drdy); } while (!drdy); /* Read data and accumulate the mg value */ lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw); for (j = 0; j < 3; j++) { val_st_off[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps( data_raw[j]); } } /* Calculate the mg average values */ for (i = 0; i < 3; i++) { val_st_off[i] /= 5.0f; } /* Enable Self Test positive (or negative) */ lsm6ds3tr_c_gy_self_test_set(&dev_ctx, LSM6DS3TR_C_GY_ST_POSITIVE); //lsm6ds3tr_c_gy_self_test_set(&dev_ctx, LIS2DH12_GY_ST_NEGATIVE); /* Wait stable output */ platform_delay(WAIT_TIME_G_02); /* Read 5 sample and get the average vale for each axis */ memset(val_st_on, 0x00, 3 * sizeof(float)); for (i = 0; i < 5; i++) { /* Check if new value available */ do { lsm6ds3tr_c_gy_flag_data_ready_get(&dev_ctx, &drdy); } while (!drdy); /* Read data and accumulate the mg value */ lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw); for (j = 0; j < 3; j++) { val_st_on[j] += lsm6ds3tr_c_from_fs2000dps_to_mdps( data_raw[j]); } } /* Calculate the mg average values */ for (i = 0; i < 3; i++) { val_st_on[i] /= 5.0f; } /* Calculate the mg values for self test */ for (i = 0; i < 3; i++) { test_val[i] = fabs((val_st_on[i] - val_st_off[i])); } /* Check self test limit */ for (i = 0; i < 3; i++) { if (( MIN_ST_LIMIT_mdps > test_val[i] ) || ( test_val[i] > MAX_ST_LIMIT_mdps)) { st_result = ST_FAIL; } } /* Disable Self Test */ lsm6ds3tr_c_gy_self_test_set(&dev_ctx, LSM6DS3TR_C_GY_ST_DISABLE); /* Disable sensor. */ lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, LSM6DS3TR_C_GY_ODR_OFF); if (st_result == ST_PASS) { SEGGER_RTT_printf(0,"Self Test - PASS\r\n"); } else { SEGGER_RTT_printf(0,"Self Test - FAIL\r\n"); } } void lsm6ds3tr_c_fifo_mode(void) { uint16_t max_odr = 0, min_odr = 0xffff; /* 6ds3 Accelerometer test parameters */ test_6ds3_xl.enable = PROPERTY_ENABLE; test_6ds3_xl.odr = LSM6DS3TR_C_XL_ODR_104Hz; test_6ds3_xl.odr_hz_val = 0; test_6ds3_xl.fs = LSM6DS3TR_C_16g; test_6ds3_xl.decimation = 0; test_6ds3_xl.samples_num_in_pattern = 0; // /* 6ds3 Gyroscope test parameters */ // test_6ds3_gyro.enable = PROPERTY_ENABLE; // test_6ds3_gyro.odr = LSM6DS3TR_C_GY_ODR_104Hz; // test_6ds3_gyro.odr_hz_val = 0; // test_6ds3_gyro.fs = LSM6DS3TR_C_2000dps; // test_6ds3_gyro.decimation = 0; // test_6ds3_gyro.samples_num_in_pattern = 0; /* Interrupt generation on FIFO watermark INT1/INT2 pin */ //lsm6ds3_int2_route_t int_2_reg; //lsm6ds3_int1_route_t int_1_reg; lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL); /* Set XL and Gyro Output Data Rate */ lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, test_6ds3_xl.odr); // lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, test_6ds3_gyro.odr); /* Set XL full scale and Gyro full scale */ lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, test_6ds3_xl.fs); // lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, test_6ds3_gyro.fs); lsm6ds3tr_c_gy_sleep_mode_set(&dev_ctx, 1); /* Calculate number of sensors samples in each FIFO pattern */ pattern_len = LSM6DS3TR_C_Calculate_FIFO_Pattern(&min_odr, &max_odr); /* Set FIFO watermark to a multiple of a pattern */ lsm6ds3tr_c_fifo_watermark_set(&dev_ctx, pattern_len); /* Set FIFO mode to Stream mode */ lsm6ds3tr_c_fifo_mode_set(&dev_ctx, LSM6DS3TR_C_STREAM_MODE); /* Enable FIFO watermark interrupt generation on INT1 pin */ //lsm6ds3_pin_int1_route_get(&dev_ctx, &int_1_reg); //int_1_reg.int1_fth = PROPERTY_ENABLE; //lsm6ds3_pin_int1_route_set(&dev_ctx, &int_1_reg); /* FIFO watermark interrupt on INT2 pin */ //lsm6ds3_pin_int2_route_get(&dev_ctx, &int_2_reg); //int_2_reg.int2_fth = PROPERTY_ENABLE; //lsm6ds3_pin_int2_route_set(&dev_ctx, &int_2_reg); /* Set ODR FIFO */ lsm6ds3tr_c_fifo_data_rate_set(&dev_ctx, LSM6DS3TR_C_FIFO_416Hz); /* Set FIFO sensor decimator */ lsm6ds3tr_c_fifo_xl_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_xl_t)test_6ds3_xl.decimation); // lsm6ds3tr_c_fifo_gy_batch_set(&dev_ctx, (lsm6ds3tr_c_dec_fifo_gyro_t)test_6ds3_gyro.decimation); } void lsm6ds3tr_c_fifo_read(axis3bit16_t **acc, uint16_t *acc_num, axis3bit16_t **gry, uint16_t *gry_num) { uint8_t wt; uint16_t num = 0; uint16_t num_pattern = 0; uint16_t acc_buf_num = 0, gry_buf_num = 0; static axis3bit16_t acc_buf[100], gry_buf[100]; /* Read FIFO watermark flag in polling mode */ lsm6ds3tr_c_fifo_wtm_flag_get(&dev_ctx, &wt); if (wt) { /* Read number of word in FIFO */ lsm6ds3tr_c_fifo_data_level_get(&dev_ctx, &num); num_pattern = num / pattern_len; while (num_pattern-- > 0) LSM6DS3TR_C_Read_FIFO_Pattern(acc_buf, &acc_buf_num, gry_buf, &gry_buf_num); } if(acc != NULL) *acc = acc_buf; if(acc_num != NULL) *acc_num = acc_buf_num; if(gry != NULL) *gry = gry_buf; if(gry_num != NULL) *gry_num = gry_buf_num; } void lsm6ds3tr_c_read_data_polling_mode(void) { /* 6ds3 Accelerometer test parameters */ test_6ds3_xl.enable = PROPERTY_ENABLE; test_6ds3_xl.odr = LSM6DS3TR_C_XL_ODR_104Hz; test_6ds3_xl.odr_hz_val = 0; test_6ds3_xl.fs = LSM6DS3TR_C_16g; test_6ds3_xl.decimation = 0; test_6ds3_xl.samples_num_in_pattern = 0; /* 6ds3 Gyroscope test parameters */ test_6ds3_gyro.enable = PROPERTY_ENABLE; test_6ds3_gyro.odr = LSM6DS3TR_C_GY_ODR_104Hz; test_6ds3_gyro.odr_hz_val = 0; test_6ds3_gyro.fs = LSM6DS3TR_C_2000dps; test_6ds3_gyro.decimation = 0; test_6ds3_gyro.samples_num_in_pattern = 0; /* Check device ID */ whoamI = 0; lsm6ds3tr_c_device_id_get(&dev_ctx, &whoamI); if(whoamI != LSM6DS3TR_C_ID) { SEGGER_RTT_printf(0,"lsm6ds3tr_c_read_data_polling_config error!!!\r\n"); return; } /* Restore default configuration */ lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE); do { lsm6ds3tr_c_reset_get(&dev_ctx, &rst); } while (rst); /* Enable Block Data Update */ lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); /* Set Output Data Rate */ lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, test_6ds3_xl.odr); lsm6ds3tr_c_gy_data_rate_set(&dev_ctx, test_6ds3_gyro.odr); /* Set full scale */ lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, test_6ds3_xl.fs); lsm6ds3tr_c_gy_full_scale_set(&dev_ctx, test_6ds3_gyro.fs); /* Configure filtering chain(No aux interface) */ /* Accelerometer - analog filter */ // lsm6ds3tr_c_xl_filter_analog_set(&dev_ctx, // LSM6DS3TR_C_XL_ANA_BW_400Hz); // /* Accelerometer - LPF1 path ( LPF2 not used )*/ // //lsm6ds3tr_c_xl_lp1_bandwidth_set(&dev_ctx, LSM6DS3TR_C_XL_LP1_ODR_DIV_4); // /* Accelerometer - LPF1 + LPF2 path */ // lsm6ds3tr_c_xl_lp2_bandwidth_set(&dev_ctx, // LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100); /* Accelerometer - High Pass / Slope path */ //lsm6ds3tr_c_xl_reference_mode_set(&dev_ctx, PROPERTY_DISABLE); //lsm6ds3tr_c_xl_hp_bandwidth_set(&dev_ctx, LSM6DS3TR_C_XL_HP_ODR_DIV_100); /* Gyroscope - filtering chain */ // lsm6ds3tr_c_gy_band_pass_set(&dev_ctx, // LSM6DS3TR_C_HP_260mHz_LP1_STRONG); } void lsm6ds3tr_c_read_data_polling(int16_t *acc, int16_t *gry, int16_t *temp) { /* Read samples in polling mode (no int) */ /* Read output only if new value is available */ lsm6ds3tr_c_reg_t reg; lsm6ds3tr_c_status_reg_get(&dev_ctx, ®.status_reg); if (reg.status_reg.xlda) { /* Read magnetic field data */ memset( data_raw_acceleration.i16bit, 0x00, 3 * sizeof(int16_t)); lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); // acceleration_mg[0] = lsm6ds3tr_c_from_fs2g_to_mg( // data_raw_acceleration[0]); // acceleration_mg[1] = lsm6ds3tr_c_from_fs2g_to_mg( // data_raw_acceleration[1]); // acceleration_mg[2] = lsm6ds3tr_c_from_fs2g_to_mg( // data_raw_acceleration[2]); // sprintf((char *)tx_buffer, // "Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n", // acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]); acc[0] = data_raw_acceleration.i16bit[0]; acc[1] = data_raw_acceleration.i16bit[1]; acc[2] = data_raw_acceleration.i16bit[2]; } if (reg.status_reg.gda) { /* Read magnetic field data */ memset(data_raw_angular_rate.i16bit, 0x00, 3 * sizeof(int16_t)); lsm6ds3tr_c_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.i16bit); // angular_rate_mdps[0] = lsm6ds3tr_c_from_fs2000dps_to_mdps( // data_raw_angular_rate[0]); // angular_rate_mdps[1] = lsm6ds3tr_c_from_fs2000dps_to_mdps( // data_raw_angular_rate[1]); // angular_rate_mdps[2] = lsm6ds3tr_c_from_fs2000dps_to_mdps( // data_raw_angular_rate[2]); // sprintf((char *)tx_buffer, // "Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n", // angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]); gry[0] = data_raw_angular_rate.i16bit[0]; gry[1] = data_raw_angular_rate.i16bit[1]; gry[2] = data_raw_angular_rate.i16bit[2]; } if (reg.status_reg.tda) { /* Read temperature data */ memset(&data_raw_temperature, 0x00, sizeof(int16_t)); lsm6ds3tr_c_temperature_raw_get(&dev_ctx, &data_raw_temperature); // temperature_degC = lsm6ds3tr_c_from_lsb_to_celsius( // data_raw_temperature ); // sprintf((char *)tx_buffer, "Temperature [degC]:%6.2f\r\n", // temperature_degC ); *temp = data_raw_temperature; } } void lsm6ds3tr_c_low_power_acc_mode(void) { /* 6ds3 Accelerometer test parameters */ test_6ds3_xl.enable = PROPERTY_ENABLE; test_6ds3_xl.odr = LSM6DS3TR_C_XL_ODR_12Hz5; test_6ds3_xl.odr_hz_val = 0; test_6ds3_xl.fs = LSM6DS3TR_C_16g; test_6ds3_xl.decimation = 0; test_6ds3_xl.samples_num_in_pattern = 0; /* Restore default configuration */ lsm6ds3tr_c_reset_set(&dev_ctx, PROPERTY_ENABLE); do { lsm6ds3tr_c_reset_get(&dev_ctx, &rst); } while (rst); /* Enable Block Data Update */ lsm6ds3tr_c_block_data_update_set(&dev_ctx, PROPERTY_ENABLE); lsm6ds3tr_c_xl_power_mode_set(&dev_ctx,LSM6DS3TR_C_XL_NORMAL); lsm6ds3tr_c_xl_data_rate_set(&dev_ctx, test_6ds3_xl.odr); lsm6ds3tr_c_xl_full_scale_set(&dev_ctx, test_6ds3_xl.fs); lsm6ds3tr_c_xl_filter_analog_set(&dev_ctx, LSM6DS3TR_C_XL_ANA_BW_400Hz); lsm6ds3tr_c_xl_lp2_bandwidth_set(&dev_ctx, LSM6DS3TR_C_XL_LOW_NOISE_LP_ODR_DIV_100); lsm6ds3tr_c_gy_sleep_mode_set(&dev_ctx, 1); } void lsm6ds3tr_c_low_power_acc(int16_t *acc) { /* Read samples in polling mode (no int) */ /* Read output only if new value is available */ lsm6ds3tr_c_reg_t reg; lsm6ds3tr_c_status_reg_get(&dev_ctx, ®.status_reg); if (reg.status_reg.xlda) { /* Read magnetic field data */ memset( data_raw_acceleration.i16bit, 0x00, 3 * sizeof(int16_t)); lsm6ds3tr_c_acceleration_raw_get(&dev_ctx, data_raw_acceleration.i16bit); // acceleration_mg[0] = lsm6ds3tr_c_from_fs2g_to_mg( // data_raw_acceleration[0]); // acceleration_mg[1] = lsm6ds3tr_c_from_fs2g_to_mg( // data_raw_acceleration[1]); // acceleration_mg[2] = lsm6ds3tr_c_from_fs2g_to_mg( // data_raw_acceleration[2]); // sprintf((char *)tx_buffer, // "Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n", // acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]); acc[0] = data_raw_acceleration.i16bit[0]; acc[1] = data_raw_acceleration.i16bit[1]; acc[2] = data_raw_acceleration.i16bit[2]; } } void lsm6ds3tr_c_get_accel_power_mode_stat(uint8_t *stat) { uint8_t reg_val; int32_t ret; ret = lsm6ds3tr_c_read_reg(&dev_ctx, LSM6DS3TR_C_FIFO_CTRL5, ®_val, 1); if(ret == 0 && (reg_val & 0x78)!=0) { *stat = 1; //fifo mode return; } ret = lsm6ds3tr_c_gy_sleep_mode_get(&dev_ctx, ®_val); if(ret == 0 && (1 == reg_val)) { *stat = 2; //low power acc mode return; } *stat = 3; //acc + gy } // //test // uint16_t i; // uint8_t stat = 0; // int16_t acc[3], gry[3], temp; // axis3bit16_t *acc_fifo, *gry_fifo; // uint16_t acc_num, gry_num; // lsm6ds3tr_c_init(); //// lsm6ds3tr_c_read_data_polling_mode(); //// lsm6ds3tr_c_low_power_acc_mode(); //// lsm6ds3tr_c_fifo_mode(); // while(1) // { //// lsm6ds3tr_c_read_data_polling(acc, gry, &temp); //// SEGGER_RTT_printf(0,"h_ax=%d\r,h_ay=%d\r,h_az=%d\r\n",acc[0],acc[1],acc[2]); //// SEGGER_RTT_printf(0,"h_gx=%d\r,h_gy=%d\r,h_gz=%d\r\n",gry[0],gry[1],gry[2]); //// SEGGER_RTT_printf(0,"temp:%d\r\n",temp); //// nrf_delay_ms(10); // // //// lsm6ds3tr_c_read_data_polling_mode(); //// lsm6ds3tr_c_get_accel_power_mode_stat(&stat); //// SEGGER_RTT_printf(0,"lsm6ds3tr_c_read_data_polling_mode stat:%d\r\n",stat); //// nrf_delay_ms(1000); //// //// lsm6ds3tr_c_low_power_acc_mode(); //// lsm6ds3tr_c_get_accel_power_mode_stat(&stat); //// SEGGER_RTT_printf(0,"lsm6ds3tr_c_low_power_acc_mode stat:%d\r\n",stat); //// nrf_delay_ms(1000); //// //// lsm6ds3tr_c_fifo_mode(); //// lsm6ds3tr_c_get_accel_power_mode_stat(&stat); //// SEGGER_RTT_printf(0,"lsm6ds3tr_c_fifo_mode stat:%d\r\n",stat); //// nrf_delay_ms(1000); // //// lsm6ds3tr_c_low_power_acc(acc); //// SEGGER_RTT_printf(0,"h_ax=%d\r,h_ay=%d\r,h_az=%d\r\n",acc[0],acc[1],acc[2]); //// nrf_delay_ms(10); // //// lsm6ds3tr_c_fifo_read(&acc_fifo, &acc_num, &gry_fifo, &gry_num); //// for(i=0;i