270 lines
7.8 KiB
C
270 lines
7.8 KiB
C
|
|
/* Includes ------------------------------------------------------------------*/
|
|
#include "lsm6dsl_app.h"
|
|
#include "lsm6dsl_reg.h"
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include "bsp.h"
|
|
#include "UartDebug.h"
|
|
#include "main.h"
|
|
#include "i2c.h"
|
|
#include "Algorithm.h"
|
|
|
|
dev_ctx_t dev_ctx;
|
|
static bool InitFlag;
|
|
|
|
#define ACC_CVTTIME (20 * 1000)
|
|
#define ACC_COLL_CNT_MAX 6
|
|
#define ACC_FIFO_WATERMASK (ACC_COLL_CNT_MAX * 6) //采集一秒数据
|
|
#define TX_BUF_DIM 1000
|
|
#define BOOT_TIME 15 //ms
|
|
|
|
typedef enum {
|
|
Acc_Idle,
|
|
Acc_Start,
|
|
Acc_Get_Data,
|
|
Acc_Wait_GetData,
|
|
Acc_Go_Bypass,
|
|
}AccStatusType_m;
|
|
|
|
static AccStatusType_m acc_status;
|
|
static uint32_t acc_cvttime;
|
|
static bool Lsm6dslWaterMarkFullIntFlag;
|
|
static bool Lsm6dslWakeupIntFlag;
|
|
|
|
static int16_t data_raw_acceleration[3];
|
|
static int16_t data_raw_angular_rate[3];
|
|
static int16_t data_raw_temperature;
|
|
static float acceleration_mg[3][ACC_COLL_CNT_MAX];
|
|
static float angular_rate_mdps[3][ACC_COLL_CNT_MAX];
|
|
static float temperature_degC;
|
|
static uint8_t whoamI, rst;
|
|
|
|
|
|
static void platform_delay(uint32_t ms)
|
|
{
|
|
delay_ms(ms);
|
|
}
|
|
|
|
|
|
static void platform_write(uint8_t reg, uint8_t *bufp, uint32_t len)
|
|
{
|
|
uint16_t Selve_Add = LSM6DSL_I2C_ADD_L >> 1;
|
|
I2C_MasterWriteData(M0P_I2C0, Selve_Add, reg, bufp, len);
|
|
}
|
|
|
|
|
|
static void platform_read(uint8_t reg, uint8_t *bufp, uint32_t len)
|
|
{
|
|
uint16_t Selve_Add = LSM6DSL_I2C_ADD_L >> 1;
|
|
I2C_MasterReadData(M0P_I2C0, Selve_Add, reg, bufp, len);
|
|
}
|
|
|
|
static void Lsm6dsl_Init(void)
|
|
{
|
|
/* Initialize mems driver interface */
|
|
dev_ctx.write_reg = platform_write;
|
|
dev_ctx.read_reg = platform_read;
|
|
dev_ctx.mdelay = platform_delay;
|
|
/* Wait sensor boot time */
|
|
platform_delay(BOOT_TIME);
|
|
/* Check device ID */
|
|
whoamI = 0;
|
|
lsm6dsl_device_id_get(&dev_ctx, &whoamI);
|
|
acc_cvttime = 100;
|
|
while(whoamI != LSM6DSL_ID){
|
|
if(acc_cvttime <= 0){
|
|
DBG_LOG("LSM6DS INIT Err...\r\n"); /*manage here device not found */
|
|
InitFlag = false;
|
|
return;
|
|
}
|
|
}
|
|
|
|
/* Restore default configuration */
|
|
lsm6dsl_reset_set(&dev_ctx, PROPERTY_ENABLE);
|
|
|
|
do {
|
|
lsm6dsl_reset_get(&dev_ctx, &rst);
|
|
} while (rst);
|
|
|
|
lsm6dsl_xl_power_mode_set(&dev_ctx, LSM6DSL_XL_NORMAL);
|
|
/* Enable Block Data Update */
|
|
lsm6dsl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
|
|
/* Set Output Data Rate */
|
|
lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_12Hz5);
|
|
lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_12Hz5);
|
|
//lsm6dsl_gy_data_rate_set(&left_dev_ctx, LSM6DSL_GY_ODR_OFF); //关闭地磁
|
|
/* Set full scale */
|
|
lsm6dsl_xl_full_scale_set(&dev_ctx, LSM6DSL_2g);
|
|
lsm6dsl_gy_full_scale_set(&dev_ctx, LSM6DSL_2000dps);
|
|
/* Configure filtering chain(No aux interface) */
|
|
/* Accelerometer - analog filter */
|
|
lsm6dsl_xl_filter_analog_set(&dev_ctx, LSM6DSL_XL_ANA_BW_400Hz);
|
|
/* Accelerometer - LPF1 path ( LPF2 not used )*/
|
|
/*lsm6dsl_xl_lp1_bandwidth_set(&left_dev_ctx, LSM6DSL_XL_LP1_ODR_DIV_4);*/
|
|
/* Accelerometer - LPF1 + LPF2 path */
|
|
lsm6dsl_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSL_XL_LOW_NOISE_LP_ODR_DIV_9);
|
|
/* Accelerometer - High Pass / Slope path */
|
|
/*lsm6dsl_xl_reference_mode_set(&left_dev_ctx, PROPERTY_DISABLE);*/
|
|
/*lsm6dsl_xl_hp_bandwidth_set(&left_dev_ctx, LSM6DSL_XL_HP_ODR_DIV_100);*/
|
|
/* Gyroscope - filtering chain */
|
|
lsm6dsl_gy_band_pass_set(&dev_ctx, LSM6DSL_HP_260mHz_LP1_STRONG);
|
|
|
|
//fifo模式设置
|
|
lsm6dsl_fifo_xl_batch_set(&dev_ctx, LSM6DSL_FIFO_XL_NO_DEC);//fifo加速度写入频率设置
|
|
lsm6dsl_fifo_gy_batch_set(&dev_ctx, LSM6DSL_FIFO_GY_NO_DEC);//fifo陀螺仪写入频率设置
|
|
lsm6dsl_fifo_data_rate_set(&dev_ctx, LSM6DSL_FIFO_12Hz5);//FIFO ODR选择
|
|
|
|
//中断1设置，watermark full中断
|
|
lsm6dsl_int1_route_t int1val ={0, 0, 0};
|
|
int1val.int1_full_flag = 1; //开阀值中断，fifo存储到缓存深度时触发
|
|
lsm6dsl_pin_int1_route_set(&dev_ctx, int1val);
|
|
lsm6dsl_fifo_watermark_set(&dev_ctx, ACC_FIFO_WATERMASK);//FIFO水印电平设置，阈值设置，缓存深度
|
|
lsm6dsl_fifo_stop_on_wtm_set(&dev_ctx, 1);//fifo缓存深度存满时，不再存储
|
|
//lsm6dsl_fifo_xl_gy_8bit_format_set(&left_dev_ctx, 1);//在FIFO中为XL和陀螺启用MSByte记忆，缓存宽度
|
|
lsm6dsl_block_data_update_set(&dev_ctx, 1); //fifo模式ctrl3_c bdu必须为1,直到读取MSB和LSB后才更新输出寄存器
|
|
lsm6dsl_auto_increment_set(&dev_ctx, 1); //fifo模式ctrl3_c if_inc必须为1,通过串行接口(I2C或SPI)进行多字节访问时自动增加寄存器地址。
|
|
lsm6dsl_ctrl3_c_t ctrl3_c;
|
|
platform_read(LSM6DSL_CTRL3_C, (uint8_t *)&ctrl3_c, 1);
|
|
//lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_FIFO_MODE);
|
|
|
|
//中断2设置，唤醒中断
|
|
// uint8_t temp = 0x90; //唤醒中断
|
|
// platform_write(LSM6DSL_TAP_CFG, &temp, 1);
|
|
// temp = 0;
|
|
// platform_write(LSM6DSL_WAKE_UP_DUR, &temp, 1);
|
|
// lsm6dsl_wkup_threshold_set(&left_dev_ctx, 0x20);
|
|
// lsm6dsl_int2_route_t int2val ={0, 0, 0};
|
|
// int2val.int2_wu = 1;
|
|
// lsm6dsl_pin_int2_route_set(&left_dev_ctx, int2val);
|
|
|
|
if(InitFlag == false){
|
|
DBG_LOG("LSM6DS INIT OK...\r\n");
|
|
InitFlag = true;
|
|
}
|
|
}
|
|
|
|
extern void Lsm6dDataProcessCallBack(float *Acc, float *Gy);
|
|
static void Acc_IntHandler(void)
|
|
{
|
|
uint16_t DiffFifo;
|
|
AccGyData_t AccGy;
|
|
AccData_t Acc;
|
|
float VectorG;
|
|
float AverageAcc[XYZ], AverageGy[XYZ];
|
|
|
|
lsm6dsl_fifo_data_level_get(&dev_ctx, &DiffFifo);
|
|
if(DiffFifo == 0) {
|
|
return;
|
|
}
|
|
|
|
if(DiffFifo > ACC_FIFO_WATERMASK)
|
|
DiffFifo = ACC_FIFO_WATERMASK;
|
|
for(int i = 0; i < DiffFifo / 6; i++) {
|
|
lsm6dsl_fifo_raw_data_get(&dev_ctx, (uint8_t *)&AccGy, sizeof(AccGy));
|
|
angular_rate_mdps[X][i] = lsm6dsl_from_fs2000dps_to_mdps(AccGy.GyX);
|
|
angular_rate_mdps[Y][i] = lsm6dsl_from_fs2000dps_to_mdps(AccGy.GyY);
|
|
angular_rate_mdps[Z][i] = lsm6dsl_from_fs2000dps_to_mdps(AccGy.GyZ);
|
|
acceleration_mg[X][i] = lsm6dsl_from_fs2g_to_mg(AccGy.AccX);
|
|
acceleration_mg[Y][i] = lsm6dsl_from_fs2g_to_mg(AccGy.AccY);
|
|
acceleration_mg[Z][i] = lsm6dsl_from_fs2g_to_mg(AccGy.AccZ);
|
|
}
|
|
AverageGy[X] = IntFilter_Float(angular_rate_mdps[0], DiffFifo / 6, 2);
|
|
AverageGy[Y] = IntFilter_Float(angular_rate_mdps[1], DiffFifo / 6, 2);
|
|
AverageGy[Z] = IntFilter_Float(angular_rate_mdps[2], DiffFifo / 6, 2);
|
|
AverageAcc[X] = IntFilter_Float(acceleration_mg[0], DiffFifo / 6, 2);
|
|
AverageAcc[Y] = IntFilter_Float(acceleration_mg[1], DiffFifo / 6, 2);
|
|
AverageAcc[Z] = IntFilter_Float(acceleration_mg[2], DiffFifo / 6, 2);
|
|
Lsm6dDataProcessCallBack(AverageAcc, AverageGy);
|
|
|
|
acc_status = Acc_Go_Bypass;
|
|
}
|
|
|
|
static void ReadWakeUpIntStatus(void)
|
|
{
|
|
lsm6dsl_wake_up_src_t wake_up_src;
|
|
if(Lsm6dslWakeupIntFlag) {
|
|
Lsm6dslWakeupIntFlag = false;
|
|
platform_read(LSM6DSL_WAKE_UP_SRC, (uint8_t *)&wake_up_src, sizeof(lsm6dsl_wake_up_src_t));
|
|
}
|
|
}
|
|
|
|
void Lsm6dslLoopHandler(void)
|
|
{
|
|
ReadWakeUpIntStatus();
|
|
switch(acc_status)
|
|
{
|
|
case Acc_Idle:
|
|
break;
|
|
|
|
case Acc_Start:
|
|
if(acc_cvttime > 0)
|
|
break;
|
|
lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_FIFO_MODE);
|
|
acc_status = Acc_Get_Data;
|
|
acc_cvttime = ACC_CVTTIME;
|
|
break;
|
|
|
|
case Acc_Get_Data:
|
|
acc_cvttime = 1000;
|
|
acc_status = Acc_Wait_GetData;
|
|
break;
|
|
|
|
case Acc_Wait_GetData:
|
|
if(acc_cvttime <= 0){
|
|
acc_status = Acc_Go_Bypass;
|
|
}
|
|
else if(Lsm6dslWaterMarkFullIntFlag) {
|
|
Lsm6dslWaterMarkFullIntFlag = false;
|
|
uint8_t wtm_flag;
|
|
lsm6dsl_fifo_wtm_flag_get(&dev_ctx, &wtm_flag);
|
|
Acc_IntHandler();
|
|
acc_status = Acc_Go_Bypass;
|
|
}
|
|
break;
|
|
|
|
case Acc_Go_Bypass:
|
|
lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_BYPASS_MODE);
|
|
Lsm6dsStop();
|
|
acc_status = Acc_Idle;
|
|
acc_cvttime = 1;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void Lsm6dsl1mSRoutine(void)
|
|
{
|
|
if(acc_cvttime > 0)
|
|
acc_cvttime--;
|
|
}
|
|
|
|
void Lsm6dslInit(void)
|
|
{
|
|
Lsm6dsl_Init();
|
|
}
|
|
|
|
void Lsm6dsStart(void)
|
|
{
|
|
Lsm6dsl_Init();
|
|
acc_status = Acc_Start;
|
|
}
|
|
|
|
void Lsm6dsStop(void)
|
|
{
|
|
lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_OFF);
|
|
lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_OFF);
|
|
lsm6dsl_fifo_data_rate_set(&dev_ctx, LSM6DSL_FIFO_DISABLE);
|
|
acc_status = Acc_Idle;
|
|
}
|
|
|
|
void Lsm6dsInt1CallBack(void)
|
|
{
|
|
Lsm6dslWaterMarkFullIntFlag = true;
|
|
}
|
|
|
|
void Lsm6dsInt2CallBack(void)
|
|
{
|
|
Lsm6dslWakeupIntFlag = true;
|
|
//Tilt.Acc_IRQ_Status = true;
|
|
}
|