LSM6DSL 6D位置检测 STM32L4R5

littleshrimp

LSM6DSL 6D位置检测 STM32L4R5 [复制链接]

使用STMems_Standard_C_drivers库
程序参考了STM32CubeExpansion MEMS-XT1的LSM6DSL_6DOrientation
在STMems_Standard_C_drivers库中没找到操读取LSM6DSL_D6D_SRC（0x1D）寄存器的函数
自己添加了下边这个

int32_t lsm6dsl_d6d_orientation_get(lsm6dsl_ctx_t *ctx,
lsm6dsl_d6d_orientation_t *val)
{
lsm6dsl_reg_t reg;
int32_t ret;
ret = lsm6dsl_read_reg(ctx, LSM6DSL_D6D_SRC,
®.byte, 1);
if(ret == 0){
val->xl = reg.d6d_src.xl;
val->xh = reg.d6d_src.xh;
val->yl = reg.d6d_src.yl;
val->yh = reg.d6d_src.yh;
val->zl = reg.d6d_src.zl;
val->zh = reg.d6d_src.zh;
}
return ret;
}

复制代码

配置好Output Data Rate、full scale后通过lsm6dsl_pin_int1_route_set(&dev_ctx,lsm6dsl_int1_route);函数使能6D中断
设置阈值为60度lsm6dsl_6d_threshold_set(&dev_ctx, LSM6DSL_DEG_60);
循环检测INT1中断，有中断时读取LSM6DSL_D6D_SRC寄存器lsm6dsl_d6d_orientation_get(&dev_ctx,&lsm6dsl_d6d_orientation);
再将位置通过串口输出

/**
* @brief Send actual 6D orientation to UART
* @param None
* @retval None
*/
static void lsm6dsl_6d_send_orientation(void)
{
lsm6dsl_d6d_orientation_t lsm6dsl_d6d_orientation;
lsm6dsl_d6d_orientation_get(&dev_ctx,&lsm6dsl_d6d_orientation);
if (lsm6dsl_d6d_orientation.xl == 0 && lsm6dsl_d6d_orientation.yl == 0 && lsm6dsl_d6d_orientation.zl == 0 && lsm6dsl_d6d_orientation.xh == 0 && lsm6dsl_d6d_orientation.yh == 1 && lsm6dsl_d6d_orientation.zh == 0)
{
snprintf(data_out, MAX_BUF_SIZE, "\r\n ________________ " \
"\r\n | | " \
"\r\n | * | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n |________________| \r\n");
}
else if (lsm6dsl_d6d_orientation.xl == 1 && lsm6dsl_d6d_orientation.yl == 0 && lsm6dsl_d6d_orientation.zl == 0 && lsm6dsl_d6d_orientation.xh == 0 && lsm6dsl_d6d_orientation.yh == 0 && lsm6dsl_d6d_orientation.zh == 0)
{
snprintf(data_out, MAX_BUF_SIZE, "\r\n ________________ " \
"\r\n | | " \
"\r\n | * | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n |________________| \r\n");
}
else if (lsm6dsl_d6d_orientation.xl == 0 && lsm6dsl_d6d_orientation.yl == 0 && lsm6dsl_d6d_orientation.zl == 0 && lsm6dsl_d6d_orientation.xh == 1 && lsm6dsl_d6d_orientation.yh == 0 && lsm6dsl_d6d_orientation.zh == 0)
{
snprintf(data_out, MAX_BUF_SIZE, "\r\n ________________ " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | * | " \
"\r\n |________________| \r\n");
}
else if (lsm6dsl_d6d_orientation.xl == 0 && lsm6dsl_d6d_orientation.yl == 1 && lsm6dsl_d6d_orientation.zl == 0 && lsm6dsl_d6d_orientation.xh == 0 && lsm6dsl_d6d_orientation.yh == 0 && lsm6dsl_d6d_orientation.zh == 0)
{
snprintf(data_out, MAX_BUF_SIZE, "\r\n ________________ " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | | " \
"\r\n | * | " \
"\r\n |________________| \r\n");
}
else if (lsm6dsl_d6d_orientation.xl == 0 && lsm6dsl_d6d_orientation.yl == 0 && lsm6dsl_d6d_orientation.zl == 0 && lsm6dsl_d6d_orientation.xh == 0 && lsm6dsl_d6d_orientation.yh == 0 && lsm6dsl_d6d_orientation.zh == 1)
{
snprintf(data_out, MAX_BUF_SIZE, "\r\n __*_____________ " \
"\r\n |________________| \r\n");
}
else if (lsm6dsl_d6d_orientation.xl == 0 && lsm6dsl_d6d_orientation.yl == 0 && lsm6dsl_d6d_orientation.zl == 1 && lsm6dsl_d6d_orientation.xh == 0 && lsm6dsl_d6d_orientation.yh == 0 && lsm6dsl_d6d_orientation.zh == 0)
{
snprintf(data_out, MAX_BUF_SIZE, "\r\n ________________ " \
"\r\n |________________| " \
"\r\n * \r\n");
}
else
{
snprintf(data_out, MAX_BUF_SIZE, "None of the 6D orientation axes is set in LSM6DSL - accelerometer.\r\n");
}
tx_com((uint8_t*)data_out, strlen(data_out));
}
void lsm6dsl_6d_orientation(void)
{
/*
* Initialize mems driver interface
*/
dev_ctx.write_reg = platform_write;
dev_ctx.read_reg = platform_read;
dev_ctx.handle = &hi2c1;
/*
* Check device ID
*/
whoamI = 0;
lsm6dsl_device_id_get(&dev_ctx, &whoamI);
if ( whoamI != LSM6DSL_ID )
while(1); /*manage here device not found */
/*
* Restore default configuration
*/
lsm6dsl_reset_set(&dev_ctx, PROPERTY_ENABLE);
do {
lsm6dsl_reset_get(&dev_ctx, &rst);
} while (rst);
/*
* 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_416Hz);
/*
* Set full scale
*/
lsm6dsl_xl_full_scale_set(&dev_ctx, LSM6DSL_2g);
/*
* 中断
*/
lsm6dsl_int1_route_t lsm6dsl_int1_route;
lsm6dsl_int1_route.int1_6d = 1;
lsm6dsl_pin_int1_route_set(&dev_ctx,lsm6dsl_int1_route);
/*
* 6d
*/
lsm6dsl_6d_threshold_set(&dev_ctx, LSM6DSL_DEG_60);
while(1)
{
if(int1_flag == 1)
{
int1_flag = 0;
lsm6dsl_6d_send_orientation();
}
}
}

复制代码

工程文件：

stm32l4r5 6d_orientation.rar (16 MB, 下载次数: 35)