YuanHongbin/LaserTracing
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: continuous sensor sampling (10Hz attitude) + LoRa on-demand power + I2C driver hardening

Browse Source 
- 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:
YuanHongbin
2026-09-03 16:15:09 +08:00
parent 706eb1247e
commit 93e00b8b0a
5 changed files with 387 additions and 358 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Module/lsm6dsl/lsm6dsl_app.c
+115 -135
View File
@@ -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;
}
Module/mmc5983/mmc5983.c
+115 -126
View File
@@ -1,14 +1,21 @@
#include "main.h"
#include "main.h"
#include "mmc5983.h"
#include "UartDebug.h"
#include "i2c.h"
#include "Algorithm.h"
static int32_t max_in[XYZ];
static int32_t min_in[XYZ];
#define MAG_AVG_CNT 2 //每2个样本平均输出1次,输出速率=20Hz/2=10Hz
#define MAG_POLL_MS 20 //测量完成标志轮询周期,与20Hz测量周期一致
#define MAG_POLL_FAIL_MAX 5 //连续5次(100ms)轮询无数据则重写配置自恢复
static bool InitFlag;
static bool MMC5983IntFlag;
static MMC5983Var_t MMC5983Var;
static int32_t mag_array[XYZ][MAG_AVG_CNT];
static uint8_t magcnt;
static uint8_t PollFailCnt;
extern void MmcDataProcessCallBack(float *Mag);
/*****************************************************************************************
* 函数名称: MMC5983_Write_Reg
@@ -50,19 +57,24 @@ static bool MMC5983_Read_Buffer(uint8_t reg, void *buffer, uint8_t len)
}
/*****************************************************************************************
* 函数名称: MMC5983StartCov
* 功能描述: MMC5983开启连续转换
* 函数名称: MMC5983StartContinuous
* 功能描述: MMC5983开启20Hz连续转换(常开,不进入低功耗)
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
static void MMC5983StartCov(void)
static void MMC5983StartContinuous(void)
{
MMC5983RegCtrl0 Ctrl0Reg;
MMC5983RegCtrl2 Ctrl2Reg; //开启/关闭连续转换
Ctrl0Reg.Byte = 0x00;
Ctrl0Reg.Bit.Auto_SR_en = 1; //自动SET/RESET,消除温漂零场漂移
Ctrl0Reg.Bit.NT_meas_done_en = 1; //测量完成中断使能(Stop会清CTRL0,启动时必须重写)
MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
Ctrl2Reg.Byte = 0x00;
Ctrl2Reg.Bit.Cm_freq = 0x04;
Ctrl2Reg.Bit.Cm_freq = 0x03; //011=20Hz连续测量
Ctrl2Reg.Bit.Cmm_en = 1; //连续设1，单次设0
Ctrl2Reg.Bit.Prd_set = 3;
Ctrl2Reg.Bit.En_prd_set = 1; //连续设1，单次设0
Ctrl2Reg.Bit.Prd_set = 3; //每100次测量自动SET一次,消除温漂
Ctrl2Reg.Bit.En_prd_set = 1;
MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
}
@@ -75,54 +87,49 @@ static void MMC5983StartCov(void)
static void MMC5983_Init(void)
{
uint8_t id;
id = MMC5983_Read_Reg(MMC5983_ID);
//DBG_LOG("MMC5983 id=0x%x\r\n",id);
MMC5983Var.MMC5983Delay1mSCnt = 100;
while(id != 0x30){
if(MMC5983Var.MMC5983Delay1mSCnt <= 0) {
DBG_LOG("LEFT MMC5983 INIT Err...\r\n");
DBG_LOG("MMC5983 INIT Err...\r\n");
InitFlag = false;
return;
}
}
MMC5983RegCtrl1 Ctrl1Reg;
Ctrl1Reg.Byte = 0x00;
Ctrl1Reg.Bit.SW_RST = 1;
MMC5983_Write_Reg(MMC5983_CTRL_1, Ctrl1Reg.Byte);
MMC5983Delay(15);
MMC5983RegCtrl2 Ctrl2Reg; //开启/关闭连续转换
MMC5983RegCtrl2 Ctrl2Reg; //连续模式在StartContinuous中开启
Ctrl2Reg.Byte = 0x00;
Ctrl2Reg.Bit.Cm_freq = 0x04;
Ctrl2Reg.Bit.Cmm_en = 0; //连续设1，单次设0
Ctrl2Reg.Bit.Prd_set = 3;
Ctrl2Reg.Bit.En_prd_set = 0; //连续设1，单次设0
MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
MMC5983RegCtrl3 Ctrl3Reg;
Ctrl3Reg.Byte = 0x00;
Ctrl3Reg.Bit.St_enm = 1;
Ctrl3Reg.Bit.St_enp = 1;
MMC5983_Write_Reg(MMC5983_CTRL_3, Ctrl3Reg.Byte);
MMC5983RegCtrl0 Ctrl0Reg;
Ctrl0Reg.Byte = 0x00;
Ctrl0Reg.Bit.TM_M = 1;
Ctrl0Reg.Bit.Auto_SR_en = 1;
Ctrl0Reg.Bit.NT_meas_done_en = 1;
Ctrl0Reg.Bit.Auto_SR_en = 1; //自动SET/RESET,消除温漂零场漂移
Ctrl0Reg.Bit.NT_meas_done_en = 1; //测量完成中断使能
MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
if(InitFlag == false){
DBG_LOG("MMC5983 INIT OK...\r\n");
DBG_LOG("MMC5983 INIT OK...\r\n");
InitFlag = true;
}
}
/*****************************************************************************************
* 函数名称: MMC5983_Read
* 功能描述: MMC5983读取原始数据
* 功能描述: MMC5983读取原始数据(18bit)
* 参 数: MagAdcX X轴数据
MagAdcY Y轴数据
MagAdcZ Z轴数据
@@ -131,7 +138,7 @@ static void MMC5983_Init(void)
static bool MMC5983_Read(int32_t *MagAdcX, int32_t *MagAdcY, int32_t *MagAdcZ)
{
static uint8_t buff[7];
if (MMC5983_Read_Buffer(MMC5983_X_OUT_H, buff, 7) == false)
return false;
@@ -151,125 +158,114 @@ void MMC5983_Get_Mag(float *mag)
{
if(mag == NULL)
return;
float ADVal[XYZ];
ADVal[X] = MMC5983Var.Mag_Original[X] - 131072.0f;
ADVal[Y] = MMC5983Var.Mag_Original[Y] - 131072.0f;
ADVal[Z] = MMC5983Var.Mag_Original[Z] - 131072.0f;
ADVal[X] = MMC5983Var.Mag_Original[X] - 131072.0f;
ADVal[Y] = MMC5983Var.Mag_Original[Y] - 131072.0f;
ADVal[Z] = MMC5983Var.Mag_Original[Z] - 131072.0f;
mag[0] = (float) (ADVal[X] * MMC5983_MAG_SCALE_18BIT);
mag[1] = (float) (ADVal[Y] * MMC5983_MAG_SCALE_18BIT);
mag[2] = (float) (ADVal[Z] * MMC5983_MAG_SCALE_18BIT);
}
/*****************************************************************************************
* 函数名称: MMC5983LoopHandler
* 功能描述: MMC5983循环处理函数
* 函数名称: MMC5983MeasDoneProcess
* 功能描述: 单次测量完成处理:读取样本、清除中断锁存,凑够MAG_AVG_CNT个样本后平均输出
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
static void MMC5983MeasDoneProcess(void)
{
MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS;
if(MMC5983_Read(&mag_array[X][magcnt], &mag_array[Y][magcnt], &mag_array[Z][magcnt])){
magcnt++;
}
MMC5983_Write_Reg(MMC5983_STATUS, 0x01); //写1清Meas_M_Done,释放INT锁存,下次测量产生新上升沿
if(magcnt >= MAG_AVG_CNT){
MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
magcnt = 0;
MMC5983_Get_Mag(MMC5983Var.Mag);
MmcDataProcessCallBack(MMC5983Var.Mag);
}
}
/*****************************************************************************************
* 函数名称: MMC5983LoopHandler
* 功能描述: MMC5983循环处理函数,20Hz连续采样,2样本平均后10Hz输出
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
extern void MmcDataProcessCallBack(float *Mag);
void MMC5983LoopHandler(void)
{
static int32_t mag_array[XYZ][10];
static uint8_t magcnt;
static uint8_t TimeOutMagcnt;
switch(MMC5983Var.Status) {
case MMC5983_STATUS_IDLE:{
break;}
case MMC5983_STATUS_START:{
for(int i = 0; i < 10; i++) {
mag_array[X][i] = 0;
mag_array[Y][i] = 0;
mag_array[Z][i] = 0;
}
magcnt = 0;
TimeOutMagcnt = 0;
memset(&MMC5983Var, NULL, sizeof(MMC5983Var));
MMC5983Var.Status = MMC5983_STATUS_DELAY;
break;}
case MMC5983_STATUS_DELAY:{
MMC5983StartRead();
MMC5983Var.MMC5983Delay1mSCnt = 50;
MMC5983Var.Status = MMC5983_STATUS_WAIT_DELAY;
break;}
case MMC5983_STATUS_WAIT_DELAY:{
if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
MMC5983Var.Status = MMC5983_STATUS_READ;
if(!InitFlag){
MMC5983_Init();
if(!InitFlag){
MMC5983Var.Status = MMC5983_STATUS_IDLE;
break;
}
}
break;}
case MMC5983_STATUS_READ:{
MMC5983Var.MMC5983Delay1mSCnt = 50;
magcnt = 0;
PollFailCnt = 0;
MMC5983StartContinuous(); //开启20Hz连续测量
MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS;
MMC5983Var.Status = MMC5983_STATUS_WAIT_READ;
break;}
case MMC5983_STATUS_WAIT_READ:{
if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
MMC5983Var.Status = MMC5983_STATUS_DELAY;
TimeOutMagcnt++;
}
case MMC5983_STATUS_WAIT_READ:{
if(MMC5983IntFlag == true){
MMC5983IntFlag = false;
MMC5983_Read(&mag_array[X][magcnt], &mag_array[Y][magcnt], &mag_array[Z][magcnt]);
magcnt++;
MMC5983Var.Status = MMC5983_STATUS_DELAY;
PollFailCnt = 0;
MMC5983MeasDoneProcess(); //INT触发,立即读数
}
if(TimeOutMagcnt >= 10){
TimeOutMagcnt = 0;
if(magcnt > 0){
MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
magcnt = 0;
MMC5983_Get_Mag(MMC5983Var.Mag);
MmcDataProcessCallBack(MMC5983Var.Mag);
MMC5983Var.Status = MMC5983_STATUS_STOP;
else if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
uint8_t StatusReg;
MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS; //20ms轮询一次测量完成标志
StatusReg = MMC5983_Read_Reg(MMC5983_STATUS);
if((StatusReg & 0x01) != 0){ //Meas_M_Done=1,测量完成
PollFailCnt = 0;
MMC5983MeasDoneProcess();
}
else{
MMC5983Var.Status = MMC5983_STATUS_STOP;
// DBG_LOG("LEFT MMC5983 TimeOut...\r\n");
PollFailCnt++;
if(PollFailCnt >= MAG_POLL_FAIL_MAX){
PollFailCnt = 0;
MMC5983StartContinuous(); //连续无数据,重写配置自恢复
}
}
}
if(magcnt >= 10){
MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
magcnt = 0;
MMC5983_Get_Mag(MMC5983Var.Mag);
MmcDataProcessCallBack(MMC5983Var.Mag);
MMC5983Var.Status = MMC5983_STATUS_STOP;
}
break;}
case MMC5983_STATUS_STOP:{
MMC5983Stop();
MMC5983Var.Status = MMC5983_STATUS_IDLE;
//MMC5983Var.Status = MMC5983_STATUS_START;//持续工作
break;}
}
}
/*****************************************************************************************
* 函数名称: LeftMMC59831mSRoutine
* 函数名称: MMC59831mSRoutine
* 功能描述: MMC59831ms时基循环处理函数
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC59831mSRoutine(void)
{
if(MMC5983Var.MMC5983Delay1mSCnt > 0)
if(MMC5983Var.MMC5983Delay1mSCnt > 0)
MMC5983Var.MMC5983Delay1mSCnt--;
}
/*****************************************************************************************
* 函数名称: LeftMMC5983Init
* 函数名称: MMC5983Init
* 功能描述: MMC5983初始化
* 参 数: 无
* 返 回 值: 无
@@ -280,58 +276,52 @@ void MMC5983Init(void)
MMC5983_Init();
return;
}
MMC5983Var.Status = MMC5983_STATUS_START;
}
/*****************************************************************************************
* 函数名称: LeftMMC5983Start
* 功能描述: MMC5983启动
* 函数名称: MMC5983Start
* 功能描述: MMC5983启动(幂等,已运行则直接返回)
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC5983Start(void)
{
if(MMC5983Var.Status == MMC5983_STATUS_WAIT_READ){
return; //已在连续采样,不重复配置
}
if(!InitFlag){
MMC5983_Init();
return;
if(!InitFlag){
return;
}
}
MMC5983Var.Status = MMC5983_STATUS_START;
}
/*****************************************************************************************
* 函数名称: LeftMMC5983StartRead
* 功能描述: MMC5983启动读取
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC5983StartRead(void)
{
MMC5983_Init();//读取时重新初始化
}
/*****************************************************************************************
* 函数名称: MMC5983Stop
* 功能描述: MMC5983停止
* 功能描述: MMC5983停止(关闭连续转换)
* 参 数: 无
* 返 回 值: 无
*****************************************************************************************/
void MMC5983Stop(void)
{
{
MMC5983RegCtrl2 Ctrl2Reg;
Ctrl2Reg.Byte = 0x00;
MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte); //Cmm_en=0,关闭连续转换
MMC5983RegCtrl0 Ctrl0Reg;
Ctrl0Reg.Byte = 0x00;
MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
MMC5983RegStatus StatusReg;
StatusReg.Byte = MMC5983_Read_Reg(MMC5983_STATUS);
if(StatusReg.Bit.Meas_M_Done == 1){
MMC5983_Write_Reg(MMC5983_STATUS, StatusReg.Byte);
}
MMC5983Var.Status = MMC5983_STATUS_IDLE;
}
/*****************************************************************************************
* 函数名称: LeftMMC5983IntCallBack
* 函数名称: MMC5983IntCallBack
* 功能描述: MMC5983中断回调
* 参 数: 无
* 返 回 值: 无
@@ -340,4 +330,3 @@ void MMC5983IntCallBack(void)
{
MMC5983IntFlag = true;
}
Module/mmc5983/mmc5983.h
-4
View File
@@ -103,9 +103,6 @@ typedef union {
typedef enum{
MMC5983_STATUS_IDLE,
MMC5983_STATUS_START,
MMC5983_STATUS_DELAY,
MMC5983_STATUS_WAIT_DELAY,
MMC5983_STATUS_READ,
MMC5983_STATUS_WAIT_READ,
MMC5983_STATUS_STOP,
}MMC5983Status_m;
@@ -129,7 +126,6 @@ void MMC5983Calib(void);
void MMC5983GetADValue(int32_t *Mag);
void MMC5983_Get_Mag(float *mag);
MMC5983Status_m GetMMC5983Status(void);
void MMC5983StartRead(void);
bool GetMMC5983CalibStatus(void);
#endif