/*/*
******************************************************************************
* @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