/*/* ****************************************************************************** * @file _self_test.c * @author Sensors Software Solution Team * @brief This file implements the self test procedure. * ****************************************************************************** * @attention * *

© Copyright (c) 2020 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under BSD 3-Clause license, * the "License"; You may not use this file except in compliance with the * License. You may obtain a copy of the License at: * opensource.org/licenses/BSD-3-Clause * ****************************************************************************** */ /* Includes ------------------------------------------------------------------*/ #include #include #include "twi_middleware.h" #include "lsm6dsox_i2c_helper.h" #include "lsm6dsox_activity.h" #include "finallogic_1704_3.h" #if defined(nRF52840) #include "nrf_drv_twi.h" #include "nrf_delay.h" #include "app_error.h" #include "nrf_log.h" #include "nrf_log_ctrl.h" #include "nrf_log_default_backends.h" #include "lsm6dsox_reg.h" #include "lsm6dsox_self_test.h" #include "FreeRTOS.h" #include "task.h" #include "settings.h" #endif /* * WARNING: * Functions declare in this section are defined at the end of this file * and are strictly related to the hardware platform used. * */ int32_t lsm_initialize(stmdev_ctx_t* dev_ctx) { int32_t ret; uint8_t temp = 0 ; uint16_t temp1 = 0; /* Initialize mems driver interface */ dev_ctx->write_reg = platform_write; dev_ctx->read_reg = platform_read; dev_ctx->handle = &m_twi_0; uint8_t whoamI = 0x00; uint8_t rst; lsm6dsox_xl_hm_mode_t vala; lsm6dsox_g_hm_mode_t valg; platform_delay(BOOT_TIME); /* Check device ID */ ret = lsm6dsox_device_id_get(dev_ctx, &whoamI); if (whoamI != LSM6DSOX_ID) { NRF_LOG_INFO("LSM6DSOX: I2C whoamI resistor issue" ); LSM6DSOX_error = 1; return ret; } /* Restore default configuration */ ret = lsm6dsox_reset_set(dev_ctx, PROPERTY_ENABLE); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_reset_set" ); LSM6DSOX_error = 2; } do { lsm6dsox_reset_get(dev_ctx, &rst); } while (rst); /* Disable I3C interface */ ret = lsm6dsox_i3c_disable_set(dev_ctx, LSM6DSOX_I3C_DISABLE); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_i3c_disable_set" ); LSM6DSOX_error = 3; } /* Enable Block Data Update */ ret = lsm6dsox_block_data_update_set(dev_ctx, PROPERTY_ENABLE); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_block_data_update_set" ); LSM6DSOX_error = 4; } ret = lsm6dsox_mlc_init_set(dev_ctx, PROPERTY_ENABLE); lsm6dsox_mlc_odr_t mlc_odr = LSM6DSOX_ODR_PRGS_26Hz; ret = lsm6dsox_mlc_data_rate_set(dev_ctx,mlc_odr); for (int8_t i = 0; i < (sizeof(finallogic_1704_3) / sizeof(ucf_line_t) ); i++ ) { lsm6dsox_write_reg(dev_ctx, finallogic_1704_3[i].address, (uint8_t *)&finallogic_1704_3[i].data, 1); } /* Disable Self Test */ // lsm6dsox_gy_self_test_set(dev_ctx, LSM6DSOX_GY_ST_DISABLE); /* Disable sensor. */ //power mode set ret = lsm6dsox_gy_power_mode_set(dev_ctx, LSM6DSOX_GY_NORMAL); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_gy_power_mode_set" ); LSM6DSOX_error = 5; } //lsm6dsox_gy_power_mode_get(dev_ctx, &valg); //NRF_LOG_INFO(" lsm6dsox_gy_power_mode_get %x",valg); ret = lsm6dsox_gy_data_rate_set(dev_ctx, LSM6DSOX_GY_ODR_OFF); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_gy_data_rate_set" ); LSM6DSOX_error = 6; } ret = lsm6dsox_xl_data_rate_set(dev_ctx, LSM6DSOX_XL_ODR_OFF); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_xl_data_rate_set" ); LSM6DSOX_error = 7; } ret = lsm6dsox_xl_power_mode_set(dev_ctx,LSM6DSOX_ULTRA_LOW_POWER_MD); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_xl_power_mode_set" ); LSM6DSOX_error = 8; } ret = lsm6dsox_xl_power_mode_get(dev_ctx,&vala); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_xl_power_mode_get" ); LSM6DSOX_error = 9; } NRF_LOG_INFO(" lsm6dsox_xl_power_mode_get %x",vala); ret = lsm6dsox_gy_power_mode_get(dev_ctx, &valg); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_gy_power_mode_get" ); LSM6DSOX_error = 10; } NRF_LOG_INFO(" lsm6dsox_gy_power_mode_get %x",valg); // /* disable HW Timestamp */ ret = lsm6dsox_timestamp_set(dev_ctx, PROPERTY_DISABLE); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_timestamp_set\n" ); LSM6DSOX_error = 11; } NRF_LOG_INFO("lsm6dsox_timestamp DISABLED"); ret = lsm6dsox_pedo_steps_period_set(dev_ctx,0x00); if(ret < 0 ){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_pedo_steps_period_set" ); LSM6DSOX_error = 12; } NRF_LOG_INFO("steps_period DISABLED to 0"); //ret = lsm6dsox_timestamp_get(dev_ctx, &temp); //if(ret < 0){ // NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_timestamp_get" ); // LSM6DSOX_error = 10; //} // NRF_LOG_INFO(" lsm6dsox_timestamp_get %x",temp); //ret = lsm6dsox_pedo_steps_period_get(dev_ctx,&temp1); //if(ret < 0 ){ // NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_pedo_steps_period_get" ); // LSM6DSOX_error = 25; //} // NRF_LOG_INFO(" lsm6dsox_pedo_steps_period_get %x",temp1); NRF_LOG_INFO("lsm6dsox init Successfully!!!\n" ); //for FIFO DATA ENABLE ///* // * Set FIFO watermark (number of unread sensor data TAG + 6 bytes // * stored in FIFO) to 10 samples // */ // lsm6dsox_fifo_watermark_set(dev_ctx, 10); // /* Set FIFO batch XL/Gyro ODR to 12.5Hz */ // lsm6dsox_fifo_xl_batch_set(dev_ctx, LSM6DSOX_XL_BATCHED_AT_12Hz5); // // lsm6dsox_fifo_gy_batch_set(dev_ctx, LSM6DSOX_GY_BATCHED_AT_12Hz5); // /* Set FIFO mode to Stream mode (aka Continuous Mode) */ // lsm6dsox_fifo_mode_set(dev_ctx, LSM6DSOX_STREAM_MODE); // /* Enable drdy 75 μs pulse: uncomment if interrupt must be pulsed */ // //lsm6dsox_data_ready_mode_set(dev_ctx, LSM6DSOX_DRDY_PULSED); // /* Uncomment if interrupt generation on Free Fall INT1 pin */ // //lsm6dsox_pin_int1_route_get(dev_ctx, &int1_route); // //int1_route.reg.int1_ctrl.int1_fifo_th = PROPERTY_ENABLE; // //lsm6dsox_pin_int1_route_set(dev_ctx, &int1_route); // /* Uncomment if interrupt generation on Free Fall INT2 pin */ // //lsm6dsox_pin_int2_route_get(dev_ctx, &int2_route); // //int2_route.reg.int2_ctrl.int2_fifo_th = PROPERTY_ENABLE; // //lsm6dsox_pin_int2_route_set(dev_ctx, &int2_route); // /* Set Output Data Rate */ // lsm6dsox_xl_data_rate_set(dev_ctx, LSM6DSOX_XL_ODR_12Hz5); // //lsm6dsox_gy_data_rate_set(dev_ctx, LSM6DSOX_GY_ODR_12Hz5); return ret; } /* Main Example --------------------------------------------------------------*/ int32_t lsm6dsox_activity(stmdev_ctx_t* dev_ctx) { int32_t ret = 0 ; lsm6dsox_pin_int1_route_t int1_route; lsm6dsox_pin_int2_route_t int2_route; //lsm6dsox_all_sources_t all_source; lsm6dsox_emb_sens_t emb_sens; uint8_t debounce = DEBOUNCE_INIT; uint16_t period = 50; //50// it is represented as a 16-bit unsigned value with a resolution of 6.4 ms //ret = lsm6dsox_gy_data_rate_set(dev_ctx, LSM6DSOX_GY_ODR_OFF); //if(ret < 0){ // NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_gy_data_rate_set" ); // LSM6DSOX_error = 12; //} //pedometer settings------------ /* Save current embedded features */ /* Turn off embedded features */ if (lsm6dsox_embedded_sens_off(dev_ctx) != 0) { //return 1; NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_embedded_sens_off\n" ); LSM6DSOX_error = 17; } // /* Wait for 10 ms */ vTaskDelay(100); if (lsm6dsox_embedded_sens_get(dev_ctx, &emb_sens) != 0) { //return 1; error NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_embedded_sens_get\n" ); LSM6DSOX_error = 18; } /* Enable pedometer algorithm. */ emb_sens.step = PROPERTY_ENABLE; emb_sens.mlc = PROPERTY_ENABLE; //emb_sens.tilt = PROPERTY_ENABLE; //emb_sens.sig_mot = PROPERTY_ENABLE; //emb_sens.sig_mot = PROPERTY_DISABLE; /* Turn on embedded features */ if (lsm6dsox_embedded_sens_set(dev_ctx, &emb_sens) != 0) { //return 1; NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_embedded_sens_set\n" ); LSM6DSOX_error = 19; } if (lsm6dsox_pedo_sens_set(dev_ctx, LSM6DSOX_FALSE_STEP_REJ_ADV_MODE) != 0) //LSM6DSOX_FALSE_STEP_REJ_ADV_MODE //LSM6DSOX_PEDO_BASE_MODE { // return 1; NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_pedo_sens_set\n" ); LSM6DSOX_error = 20; } vTaskDelay(100); //for above to set // /* Enable step detector on INT1 pin */ if (lsm6dsox_pin_int1_route_get(dev_ctx, &int1_route) != 0) { //return 1; NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_pin_int1_route_get\n" ); LSM6DSOX_error = 21; } int1_route.step_detector = PROPERTY_ENABLE; //PROPERTY_DISABLE; int1_route.wake_up = PROPERTY_ENABLE; int1_route.mlc1 = PROPERTY_ENABLE; //int1_route.tilt = PROPERTY_ENABLE; //int1_route.sig_mot = PROPERTY_ENABLE; if (lsm6dsox_pin_int1_route_set(dev_ctx, int1_route) != 0) { // return 1; NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_pin_int1_route_set\n" ); LSM6DSOX_error = 22; } /* Configure interrupt pin mode notification */ ret = lsm6dsox_int_notification_set(dev_ctx, LSM6DSOX_ALL_INT_PULSED);//LSM6DSOX_ALL_INT_PULSED//LSM6DSOX_ALL_INT_LATCHED //LSM6DSOX_BASE_PULSED_EMB_LATCHED if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_gy_power_mode_get" ); LSM6DSOX_error = 23; } ret = lsm6dsox_pedo_debounce_steps_set(dev_ctx,&debounce); if(ret < 0 ){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_pedo_debounce_steps_set" ); LSM6DSOX_error = 24; } /* Enable HW Timestamp */ ret = lsm6dsox_timestamp_set(dev_ctx, PROPERTY_ENABLE); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_timestamp_set\n" ); LSM6DSOX_error = 25; } //ret = lsm6dsox_pedo_steps_period_set(dev_ctx,period); //if(ret < 0 ){ // NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_pedo_steps_period_set" ); // LSM6DSOX_error = 26; //} /* Set XL and Gyro Output Data Rate */ ret = lsm6dsox_xl_data_rate_set(dev_ctx, LSM6DSOX_XL_ODR_26Hz); //LSM6DSOX_XL_ODR_208Hz //LSM6DSOX_XL_ODR_26Hz if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_xl_data_rate_set" ); LSM6DSOX_error = 28; } /* Set 2g full XL scale and 250 dps full Gyro */ ret = lsm6dsox_xl_full_scale_set(dev_ctx, LSM6DSOX_4g); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_xl_full_scale_set" ); LSM6DSOX_error = 29; } lsm6dsox_fifo_xl_batch_set(dev_ctx, LSM6DSOX_XL_BATCHED_AT_26Hz); lsm6dsox_fifo_mode_set(dev_ctx,LSM6DSOX_BYPASS_MODE); // flush any previous value in FIFO before start lsm6dsox_fifo_mode_set(dev_ctx,LSM6DSOX_STREAM_MODE); // start batching in continous mode // Set FIFO watermark level. Can be used to check when the number of samples in buffer reaches this defined threshold level. // lsm6dsox_fifo_watermark_set(dev_ctx, 2); ret = lsm6dsox_steps_reset(dev_ctx); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_gy_power_mode_get" ); LSM6DSOX_error = 27; } NRF_LOG_INFO("lsm6dsox Pedometer settings done--\n" ); vTaskDelay(10); //------------------wake up activity settings /* Set duration for Activity detection to 9.62 ms (= 2 * 1 / ODR_XL) */ //lsm6dsox_wkup_dur_set(dev_ctx, 0x08); ret = lsm6dsox_wkup_dur_set(dev_ctx, 0x01); //0x01 - soft //0x02 -h if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_wkup_dur_set" ); LSM6DSOX_error = 13; } /* Set duration for Inactivity detection to 4.92 s (= 2 * 512 / ODR_XL) */ ret = lsm6dsox_act_sleep_dur_set(dev_ctx, 0x04); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_act_sleep_dur_set" ); LSM6DSOX_error = 14; } /* Set Activity/Inactivity threshold to 62.5 mg */ //lsm6dsox_wkup_threshold_set(dev_ctx, 0x0f); ret = lsm6dsox_wkup_threshold_set(dev_ctx, 0x01); //0x03 if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_wkup_threshold_set" ); LSM6DSOX_error = 15; } /* Inactivity configuration: XL to 12.5 in LP, gyro to Power-Down */ ret = lsm6dsox_act_mode_set(dev_ctx, LSM6DSOX_XL_12Hz5_GY_NOT_AFFECTED);//LSM6DSOX_XL_12Hz5_GY_NOT_AFFECTED //LSM6DSOX_XL_12Hz5_GY_PD if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error lsm6dsox_act_mode_set" ); LSM6DSOX_error = 16; } vTaskDelay(100); //for above to set NRF_LOG_INFO("lsm6dsox Wakeup settings done--\n" ); return ret; } //uint16_t get_steps(){ // uint16_t steps = 0; // uint8_t ret; // lsm6dsox_number_of_steps_get(dev_ctx, &steps); // NRF_LOG_INFO("in steps %d", steps); // vTaskDelay(20); // ret = lsm6dsox_steps_reset(dev_ctx); // NRF_LOG_INFO("reset ret %d", ret); // return steps; //} //void get_all_sources(lsm6dsox_all_sources_t * all_sources){ // lsm6dsox_all_sources_get(dev_ctx, all_sources); //} void lsm6dsox_fifo_step(stmdev_ctx_t* dev_ctx) { lsm6dsox_emb_sens_t emb_sens; /* Uncomment to configure INT 1 */ lsm6dsox_pin_int1_route_t int1_route; /* Uncomment to configure INT 2 */ lsm6dsox_pin_int2_route_t int2_route; /* Set FIFO mode to Stream mode (aka Continuous Mode) */ lsm6dsox_fifo_mode_set(dev_ctx, LSM6DSOX_STREAM_MODE); /* Enable latched interrupt notification. */ lsm6dsox_int_notification_set(dev_ctx, LSM6DSOX_ALL_INT_LATCHED); /* Enable drdy 75 μs pulse: uncomment if interrupt must be pulsed. */ lsm6dsox_data_ready_mode_set(dev_ctx, LSM6DSOX_DRDY_PULSED); /* * FIFO watermark interrupt routed on INT1 pin * * Remember that INT1 pin is used by sensor to switch in I3C mode * Uncomment to configure INT 1 */ lsm6dsox_pin_int1_route_get(dev_ctx, &int1_route); int1_route.step_detector = PROPERTY_ENABLE; lsm6dsox_pin_int1_route_set(dev_ctx, int1_route); /* * FIFO watermark interrupt routed on INT2 pin * Uncomment to configure INT 2 */ //lsm6dsox_pin_int2_route_get(dev_ctx,NULL, &int2_route); //int2_route.step_detector = PROPERTY_ENABLE; //lsm6dsox_pin_int2_route_set(dev_ctx,NULL, int2_route); /* Enable HW Timestamp */ lsm6dsox_timestamp_set(dev_ctx, PROPERTY_ENABLE); /* Enable pedometer */ lsm6dsox_pedo_sens_set(dev_ctx, LSM6DSOX_PEDO_BASE_MODE); emb_sens.step = PROPERTY_ENABLE; lsm6dsox_embedded_sens_set(dev_ctx, &emb_sens); lsm6dsox_fifo_pedo_batch_set(dev_ctx, PROPERTY_ENABLE); lsm6dsox_steps_reset(dev_ctx); /* Set Output Data Rate */ lsm6dsox_xl_data_rate_set(dev_ctx, LSM6DSOX_XL_ODR_26Hz); //while (1) { // uint16_t num = 0; // lsm6dsox_fifo_tag_t reg_tag; // pedo_count_sample_t pedo_sample; // /* Read FIFO samples number */ // lsm6dsox_fifo_data_level_get(dev_ctx, &num); // if (num > 0) { // while (num--) { // /* Read FIFO tag */ // lsm6dsox_fifo_sensor_tag_get(dev_ctx, ®_tag); // switch (reg_tag) { // case LSM6DSOX_STEP_CPUNTER_TAG: // lsm6dsox_fifo_out_raw_get(dev_ctx, pedo_sample.byte); // /*sprintf((char *)tx_buffer, "Step Count :%u T %u\r\n", // (unsigned int)pedo_sample.step_count, // (unsigned int)pedo_sample.timestamp); // tx_com(tx_buffer, strlen((char const *)tx_buffer));*/ // NRF_LOG_INFO("Step Count :%u T %u", // (unsigned int)pedo_sample.step_count, // (unsigned int)pedo_sample.timestamp) // break; // default: // break; // } // } // } //} } void lsm6dsox_wake_up_en(stmdev_ctx_t* dev_ctx,uint8_t enable){ lsm6dsox_tap_cfg2_t reg; int32_t ret; ret = lsm6dsox_read_reg(dev_ctx, LSM6DSOX_TAP_CFG2, (uint8_t *)®, 1); if (ret == 0) { if(enable){ reg.interrupts_enable = 1; } else{ reg.interrupts_enable = 0; } ret = lsm6dsox_write_reg(dev_ctx, LSM6DSOX_TAP_CFG2, (uint8_t *)®, 1); if(ret < 0){ NRF_LOG_INFO("LSM6DSOX: Error LSM6DSOX_TAP_CFG2" ); } } } //turning off lsm