feat: continuous sensor sampling (10Hz attitude) + LoRa on-demand power + I2C driver hardening
- LSM6DSL: ODR 12.5->52Hz high-performance, gyro FS 2000->500dps, FIFO stream mode, watermark 4 sets (24 words) interrupt driven, 4-sample average output 13Hz, 1s no-data watchdog auto recovery - MMC5983: enable 20Hz continuous measurement mode (was dead code), remove per-read SW_RST, STATUS polling + INT dual path, write-1-clear Meas_M_Done to release INT latch, 2-sample average output 10Hz, timeout auto recovery - I2C driver: add wait timeout and state machine retry limit to read/write, NACK fast-return FALSE, eliminate bus-stuck infinite loop - main.c: continuous acquire loop (no IDLE dead-end on first failure), keep UpdateOk valid to fix up/coll command race, TimingAcquisition compatible with continuous mode, add LoraPowerHandler (LoRa RX during comm, sleep otherwise, saves ~10mA), remove 10Hz hot-path debug prints - Attitude update ~10Hz (IMU 13Hz / mag 10Hz), gateway upload cadence unchanged
This commit is contained in:
+115
-135
@@ -1,4 +1,4 @@
|
||||
|
||||
|
||||
/* Includes ------------------------------------------------------------------*/
|
||||
#include "lsm6dsl_app.h"
|
||||
#include "lsm6dsl_reg.h"
|
||||
@@ -13,31 +13,24 @@
|
||||
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 ACC_COLL_CNT_MAX 4 //每组平均样本数,输出速率=ODR/4=13Hz
|
||||
#define ACC_FIFO_WATERMASK (ACC_COLL_CNT_MAX * 6) //FIFO水印:4组 × (3陀螺+3加速度)=24
|
||||
#define BOOT_TIME 15 //ms
|
||||
|
||||
typedef enum {
|
||||
Acc_Idle,
|
||||
Acc_Start,
|
||||
Acc_Get_Data,
|
||||
Acc_Wait_GetData,
|
||||
Acc_Go_Bypass,
|
||||
Acc_Get_Data, //连续采集运行中
|
||||
}AccStatusType_m;
|
||||
|
||||
static AccStatusType_m acc_status;
|
||||
static uint32_t acc_cvttime;
|
||||
static bool Lsm6dslWaterMarkFullIntFlag;
|
||||
static bool Lsm6dslWakeupIntFlag;
|
||||
static uint16_t Lsm6dslIntWatchdog; //无数据看门狗计数(1ms)
|
||||
static bool Lsm6dslRecoverReq; //自恢复请求(1ms置位,主循环执行)
|
||||
|
||||
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;
|
||||
|
||||
|
||||
@@ -60,6 +53,57 @@ static void platform_read(uint8_t reg, uint8_t *bufp, uint32_t len)
|
||||
I2C_MasterReadData(M0P_I2C0, Selve_Add, reg, bufp, len);
|
||||
}
|
||||
|
||||
/*****************************************************************************************
|
||||
* 函数名称: Lsm6dsl_Config
|
||||
* 功能描述: LSM6DSL寄存器配置,配置完成后开启连续采集(常开,不进入低功耗)
|
||||
* 参 数: 无
|
||||
* 返 回 值: 无
|
||||
*****************************************************************************************/
|
||||
static void Lsm6dsl_Config(void)
|
||||
{
|
||||
/* 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_HIGH_PERFORMANCE);
|
||||
lsm6dsl_gy_power_mode_set(&dev_ctx, LSM6DSL_GY_HIGH_PERFORMANCE);
|
||||
/* Enable Block Data Update */
|
||||
lsm6dsl_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);
|
||||
/* Set full scale */
|
||||
lsm6dsl_xl_full_scale_set(&dev_ctx, LSM6DSL_2g);
|
||||
lsm6dsl_gy_full_scale_set(&dev_ctx, LSM6DSL_500dps);
|
||||
/* Configure filtering chain(No aux interface) */
|
||||
/* Accelerometer - analog filter */
|
||||
lsm6dsl_xl_filter_analog_set(&dev_ctx, LSM6DSL_XL_ANA_BW_400Hz);
|
||||
/* Accelerometer - LPF1 + LPF2 path */
|
||||
lsm6dsl_xl_lp2_bandwidth_set(&dev_ctx, LSM6DSL_XL_LOW_NOISE_LP_ODR_DIV_9);
|
||||
/* 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_52Hz); //FIFO ODR选择,与传感器ODR一致
|
||||
lsm6dsl_fifo_watermark_set(&dev_ctx, ACC_FIFO_WATERMASK); //FIFO水印电平设置,4组数据
|
||||
lsm6dsl_fifo_stop_on_wtm_set(&dev_ctx, 0); //存满水印后继续采集,不停
|
||||
lsm6dsl_block_data_update_set(&dev_ctx, 1); //fifo模式ctrl3_c bdu必须为1
|
||||
lsm6dsl_auto_increment_set(&dev_ctx, 1); //fifo模式ctrl3_c if_inc必须为1
|
||||
|
||||
/* 中断1:FIFO水印阈值中断 */
|
||||
lsm6dsl_int1_route_t int1val ={0, 0, 0};
|
||||
int1val.int1_fth = 1; //FIFO存储达到水印深度时触发
|
||||
lsm6dsl_pin_int1_route_set(&dev_ctx, int1val);
|
||||
|
||||
/* 最后开启ODR和FIFO连续模式,保证先配置后采样 */
|
||||
lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_STREAM_MODE);
|
||||
lsm6dsl_xl_data_rate_set(&dev_ctx, LSM6DSL_XL_ODR_52Hz);
|
||||
lsm6dsl_gy_data_rate_set(&dev_ctx, LSM6DSL_GY_ODR_52Hz);
|
||||
}
|
||||
|
||||
static void Lsm6dsl_Init(void)
|
||||
{
|
||||
/* Initialize mems driver interface */
|
||||
@@ -72,7 +116,7 @@ static void Lsm6dsl_Init(void)
|
||||
whoamI = 0;
|
||||
lsm6dsl_device_id_get(&dev_ctx, &whoamI);
|
||||
acc_cvttime = 100;
|
||||
while(whoamI != LSM6DSL_ID){
|
||||
while(whoamI != LSM6DSL_ID){
|
||||
if(acc_cvttime <= 0){
|
||||
DBG_LOG("LSM6DS INIT Err...\r\n"); /*manage here device not found */
|
||||
InitFlag = false;
|
||||
@@ -80,104 +124,49 @@ static void Lsm6dsl_Init(void)
|
||||
}
|
||||
}
|
||||
|
||||
/* 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);
|
||||
Lsm6dsl_Config();
|
||||
|
||||
if(InitFlag == false){
|
||||
DBG_LOG("LSM6DS INIT OK...\r\n");
|
||||
DBG_LOG("LSM6DS INIT OK...\r\n");
|
||||
InitFlag = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
extern void Lsm6dDataProcessCallBack(float *Acc, float *Gy);
|
||||
static void Acc_IntHandler(void)
|
||||
{
|
||||
uint16_t DiffFifo;
|
||||
uint16_t Sets;
|
||||
uint8_t Cnt;
|
||||
uint8_t Slot;
|
||||
uint16_t i;
|
||||
AccGyData_t AccGy;
|
||||
AccData_t Acc;
|
||||
float VectorG;
|
||||
float AverageAcc[XYZ], AverageGy[XYZ];
|
||||
|
||||
|
||||
lsm6dsl_fifo_data_level_get(&dev_ctx, &DiffFifo);
|
||||
if(DiffFifo == 0) {
|
||||
if(DiffFifo < ACC_FIFO_WATERMASK) {
|
||||
return;
|
||||
}
|
||||
|
||||
if(DiffFifo > ACC_FIFO_WATERMASK)
|
||||
DiffFifo = ACC_FIFO_WATERMASK;
|
||||
for(int i = 0; i < DiffFifo / 6; i++) {
|
||||
|
||||
Sets = DiffFifo / 6;
|
||||
for(i = 0; i < Sets; 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);
|
||||
Slot = i % ACC_COLL_CNT_MAX; //环形缓存,始终保留最新ACC_COLL_CNT_MAX组
|
||||
angular_rate_mdps[X][Slot] = lsm6dsl_from_fs500dps_to_mdps(AccGy.GyX);
|
||||
angular_rate_mdps[Y][Slot] = lsm6dsl_from_fs500dps_to_mdps(AccGy.GyY);
|
||||
angular_rate_mdps[Z][Slot] = lsm6dsl_from_fs500dps_to_mdps(AccGy.GyZ);
|
||||
acceleration_mg[X][Slot] = lsm6dsl_from_fs2g_to_mg(AccGy.AccX);
|
||||
acceleration_mg[Y][Slot] = lsm6dsl_from_fs2g_to_mg(AccGy.AccY);
|
||||
acceleration_mg[Z][Slot] = 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);
|
||||
Cnt = (Sets >= ACC_COLL_CNT_MAX) ? ACC_COLL_CNT_MAX : (uint8_t)Sets;
|
||||
AverageGy[X] = IntFilter_Float(angular_rate_mdps[0], Cnt, 0);
|
||||
AverageGy[Y] = IntFilter_Float(angular_rate_mdps[1], Cnt, 0);
|
||||
AverageGy[Z] = IntFilter_Float(angular_rate_mdps[2], Cnt, 0);
|
||||
AverageAcc[X] = IntFilter_Float(acceleration_mg[0], Cnt, 0);
|
||||
AverageAcc[Y] = IntFilter_Float(acceleration_mg[1], Cnt, 0);
|
||||
AverageAcc[Z] = IntFilter_Float(acceleration_mg[2], Cnt, 0);
|
||||
Lsm6dDataProcessCallBack(AverageAcc, AverageGy);
|
||||
|
||||
acc_status = Acc_Go_Bypass;
|
||||
}
|
||||
|
||||
static void ReadWakeUpIntStatus(void)
|
||||
@@ -192,43 +181,19 @@ static void ReadWakeUpIntStatus(void)
|
||||
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;
|
||||
if(acc_status != Acc_Get_Data) {
|
||||
return;
|
||||
}
|
||||
if(Lsm6dslRecoverReq) {
|
||||
Lsm6dslRecoverReq = false;
|
||||
Lsm6dsl_Config(); //1秒无水印中断,重新配置FIFO/ODR自恢复
|
||||
Lsm6dslIntWatchdog = 0;
|
||||
return;
|
||||
}
|
||||
if(Lsm6dslWaterMarkFullIntFlag) {
|
||||
Lsm6dslWaterMarkFullIntFlag = false;
|
||||
Lsm6dslIntWatchdog = 0;
|
||||
Acc_IntHandler();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -236,6 +201,17 @@ void Lsm6dsl1mSRoutine(void)
|
||||
{
|
||||
if(acc_cvttime > 0)
|
||||
acc_cvttime--;
|
||||
if(acc_status == Acc_Get_Data) {
|
||||
if(Lsm6dslIntWatchdog < 2000)
|
||||
Lsm6dslIntWatchdog++;
|
||||
if(Lsm6dslIntWatchdog >= 1000) { //连续1秒无FIFO水印中断
|
||||
Lsm6dslIntWatchdog = 0;
|
||||
Lsm6dslRecoverReq = true;
|
||||
}
|
||||
}
|
||||
else {
|
||||
Lsm6dslIntWatchdog = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Lsm6dslInit(void)
|
||||
@@ -245,15 +221,19 @@ void Lsm6dslInit(void)
|
||||
|
||||
void Lsm6dsStart(void)
|
||||
{
|
||||
Lsm6dsl_Init();
|
||||
acc_status = Acc_Start;
|
||||
if(acc_status == Acc_Get_Data) {
|
||||
return; //已在连续采集,不重复配置
|
||||
}
|
||||
Lsm6dsl_Init(); //配置寄存器并开启连续采集
|
||||
acc_status = Acc_Get_Data;
|
||||
}
|
||||
|
||||
void Lsm6dsStop(void)
|
||||
{
|
||||
lsm6dsl_fifo_mode_set(&dev_ctx, LSM6DSL_BYPASS_MODE);
|
||||
lsm6dsl_fifo_data_rate_set(&dev_ctx, LSM6DSL_FIFO_DISABLE);
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user