docs: sx127x.h补充编译期信道选择注释示例(USE_LORA_CH, 0~119)

feat: LoRa频率设定范围改为433.1~492.6MHz, 步进500KHz
- sx127x.h: CH0~CH45宏表(470.1~492.6MHz, 46信道)替换为算术定义: LORA_FREQ_BASE=433100000, LORA_FREQ_STEP=500000, LORA_FREQ_MAX=492600000, LORA_CHANNEL_CNT=120(CH0~CH119); FREQ_CENT由编译期USE_LORA_CH(0~119)算术换算, 未指定时出厂默认CH0=433.1MHz - sx127x.c: LoraSetFreqCent范围校验放宽至433.1~492.6MHz; LoraSetChannel上限45->119 - main.c: Flash参数BLPara.LoraFreq范围校验同步放宽至433.1~492.6MHz - 频率公式: Freq = 433100000 + 500000*N, N = 0~119 - 新旧信道号映射: 旧CHn = 新CH(74+n), 例: 旧CH0(470.1MHz) = 新CH74 - 注意: 网关侧需使用相同频率集合 - 所有编辑文件保持UTF-8编码(无BOM)
2026-09-08 15:31:02 +08:00 · 2026-09-08 09:53:48 +08:00
7 changed files with 384 additions and 535 deletions
@@ -622,7 +622,7 @@ void Sx1276RxRetValue(void)

 bool LoraSetChannel(uint8_t Channel)
 {
-	if(Channel > 45)
+	if(Channel >= LORA_CHANNEL_CNT)		//宽带版: 0~119
 		return false;
 	LoRaPara.ucChannel = Channel;
 	return true;
@@ -667,7 +667,7 @@ bool LoraSetRegPreamble(uint8_t RegPreamble)
 }
 bool LoraSetFreqCent(uint32_t FreqCent)
 {
-	if(FreqCent < 470100000ul || FreqCent > 492600000ul)
+	if(FreqCent < LORA_FREQ_BASE || FreqCent > LORA_FREQ_MAX)		//宽带: 433.1~492.6MHz
 		return false;
 	LoRaPara.dwFreqHz = FreqCent;
 	return true;
@@ -637,7 +637,7 @@ typedef enum{
 	SX1276_IH_ON=		(1)
 }Sx1276ImplicitHeaderOnType;

-// ɱ     
+// ?     
 typedef struct _AutoSfType{
 	Sx1276SpreadFactorType Sf;
 	uint8_t ucRssiMin, ucRssiMax;
@@ -745,162 +745,40 @@ typedef struct {
 //Down Link:
 //CH0-47:500.3-509.7MHz

-#define USE_915MHZ      0//=1ʹ  915   =0ʹ  433
+#define USE_915MHZ      0//=1?  915   =0?  433

 #if (USE_915MHZ == 1)
-  //    Ƶ ʣ         Ĭ  Ϊ449000000Hz
+  //    ? ?         ?  ?449000000Hz
  #define FREQ_CENT       915000000ul
-  //860MHz-1GHzʹ  PA   ţ         Ĭ ϲ ʹ  
-  //SX127x     壬HOPERFģ    Ҫ    
+  //860MHz-1GHz?  PA   ?         ? ? ?  
+  //SX127x     壬HOPERF?    ?    
  #define USE_LORA_860_PA
 #endif

 /*
-中心频率配置：470100000ul ~ 492600000ul，步进500000
+中心频率配置(宽带版): 433100000ul ~ 492600000ul, 步进500000, 共120信道(CH0~CH119)
+频率公式: Freq = 433100000 + 500000*N, N = 0~119
+编译期可用 USE_LORA_CH(0~119) 指定频率; 未指定时出厂默认 CH0=433.1MHz
 */
-#define CH0 	(470100000ul) //出厂默认频率
-#define CH1		(470600000ul)
-#define CH2		(471100000ul)
-#define CH3		(471600000ul)
-#define CH4		(472100000ul)
-#define CH5		(472600000ul)
-#define CH6		(473100000ul)
-#define CH7		(473600000ul)
-#define CH8		(474100000ul)
-#define CH9		(474600000ul)
-#define CH10	(475100000ul)
-#define CH11	(475600000ul)
-#define CH12	(476100000ul)
-#define CH13	(476600000ul)
-#define CH14	(477100000ul)
-#define CH15	(477600000ul)
-#define CH16	(478100000ul)
-#define CH17	(478600000ul)
-#define CH18	(479100000ul)
-#define CH19	(479600000ul)
-#define CH20	(480100000ul)
-#define CH21	(480600000ul)
-#define CH22	(481100000ul)
-#define CH23	(481600000ul)
-#define CH24	(482100000ul)
-#define CH25	(482600000ul)
-#define CH26	(483100000ul)
-#define CH27	(483600000ul)
-#define CH28	(484100000ul)
-#define CH29	(484600000ul)
-#define CH30	(485100000ul)
-#define CH31	(485600000ul)
-#define CH32	(486100000ul)
-#define CH33	(486600000ul)
-#define CH34	(487100000ul)
-#define CH35	(487600000ul)
-#define CH36	(488100000ul)
-#define CH37	(488600000ul)
-#define CH38	(489100000ul)
-#define CH39	(489600000ul)
-#define CH40	(490100000ul)
-#define CH41	(490600000ul)
-#define CH42	(491100000ul)
-#define CH43	(491600000ul)
-#define CH44	(492100000ul)
-#define CH45	(492600000ul)
+#define LORA_FREQ_BASE			(433100000ul)
+#define LORA_FREQ_STEP			(500000ul)
+#define LORA_FREQ_MAX			(492600000ul)
+#define LORA_CHANNEL_CNT		(120)

-#if (USE_LORA_CH == 0)
-#define	FREQ_CENT				CH0
-#elif (USE_LORA_CH == 1)
-#define	FREQ_CENT				CH1
-#elif (USE_LORA_CH == 2)
-#define	FREQ_CENT				CH2
-#elif (USE_LORA_CH == 3)
-#define	FREQ_CENT				CH3
-#elif (USE_LORA_CH == 4)
-#define	FREQ_CENT				CH4
-#elif (USE_LORA_CH == 5)
-#define	FREQ_CENT				CH5
-#elif (USE_LORA_CH == 6)
-#define	FREQ_CENT				CH6
-#elif (USE_LORA_CH == 7)
-#define	FREQ_CENT				CH7
-#elif (USE_LORA_CH == 8)
-#define	FREQ_CENT				CH8
-#elif (USE_LORA_CH == 9)
-#define	FREQ_CENT				CH9
-#elif (USE_LORA_CH == 10)
-#define	FREQ_CENT				CH10
-#elif (USE_LORA_CH == 11)
-#define	FREQ_CENT				CH11
-#elif (USE_LORA_CH == 12)
-#define	FREQ_CENT				CH12
-#elif (USE_LORA_CH == 13)
-#define	FREQ_CENT				CH13
-#elif (USE_LORA_CH == 14)
-#define	FREQ_CENT				CH14
-#elif (USE_LORA_CH == 15)
-#define	FREQ_CENT				CH15
-#elif (USE_LORA_CH == 16)
-#define	FREQ_CENT				CH16
-#elif (USE_LORA_CH == 17)
-#define	FREQ_CENT				CH17
-#elif (USE_LORA_CH == 18)
-#define	FREQ_CENT				CH18
-#elif (USE_LORA_CH == 19)
-#define	FREQ_CENT				CH19
-#elif (USE_LORA_CH == 20)
-#define	FREQ_CENT				CH20
-#elif (USE_LORA_CH == 21)
-#define	FREQ_CENT				CH21
-#elif (USE_LORA_CH == 22)
-#define	FREQ_CENT				CH22
-#elif (USE_LORA_CH == 23)
-#define	FREQ_CENT				CH23
-#elif (USE_LORA_CH == 24)
-#define	FREQ_CENT				CH24
-#elif (USE_LORA_CH == 25)
-#define	FREQ_CENT				CH25
-#elif (USE_LORA_CH == 26)
-#define	FREQ_CENT				CH26
-#elif (USE_LORA_CH == 27)
-#define	FREQ_CENT				CH27
-#elif (USE_LORA_CH == 28)
-#define	FREQ_CENT				CH28
-#elif (USE_LORA_CH == 29)
-#define	FREQ_CENT				CH29
-#elif (USE_LORA_CH == 30)
-#define	FREQ_CENT				CH30
-#elif (USE_LORA_CH == 31)
-#define	FREQ_CENT				CH31
-#elif (USE_LORA_CH == 32)
-#define	FREQ_CENT				CH32
-#elif (USE_LORA_CH == 33)
-#define	FREQ_CENT				CH33
-#elif (USE_LORA_CH == 34)
-#define	FREQ_CENT				CH34
-#elif (USE_LORA_CH == 35)
-#define	FREQ_CENT				CH35
-#elif (USE_LORA_CH == 36)
-#define	FREQ_CENT				CH36
-#elif (USE_LORA_CH == 37)
-#define	FREQ_CENT				CH37
-#elif (USE_LORA_CH == 38)
-#define	FREQ_CENT				CH38
-#elif (USE_LORA_CH == 39)
-#define	FREQ_CENT				CH39
-#elif (USE_LORA_CH == 40)
-#define	FREQ_CENT				CH40
-#elif (USE_LORA_CH == 41)
-#define	FREQ_CENT				CH41
-#elif (USE_LORA_CH == 42)
-#define	FREQ_CENT				CH42
-#elif (USE_LORA_CH == 43)
-#define	FREQ_CENT				CH43
-#elif (USE_LORA_CH == 44)
-#define	FREQ_CENT				CH44
-#elif (USE_LORA_CH == 45)
-#define	FREQ_CENT				CH45
+#define CH0		LORA_FREQ_BASE		//出厂默认频率
+
+//#define USE_LORA_CH		1
+
+#if defined(USE_LORA_CH)
+#if (USE_LORA_CH >= 0 && USE_LORA_CH < LORA_CHANNEL_CNT)
+#define	FREQ_CENT				(LORA_FREQ_BASE + (uint32_t)(USE_LORA_CH) * LORA_FREQ_STEP)
+#else
+#error "USE_LORA_CH out of range, valid: 0 ~ 119"
+#endif
 #endif

 #if !defined FREQ_CENT
-#define FREQ_CENT				(470100000ul)//500000
+#define FREQ_CENT				(CH0)//500000
 #endif
 #if !defined FREQ_DEV
 #define FREQ_DEV				1000000ul
@@ -1,4 +1,4 @@
-
+
 /* Includes ------------------------------------------------------------------*/
 #include "lsm6dsl_app.h"
 #include "lsm6dsl_reg.h"
@@ -13,24 +13,31 @@
 dev_ctx_t dev_ctx;
 static bool InitFlag;

-#define	ACC_COLL_CNT_MAX			4								//每组平均样本数,输出速率=ODR/4=13Hz
-#define	ACC_FIFO_WATERMASK		(ACC_COLL_CNT_MAX * 6)			//FIFO水印:4组 × (3陀螺+3加速度)=24
+#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_Get_Data,			//连续采集运行中
+	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 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;


@@ -53,57 +60,6 @@ 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 */
@@ -124,7 +80,63 @@ static void Lsm6dsl_Init(void)
 		}
 	}

-  Lsm6dsl_Config();
+  /* 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"); 
@@ -136,37 +148,36 @@ 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 < ACC_FIFO_WATERMASK) {
+	if(DiffFifo == 0) {
 		return;
 	}
 	
-	Sets = DiffFifo / 6;
-	for(i = 0; i < Sets; i++) {
+	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));
-		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);
+		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);
 	}
-	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);
+	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)
@@ -181,19 +192,43 @@ static void ReadWakeUpIntStatus(void)
 void Lsm6dslLoopHandler(void)
 {
 	ReadWakeUpIntStatus();
-	if(acc_status != Acc_Get_Data) {
-		return;
+	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;
 			}
-	if(Lsm6dslRecoverReq) {
-		Lsm6dslRecoverReq = false;
-		Lsm6dsl_Config();				//1秒无水印中断,重新配置FIFO/ODR自恢复
-		Lsm6dslIntWatchdog = 0;
-		return;
-	}
-	if(Lsm6dslWaterMarkFullIntFlag) {
+			else if(Lsm6dslWaterMarkFullIntFlag) {
 				Lsm6dslWaterMarkFullIntFlag = false;
-		Lsm6dslIntWatchdog = 0;
+				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;
 	}
 }

@@ -201,17 +236,6 @@ 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)
@@ -221,19 +245,15 @@ void Lsm6dslInit(void)

 void Lsm6dsStart(void)
 {
-	if(acc_status == Acc_Get_Data) {
-		return;						//已在连续采集,不重复配置
-	}
-	Lsm6dsl_Init();					//配置寄存器并开启连续采集
-	acc_status = Acc_Get_Data;
+	Lsm6dsl_Init();
+	acc_status = Acc_Start;
 }

 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;
 }

@@ -1,21 +1,14 @@
-#include "main.h"
+#include "main.h"
 #include "mmc5983.h"
 #include "UartDebug.h"
 #include "i2c.h"
 #include "Algorithm.h"

-#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 int32_t max_in[XYZ];
+static int32_t min_in[XYZ];
 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
@@ -57,24 +50,19 @@ static bool MMC5983_Read_Buffer(uint8_t reg, void *buffer, uint8_t len)
 }

 /*****************************************************************************************
-* 函数名称: MMC5983StartContinuous
-* 功能描述: MMC5983开启20Hz连续转换(常开,不进入低功耗)
+* 函数名称: MMC5983StartCov
+* 功能描述: MMC5983开启连续转换
 * 参    数: 无
 * 返 回 值: 无
 *****************************************************************************************/
-static void MMC5983StartContinuous(void)
+static void MMC5983StartCov(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 = 0x03;	//011=20Hz连续测量
+	Ctrl2Reg.Bit.Cm_freq = 0x04;		
 	Ctrl2Reg.Bit.Cmm_en = 1;		//连续设1，单次设0
-	Ctrl2Reg.Bit.Prd_set = 3;		//每100次测量自动SET一次,消除温漂
-	Ctrl2Reg.Bit.En_prd_set = 1;
+	Ctrl2Reg.Bit.Prd_set = 3;
+	Ctrl2Reg.Bit.En_prd_set = 1;	//连续设1，单次设0
 	MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
 }

@@ -93,7 +81,7 @@ static void MMC5983_Init(void)
 	MMC5983Var.MMC5983Delay1mSCnt = 100;
 	while(id != 0x30){
 		if(MMC5983Var.MMC5983Delay1mSCnt <= 0) {
-			DBG_LOG("MMC5983 INIT Err...\r\n");
+			DBG_LOG("LEFT MMC5983 INIT Err...\r\n");
 			InitFlag = false;
 			return;
 		}
@@ -105,8 +93,12 @@ static void MMC5983_Init(void)
 	MMC5983_Write_Reg(MMC5983_CTRL_1, Ctrl1Reg.Byte);
 	MMC5983Delay(15);
 	
-	MMC5983RegCtrl2 Ctrl2Reg;		//连续模式在StartContinuous中开启
+	MMC5983RegCtrl2 Ctrl2Reg;		//开启/关闭连续转换
 	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;
@@ -117,8 +109,9 @@ static void MMC5983_Init(void)
 	
 	MMC5983RegCtrl0 Ctrl0Reg;
 	Ctrl0Reg.Byte = 0x00;
-	Ctrl0Reg.Bit.Auto_SR_en = 1;			//自动SET/RESET,消除温漂零场漂移
-	Ctrl0Reg.Bit.NT_meas_done_en = 1;		//测量完成中断使能
+	Ctrl0Reg.Bit.TM_M = 1;
+	Ctrl0Reg.Bit.Auto_SR_en = 1;
+	Ctrl0Reg.Bit.NT_meas_done_en = 1;
 	MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
 	
 	if(InitFlag == false){
@@ -129,7 +122,7 @@ static void MMC5983_Init(void)

 /*****************************************************************************************
 * 函数名称: MMC5983_Read
-* 功能描述: MMC5983读取原始数据(18bit)
+* 功能描述: MMC5983读取原始数据
 * 参    数: MagAdcX X轴数据
 				MagAdcY Y轴数据
 				MagAdcZ Z轴数据
@@ -171,89 +164,100 @@ void MMC5983_Get_Mag(float *mag)
 }

 /*****************************************************************************************
-* 函数名称: MMC5983MeasDoneProcess
-* 功能描述: 单次测量完成处理:读取样本、清除中断锁存,凑够MAG_AVG_CNT个样本后平均输出
+* 函数名称: MMC5983LoopHandler
+* 功能描述: MMC5983循环处理函数
 * 参    数: 无
 * 返 回 值: 无
 *****************************************************************************************/
-static void MMC5983MeasDoneProcess(void)
+extern void MmcDataProcessCallBack(float *Mag);
+
+void MMC5983LoopHandler(void)
 {
-	MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS;
-	if(MMC5983_Read(&mag_array[X][magcnt], &mag_array[Y][magcnt], &mag_array[Z][magcnt])){
-		magcnt++;
+	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;
 				}		
-	MMC5983_Write_Reg(MMC5983_STATUS, 0x01);	//写1清Meas_M_Done,释放INT锁存,下次测量产生新上升沿
-	if(magcnt >= MAG_AVG_CNT){
+			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;	
+			}
+		break;}
+			
+		case MMC5983_STATUS_READ:{
+			MMC5983Var.MMC5983Delay1mSCnt = 50;
+			MMC5983Var.Status = MMC5983_STATUS_WAIT_READ;
+		break;}
+		
+		case	MMC5983_STATUS_WAIT_READ:{
+			if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
+				MMC5983Var.Status = MMC5983_STATUS_DELAY;
+				TimeOutMagcnt++;
+			}
+			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;
+			}
+			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);
-	}
-}
-
-/*****************************************************************************************
-* 函数名称: MMC5983LoopHandler
-* 功能描述: MMC5983循环处理函数,20Hz连续采样,2样本平均后10Hz输出
-* 参    数: 无
-* 返 回 值: 无
-*****************************************************************************************/
-
-void MMC5983LoopHandler(void)
-{
-	switch(MMC5983Var.Status) {
-		case MMC5983_STATUS_IDLE:{
-		break;}
-
-		case MMC5983_STATUS_START:{
-			if(!InitFlag){
-				MMC5983_Init();
-				if(!InitFlag){
-					MMC5983Var.Status = MMC5983_STATUS_IDLE;
-					break;
-				}
-			}
-			magcnt = 0;
-			PollFailCnt = 0;
-			MMC5983StartContinuous();					//开启20Hz连续测量
-			MMC5983Var.MMC5983Delay1mSCnt = MAG_POLL_MS;
-			MMC5983Var.Status = MMC5983_STATUS_WAIT_READ;
-		break;}
-
-		case MMC5983_STATUS_WAIT_READ:{
-			if(MMC5983IntFlag == true){
-				MMC5983IntFlag = false;
-				PollFailCnt = 0;
-				MMC5983MeasDoneProcess();				//INT触发,立即读数
-			}
-			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();
+					MMC5983Var.Status = MMC5983_STATUS_STOP;
 				}
 				else{
-					PollFailCnt++;
-					if(PollFailCnt >= MAG_POLL_FAIL_MAX){
-						PollFailCnt = 0;
-						MMC5983StartContinuous();		//连续无数据,重写配置自恢复
+					MMC5983Var.Status = MMC5983_STATUS_STOP;
+//					DBG_LOG("LEFT MMC5983 TimeOut...\r\n"); 
 				}
 			}
+			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;}
 	}
 }

 /*****************************************************************************************
-* 函数名称: MMC59831mSRoutine
+* 函数名称: LeftMMC59831mSRoutine
 * 功能描述: MMC59831ms时基循环处理函数
 * 参    数: 无
 * 返 回 值: 无
@@ -265,7 +269,7 @@ void MMC59831mSRoutine(void)
 }

 /*****************************************************************************************
-* 函数名称: MMC5983Init
+* 函数名称: LeftMMC5983Init
 * 功能描述: MMC5983初始化
 * 参    数: 无
 * 返 回 值: 无
@@ -281,47 +285,53 @@ void MMC5983Init(void)
 }

 /*****************************************************************************************
-* 函数名称: MMC5983Start
-* 功能描述: MMC5983启动(幂等,已运行则直接返回)
+* 函数名称: LeftMMC5983Start
+* 功能描述: MMC5983启动
 * 参    数: 无
 * 返 回 值: 无
 *****************************************************************************************/
 void MMC5983Start(void)
 {
-	if(MMC5983Var.Status == MMC5983_STATUS_WAIT_READ){
-		return;									//已在连续采样,不重复配置
-	}
 	if(!InitFlag){
 		MMC5983_Init();
-		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);
 	
-	MMC5983Var.Status = MMC5983_STATUS_IDLE;
+	MMC5983RegStatus StatusReg;
+	StatusReg.Byte = MMC5983_Read_Reg(MMC5983_STATUS);
+	if(StatusReg.Bit.Meas_M_Done == 1){
+		MMC5983_Write_Reg(MMC5983_STATUS, StatusReg.Byte);
+	}
 }

 /*****************************************************************************************
-* 函数名称: MMC5983IntCallBack
+* 函数名称: LeftMMC5983IntCallBack
 * 功能描述: MMC5983中断回调
 * 参    数: 无
 * 返 回 值: 无
@@ -330,3 +340,4 @@ void MMC5983IntCallBack(void)
 {
 	MMC5983IntFlag = true;
 }
+
@@ -103,6 +103,9 @@ 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;
@@ -126,6 +129,7 @@ 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

@@ -66,11 +66,11 @@

 /**
 ******************************************************************************
- ** \brief  I2C设置波特率配置寄存器
+ ** \brief  I2C设置波特率配置寄存器
 **
- ** \param [in] u8Tm 波特率配置值
+ ** \param [in] u8Tm 波特率配置值
 **
- ** \retval enRet 成功或失败
+ ** \retval enRet 成功或失败
 **
 ******************************************************************************/
 en_result_t I2C_SetBaud(M0P_I2C_TypeDef* I2Cx, uint8_t u8Tm)
@@ -84,11 +84,11 @@
 }
 /**
 ******************************************************************************
- ** \brief  I2C功能设置相关函数
+ ** \brief  I2C功能设置相关函数
 **
- ** \param [in] enFunc功能参数
+ ** \param [in] enFunc功能参数
 **
- ** \retval enRet 成功或失败
+ ** \retval enRet 成功或失败
 **
 ******************************************************************************/
 en_result_t I2C_SetFunc(M0P_I2C_TypeDef* I2Cx, en_i2c_func_t enFunc)
@@ -102,11 +102,11 @@ en_result_t I2C_SetFunc(M0P_I2C_TypeDef* I2Cx, en_i2c_func_t enFunc)
 }
 /**
 ******************************************************************************
- ** \brief  I2C功能清除相关函数
+ ** \brief  I2C功能清除相关函数
 **
- ** \param [in] enFunc功能参数
+ ** \param [in] enFunc功能参数
 **
- ** \retval enRet 成功或失败
+ ** \retval enRet 成功或失败
 **
 ******************************************************************************/
 en_result_t I2C_ClearFunc(M0P_I2C_TypeDef* I2Cx, en_i2c_func_t enFunc)
@@ -120,11 +120,11 @@ en_result_t I2C_SetFunc(M0P_I2C_TypeDef* I2Cx, en_i2c_func_t enFunc)
 }
 /**
 ******************************************************************************
- ** \brief  I2C获取中断标记函数
+ ** \brief  I2C获取中断标记函数
 **
- ** \param 无
+ ** \param 无
 **
- ** \retval bIrq中断标记
+ ** \retval bIrq中断标记
 **
 ******************************************************************************/
 boolean_t I2C_GetIrq(M0P_I2C_TypeDef* I2Cx)
@@ -140,11 +140,11 @@ boolean_t I2C_GetIrq(M0P_I2C_TypeDef* I2Cx)
 }
 /**
 ******************************************************************************
- ** \brief  I2C清除中断标记函数
+ ** \brief  I2C清除中断标记函数
 **
- ** \param 无
+ ** \param 无
 **
- ** \retval bIrq中断标记
+ ** \retval bIrq中断标记
 **
 ******************************************************************************/
 en_result_t I2C_ClearIrq(M0P_I2C_TypeDef* I2Cx)
@@ -158,11 +158,11 @@ en_result_t I2C_ClearIrq(M0P_I2C_TypeDef* I2Cx)
 }
 /**
 ******************************************************************************
- ** \brief  I2C获取相关状态
+ ** \brief  I2C获取相关状态
 **
- ** \param 无
+ ** \param 无
 **
- ** \retval I2C状态
+ ** \retval I2C状态
 **
 ******************************************************************************/
 uint8_t I2C_GetState(M0P_I2C_TypeDef* I2Cx)
@@ -176,11 +176,11 @@ uint8_t I2C_GetState(M0P_I2C_TypeDef* I2Cx)

 /**
 ******************************************************************************
- ** \brief  字节数据写函数
+ ** \brief  字节数据写函数
 **
- ** \param  u8Data写数据
+ ** \param  u8Data写数据
 **
- ** \retval 写数据是否成功
+ ** \retval 写数据是否成功
 **
 ******************************************************************************/
 en_result_t I2C_WriteByte(M0P_I2C_TypeDef* I2Cx, uint8_t u8Data)
@@ -194,11 +194,11 @@ en_result_t I2C_WriteByte(M0P_I2C_TypeDef* I2Cx, uint8_t u8Data)
 }
 /**
 ******************************************************************************
- ** \brief  字节数据读函数
+ ** \brief  字节数据读函数
 **
- ** \param  无
+ ** \param  无
 **
- ** \retval 读取数据
+ ** \retval 读取数据
 **
 ******************************************************************************/
 uint8_t I2C_ReadByte(M0P_I2C_TypeDef* I2Cx)
@@ -212,11 +212,11 @@ uint8_t I2C_ReadByte(M0P_I2C_TypeDef* I2Cx)

 /**
 ******************************************************************************
- ** \brief  I2C模块初始化
+ ** \brief  I2C模块初始化
 **
- ** \param pstcI2CCfg初始化配置结构体
+ ** \param pstcI2CCfg初始化配置结构体
 **
- ** \retval 初始化是否成功
+ ** \retval 初始化是否成功
 **
 ******************************************************************************/
 en_result_t I2C_Init(M0P_I2C_TypeDef* I2Cx, stc_i2c_cfg_t *pstcI2CCfg)
@@ -265,89 +265,71 @@ en_result_t I2C_Init(M0P_I2C_TypeDef* I2Cx, stc_i2c_cfg_t *pstcI2CCfg)
 }
 /**
 ******************************************************************************
- ** \brief  I2C模块接收指定长度函数
+ ** \brief  I2C模块接收指定长度函数
 **
- ** \param  u8SlaveAddr从机设备地址，u8Addr从机内存地址，pu8Data读数据存放缓存，u32Len读数据长度
+ ** \param  u8SlaveAddr从机设备地址，u8Addr从机内存地址，pu8Data读数据存放缓存，u32Len读数据长度
 **
- ** \retval 读数据是否成功
+ ** \retval 读数据是否成功
 **
 ******************************************************************************/
- #define I2C_WAIT_TIMEOUT     20000u     ///< 单步等待超时计数,防止总线卡死导致系统死循环
-#define I2C_READ_PASS_MAX    200u       ///< 读传输状态机最大循环次数(48字节FIFO读约需120次)
-#define I2C_WRITE_PASS_MAX   64u        ///< 写传输状态机最大循环次数
-
-boolean_t I2C_MasterReadData(M0P_I2C_TypeDef* I2CX,uint8_t u8SlaveAddr,uint8_t u8Addr,uint8_t *pu8Data,uint32_t u32Len)
+ boolean_t I2C_MasterReadData(M0P_I2C_TypeDef* I2CX,uint8_t u8SlaveAddr,uint8_t u8Addr,uint8_t *pu8Data,uint32_t u32Len)
 {
    boolean_t enRet = FALSE;
    uint8_t u8i=0,u8State;
-    uint32_t u32Wait = 0;
-    uint16_t u16Pass = 0;
    
-    I2C_SetFunc(I2CX,I2cStart_En);//开始
+    I2C_SetFunc(I2CX,I2cStart_En);//开始
    
    while(1)
    {
-        while(0 == I2C_GetIrq(I2CX))//获取中断标记函数
-        {
-            if(++u32Wait >= I2C_WAIT_TIMEOUT)        ///< 等待超时,总线异常
-            {
-                I2C_SetFunc(I2CX,I2cStop_En);        ///< 尝试发送停止条件释放总线
-                return FALSE;
-            }
-        }
-        u32Wait = 0;
-        if(++u16Pass >= I2C_READ_PASS_MAX)           ///< 状态机重试超限(持续NACK等),放弃本次传输
-        {
-            I2C_SetFunc(I2CX,I2cStop_En);
-            return FALSE;
-        }
-        u8State = I2C_GetState(I2CX);//获取i2c相关状态
+        while(0 == I2C_GetIrq(I2CX))//获取中断标记函数
+        {;}
+        u8State = I2C_GetState(I2CX);//获取i2c相关状态
        switch(u8State)
        {
-            case 0x08:                                 ///< 主设备向总线已发出了一个起始条件
+            case 0x08:                                 ///< 主设备向总线已发出了一个起始条件
                I2C_ClearFunc(I2CX,I2cStart_En);
                I2C_WriteByte(I2CX,(u8SlaveAddr << 1));
                break;
-            case 0x18:                                 ///< 主设备向总线已发出了一个从设备地址（写方向）并且接收到从设备的应答
-                I2C_WriteByte(I2CX,u8Addr);            ///< 发送从机内存地址
+            case 0x18:                                 ///< 主设备向总线已发出了一个从设备地址（写方向）并且接收到从设备的应答
+                I2C_WriteByte(I2CX,u8Addr);            ///< 发送从机内存地址
                break;
-            case 0x28:                                 ///< 已发送数据，接收到ACK, 此处是已发送从机内存地址u8Addr并接收到ACK
-                I2C_SetFunc(I2CX,I2cStart_En);         ///< 发送重复起始条件
+            case 0x28:                                 ///< 已发送数据，接收到ACK, 此处是已发送从机内存地址u8Addr并接收到ACK
+                I2C_SetFunc(I2CX,I2cStart_En);         ///< 发送重复起始条件
                break;
-            case 0x10:                                 ///< 主设备向总线已发出了一个重复的起始条件
+            case 0x10:                                 ///< 主设备向总线已发出了一个重复的起始条件
                I2C_ClearFunc(I2CX,I2cStart_En);
-                I2C_WriteByte(I2CX,(u8SlaveAddr << 1) | 0x01);///< 发送SLA+R，开始从从机读取数据
+                I2C_WriteByte(I2CX,(u8SlaveAddr << 1) | 0x01);///< 发送SLA+R，开始从从机读取数据
                break;
-            case 0x40:                                 ///< 已发送SLA+R，并接收到ACK
+            case 0x40:                                 ///< 已发送SLA+R，并接收到ACK
                if(u32Len>1)
                {
-                    I2C_SetFunc(I2CX,I2cAck_En);       ///< 使能主机应答功能
+                    I2C_SetFunc(I2CX,I2cAck_En);       ///< 使能主机应答功能
                }
                break;
-            case 0x50:                                 ///< 已接收数据字节，并已返回ACK信号
+            case 0x50:                                 ///< 已接收数据字节，并已返回ACK信号
                pu8Data[u8i++] = I2C_ReadByte(I2CX);
                if(u8i==u32Len-1)
                {
-                    I2C_ClearFunc(I2CX,I2cAck_En);     ///< 已接收到倒数第二个字节，关闭ACK应答功能
+                    I2C_ClearFunc(I2CX,I2cAck_En);     ///< 已接收到倒数第二个字节，关闭ACK应答功能
                }
                break;
-            case 0x58:                                 ///< 已接收到最后一个数据，NACK已返回
+            case 0x58:                                 ///< 已接收到最后一个数据，NACK已返回
                pu8Data[u8i++] = I2C_ReadByte(I2CX);
-                I2C_SetFunc(I2CX,I2cStop_En);          ///< 发送停止条件
+                I2C_SetFunc(I2CX,I2cStop_En);          ///< 发送停止条件
                break;
-            case 0x38:                                 ///< 在发送地址或数据时，仲裁丢失
-                I2C_SetFunc(I2CX,I2cStart_En);         ///< 当总线空闲时发起起始条件
+            case 0x38:                                 ///< 在发送地址或数据时，仲裁丢失
+                I2C_SetFunc(I2CX,I2cStart_En);         ///< 当总线空闲时发起起始条件
                break;
-            case 0x48:                                 ///< 发送SLA+R后，收到一个NACK
-                I2C_SetFunc(I2CX,I2cStop_En);          ///< 发送停止条件
-                I2C_SetFunc(I2CX,I2cStart_En);         ///< 发送起始条件
+            case 0x48:                                 ///< 发送SLA+R后，收到一个NACK
+                I2C_SetFunc(I2CX,I2cStop_En);          ///< 发送停止条件
+                I2C_SetFunc(I2CX,I2cStart_En);         ///< 发送起始条件
                break;
            default:
-                I2C_SetFunc(I2CX,I2cStart_En);         ///< 其他错误状态，重新发送起始条件
+                I2C_SetFunc(I2CX,I2cStart_En);         ///< 其他错误状态，重新发送起始条件
                break;
        }
-        I2C_ClearIrq(I2CX);                            ///< 清除中断状态标志位
-        if(u8i==u32Len)                                ///< 数据全部读取完成，跳出while循环
+        I2C_ClearIrq(I2CX);                            ///< 清除中断状态标志位
+        if(u8i==u32Len)                                ///< 数据全部读取完成，跳出while循环
        {
                break;
        }
@@ -357,76 +339,60 @@ boolean_t I2C_MasterReadData(M0P_I2C_TypeDef* I2CX,uint8_t u8SlaveAddr,uint8_t u
 }
 /**
 ******************************************************************************
- ** \brief  I2C模块主机发送指定长度函数
+ ** \brief  I2C模块主机发送指定长度函数
 **
- ** \param u8SlaveAddr从机设备地址，u8Addr从机内存地址，pu8Data写数据，u32Len写数据长度
+ ** \param u8SlaveAddr从机设备地址，u8Addr从机内存地址，pu8Data写数据，u32Len写数据长度
 **
- ** \retval 写数据是否成功
+ ** \retval 写数据是否成功
 **
 ******************************************************************************/
 boolean_t I2C_MasterWriteData(M0P_I2C_TypeDef* I2CX,uint8_t u8SlaveAddr,uint8_t u8Addr,uint8_t *pu8Data,uint32_t u32Len)
 {
    boolean_t enRet = FALSE;
    uint8_t u8i=0,u8State, u8Flag=FALSE;
-    uint32_t u32Wait = 0;
-    uint16_t u16Pass = 0;
    I2C_SetFunc(I2CX,I2cStart_En);
    while(1)
    {
        while(0 == I2C_GetIrq(I2CX))
-        {
-            if(++u32Wait >= I2C_WAIT_TIMEOUT)        ///< 等待超时,总线异常
-            {
-                I2C_SetFunc(I2CX,I2cStop_En);        ///< 尝试发送停止条件释放总线
-                return FALSE;
-            }
-        }
-        u32Wait = 0;
-        if(++u16Pass >= I2C_WRITE_PASS_MAX)          ///< 状态机重试超限(持续NACK等),放弃本次传输
-        {
-            I2C_SetFunc(I2CX,I2cStop_En);
-            return FALSE;
-        }
+        {;}
        u8State = I2C_GetState(I2CX);
        switch(u8State)
        {
-            case 0x08:                               ///< 已发送起始条件
+            case 0x08:                               ///< 已发送起始条件
                I2C_ClearFunc(I2CX,I2cStart_En);
-                I2C_WriteByte(I2CX,(u8SlaveAddr << 1));   ///< 从设备地址发送
+                I2C_WriteByte(I2CX,(u8SlaveAddr << 1));   ///< 从设备地址发送
                break;
-            case 0x18:                               ///< 已发送SLA+W，并接收到ACK
-                I2C_WriteByte(I2CX,u8Addr);          ///< 从设备内存地址发送
+            case 0x18:                               ///< 已发送SLA+W，并接收到ACK
+                I2C_WriteByte(I2CX,u8Addr);          ///< 从设备内存地址发送
                break;
-            case 0x28:                               ///< 上一次发送数据后接收到ACK
+            case 0x28:                               ///< 上一次发送数据后接收到ACK
                if (u8i < u32Len)
                {
-                    I2C_WriteByte(I2CX,pu8Data[u8i++]);  ///< 继续发送数据
+                    I2C_WriteByte(I2CX,pu8Data[u8i++]);  ///< 继续发送数据
                }else
                {
-                    I2C_SetFunc(I2CX,I2cStop_En);     ///< 出停止条件
+                    I2C_SetFunc(I2CX,I2cStop_En);     ///< 出停止条件
                    u8Flag = TRUE;
-                    enRet = TRUE;                     ///< 全部数据发送完成
                }
                break;
-            case 0x38:                               ///< 上一次在SLA+读或写时丢失仲裁
-                I2C_SetFunc(I2CX,I2cStart_En);       ///< 当I2C总线空闲时发送起始条件
+            case 0x38:                               ///< 上一次在SLA+读或写时丢失仲裁
+                I2C_SetFunc(I2CX,I2cStart_En);       ///< 当I2C总线空闲时发送起始条件
                break;
-            case 0x30:                               ///< 已发送I2Cx_DATA中的数据，收到NACK，将传输一个STOP条件
-                I2C_SetFunc(I2CX,I2cStop_En);        ///< 发送停止条件
-                u8Flag = TRUE;
-                enRet = FALSE;                       ///< 从机NACK,本次传输失败
+            case 0x30:                               ///< 已发送I2Cx_DATA中的数据，收到NACK，将传输一个STOP条件
+                I2C_SetFunc(I2CX,I2cStop_En);        ///< 发送停止条件
                break;
            default:
                break;
        }
        
-        I2C_ClearIrq(I2CX);                          ///< 清除中断状态标志位
+        I2C_ClearIrq(I2CX);                          ///< 清除中断状态标志位
        
-        if(u8Flag == TRUE)  ///< 数据发送完成
+        if(u8Flag == TRUE)  ///< 数据发送完成
        {   
            break;
        }
    }
+    enRet = TRUE;
    return enRet;
 }
 //@} // I2cGroup
@@ -137,10 +137,10 @@ static void AppInit(void)
    App.Para.Layout.FPOTimeStart = 0;
 	}
 	
-	if(App.Para.BLPara.LoraFreq < 470100000ul || App.Para.BLPara.LoraFreq > 492600000ul)
+	if(App.Para.BLPara.LoraFreq < LORA_FREQ_BASE || App.Para.BLPara.LoraFreq > LORA_FREQ_MAX)		//宽带: 433.1~492.6MHz
 		App.Para.BLPara.LoraFreq = CH0;
 	App.Para.Lora.FreqCent = App.Para.BLPara.LoraFreq;
-	SX1276LoRaWriteFreq(App.Para.Lora.FreqCent);		//Lora频率:470100000ul + 500000*N
+	SX1276LoRaWriteFreq(App.Para.Lora.FreqCent);		//Lora频率:433100000ul + 500000*N, 范围433.1~492.6MHz
 	
 	if(App.Para.Rs485Ch1.Addr > 16 || App.Para.Rs485Ch1.Addr < 1){
 		App.Para.Rs485Ch1.Addr = 1;
@@ -235,8 +235,8 @@ static void ReadDataLoopHandler(void)
   	break;}
 		
 		case READDATA_STATUS_UPDAING:{
-			//持续采集模式下UpdateOk不在启动时清零:上一轮数据仍有效,拿到新数据后才覆盖
-			//否则在两轮数据的间隙(约100ms)键入up/coll会误报"Device data is not ready"
+			DATA_DBG_LOG("<------------ Sensor Data Updaing ------------> \r\n");
+			App.UpdateOk = false;
 			Lsm6dsStart();
 			MMC5983Start();
 			App.ReadData1mSDelayCnt = 2000;
@@ -249,7 +249,6 @@ static void ReadDataLoopHandler(void)
 					DATA_DBG_LOG("Lsm6dsl Get Data TimeOut...\r\n");								
 				if(App.MagRead_EndFlag == false)
 					DATA_DBG_LOG("MMC5983 Get Data TimeOut...\r\n");
-				App.UpdateOk = false;		//超时:数据不可信
 				App.RDStatus = READDATA_STATUS_DATA_HANDLE;
 			}
 			else if(App.MagRead_EndFlag == true
@@ -260,8 +259,9 @@ static void ReadDataLoopHandler(void)
 		
 		case READDATA_STATUS_DATA_HANDLE:{
 			if(App.MagRead_EndFlag == false || App.AccRead_EndFlag == false){
-				//有传感器超时未出数:回到采集状态持续重试(启动函数幂等,不会重复初始化)
-				App.RDStatus = READDATA_STATUS_UPDAING;
+				DATA_DBG_LOG("<------------ Sensor Data Upda End ------------> \r\n");
+				App.RDStatus = READDATA_STATUS_IDLE;
+				App.UpdateOk = false;
 				break;
 			}
 			App.MagRead_EndFlag = false;
@@ -271,6 +271,7 @@ static void ReadDataLoopHandler(void)
 			App.ConvertedData.Pitch = (int16_t)(App.AHRSData.Deg.pitch * 10);
 			App.ConvertedData.Roll = (int16_t)(App.AHRSData.Deg.roll * 10);
 			App.ConvertedData.Yaw = (int16_t)(App.AHRSData.Deg.yaw * 10);
+			DATA_DBG_LOG("PitchAngle:%06.1f, RollAngle:%06.1f, YawAngle:%06.1f\r\n",App.AHRSData.Deg.pitch, App.AHRSData.Deg.roll, App.AHRSData.Deg.yaw);
 			
 			if(SGCP.MasterCaliOk == true)
 			{
@@ -316,9 +317,9 @@ static void ReadDataLoopHandler(void)
 #endif
 			
 			memcpy(LocalDataPack, &SGCP.DevData.LaserTracing, sizeof(LocalDataPack));
-			//DATA_DBG_LOG("<------------ Sensor Data Upda End ------------> \r\n");	//10Hz循环打印会阻塞9600调试串口,屏蔽
+			DATA_DBG_LOG("<------------ Sensor Data Upda End ------------> \r\n");
 			//App.RDStatus = READDATA_STATUS_UPDAING;//持续工作
-			App.RDStatus = READDATA_STATUS_UPDAING;//持续采集,姿态数据保持10Hz更新(上传网关节奏不变,仍由UploadFlag定时器控制)
+			App.RDStatus = READDATA_STATUS_IDLE;
 			App.UpdateOk = true;
   	break;}
   }
@@ -351,34 +352,6 @@ bool IsFullPowerTime(void)
 	return false;
 }

-/*****************************************************************************************
-* 函数名称: LoraPowerHandler
-* 功能描述: LoRa按需供电管理(设备为主动上报模式):
-*						仅在与网关通信期间(GWStatus非IDLE,含注册/上传/信号查询/激光响应)开启LoRa接收,
-*						通信结束回到IDLE立即休眠LoRa,平时不耗电。
-*						单片机/加速度/地磁不受影响,保持持续采集。
-* 参    数: 无
-* 返 回 值: 无
-*****************************************************************************************/
-static void LoraPowerHandler(void)
-{
-	static bool LoraPowerOn = true;		//开机BspInit已开启,初值true
-	bool NeedLora;
-
-	NeedLora = (SGCP.GWStatus != GW_STATUS_IDLE);
-
-	if(NeedLora == true && LoraPowerOn == false) {
-		LoraPowerOn = true;
-		Sx1276LoRaWakeup();				//进入接收模式
-		LORAIntEnable();					//开启LORA中断
-	}
-	else if(NeedLora == false && LoraPowerOn == true) {
-		LoraPowerOn = false;
-		LORAIntDisable();					//关闭LORA中断
-		Sx1276LoRaSleep();				//LoRa休眠,平时不耗电
-	}
-}
-
 static void AppLoopHandler(void)
 {
 	switch(App.Status) {
@@ -420,16 +393,14 @@ static void AppLoopHandler(void)
 				SGCP.GWStatus = GW_STATUS_LOGIN;//注册
 				App.Status = APP_STATUS_WAIT_LOGIN;
 			}
-			else if(SGCP.TimingAcquisition == true)//设备数据获取(持续采集模式下RDStatus常驻非IDLE,采集请求视为已被满足)
+			else if(SGCP.TimingAcquisition == true && App.RDStatus == READDATA_STATUS_IDLE)//设备数据获取
 			{
-			SGCP.TimingAcquisition = false;
-			if(App.RDStatus == READDATA_STATUS_IDLE){
 				PrintfMess();
+				SGCP.TimingAcquisition = false;
 				App.RDStatus = READDATA_STATUS_UPDAING;
 				App.Status = APP_STATUS_ACTION;
 				App.TD1mSDelayCnt = 1000;
 			}
-		}
 			else if(SGCP.LoginOk == true && App.UploadFlag == true && SGCP.GWStatus == GW_STATUS_IDLE && App.UpdateOk == true)
 			{
 				PrintfMess();
@@ -590,7 +561,6 @@ int32_t main(void)
 	//09 00 01 00 00 00
 	
 	while (1) {
-		LoraPowerHandler();//LoRa按需供电:通信时开,平时休眠
 #if(USE_VBAT_AD == 1)
 		ADCLoopHandler();
 #endif