From 9e1d7a84266ff0b86dc4361ee3d735a4a4e37949 Mon Sep 17 00:00:00 2001
From: penglei <1624952103@qq.com>
Date: Sun, 29 Sep 2024 18:33:33 +0800
Subject: [PATCH] =?utf8?q?=E4=BC=98=E5=8C=96gsensor=20fifo=E8=BE=93?=
 =?utf8?q?=E5=85=A5?=
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

---
 .../drivers/peripheral/cywee/src/main_dml.c   | 90 ++++++++++---------
 1 file changed, 46 insertions(+), 44 deletions(-)

diff --git a/code/sdk/drivers/peripheral/cywee/src/main_dml.c b/code/sdk/drivers/peripheral/cywee/src/main_dml.c
index ceda6f6fe..455a5e1b2 100644
--- a/code/sdk/drivers/peripheral/cywee/src/main_dml.c
+++ b/code/sdk/drivers/peripheral/cywee/src/main_dml.c
@@ -1059,58 +1059,59 @@ struct AccelerationData {
     float acc_y;
     float acc_z;
 };
-struct AccelerationData rawdata[GSENSON_MSG_COUNT];  //自定义buffer，用来缓存sensor数据
-int AccdataCount = 0;				//fifo长度
+static struct AccelerationData rawdata[GSENSON_MSG_COUNT];  //自定义buffer，用来缓存sensor数据
+static int AccdataCount = 0;				//fifo长度
 
 void CWM_AP_dmlSensorDataOutput_listen(const dml_sensorData_t * pSdo) 
 {
-
-    if (pSdo->validData & DML_SENSOR_DATA_VALID_DT)
-    {
-        //CWM_OS_dbgPrintf("dt: %d us\n", pSdo->dt_us);
-        rawdata[AccdataCount].dt = pSdo->dt_us;
-    }	
-    if (pSdo->validData & DML_SENSOR_DATA_VALID_ACC)
+    if(AccdataCount < 25)
     {
-        //CWM_OS_dbgPrintf("acc: x=%.3f, y=%.3f, z=%.3f\n",pSdo->acc_data[0], pSdo->acc_data[1], pSdo->acc_data[2]);
-        rawdata[AccdataCount].acc_x = pSdo->acc_data[0];
-        rawdata[AccdataCount].acc_y = pSdo->acc_data[1];
-        rawdata[AccdataCount].acc_z = pSdo->acc_data[2];
+        if (pSdo->validData & DML_SENSOR_DATA_VALID_DT)
+        {
+            //CWM_OS_dbgPrintf("dt: %d us\n", pSdo->dt_us);
+            rawdata[AccdataCount].dt = pSdo->dt_us;
+        }	
+        if (pSdo->validData & DML_SENSOR_DATA_VALID_ACC)
+        {
+            //CWM_OS_dbgPrintf("acc: x=%.3f, y=%.3f, z=%.3f\n",pSdo->acc_data[0], pSdo->acc_data[1], pSdo->acc_data[2]);
+            rawdata[AccdataCount].acc_x = pSdo->acc_data[0];
+            rawdata[AccdataCount].acc_y = pSdo->acc_data[1];
+            rawdata[AccdataCount].acc_z = pSdo->acc_data[2];
 
-        gxyz_t.x = pSdo->acc_data[0];
-        gxyz_t.y = pSdo->acc_data[1];
-        gxyz_t.z = pSdo->acc_data[2];
+            gxyz_t.x = pSdo->acc_data[0];
+            gxyz_t.y = pSdo->acc_data[1];
+            gxyz_t.z = pSdo->acc_data[2];
 
-        if(get_acc_swtich)
-        {
-            if(num_falg==GSENSON_MSG_COUNT)
-            {
-                left_falg++;
-                right_falg++;
-                right_falg=right_falg%GSENSON_MSG_COUNT;
-                left_falg=left_falg%GSENSON_MSG_COUNT;
-                gxyz_t_queue[right_falg].x= pSdo->acc_data[0];
-                gxyz_t_queue[right_falg].y= pSdo->acc_data[1];
-                gxyz_t_queue[right_falg].z= pSdo->acc_data[2];
-            }    
-            else 
+            if(get_acc_swtich)
             {
-                //left_falg++;
-                right_falg++;
-                right_falg=right_falg%GSENSON_MSG_COUNT;
-                //left_falg=left_falg%16;
-                num_falg++;
-                gxyz_t_queue[right_falg].x= pSdo->acc_data[0];
-                gxyz_t_queue[right_falg].y= pSdo->acc_data[1];
-                gxyz_t_queue[right_falg].z= pSdo->acc_data[2];
-                //CWM_OS_dbgPrintf("write: x=%d, y=%d, z=%d\n", gxyz_t_queue[right_falg].x ,gxyz_t_queue[right_falg].y ,gxyz_t_queue[right_falg].z);
-            
-            }
-        } 
+                if(num_falg==GSENSON_MSG_COUNT)
+                {
+                    left_falg++;
+                    right_falg++;
+                    right_falg=right_falg%GSENSON_MSG_COUNT;
+                    left_falg=left_falg%GSENSON_MSG_COUNT;
+                    gxyz_t_queue[right_falg].x= pSdo->acc_data[0];
+                    gxyz_t_queue[right_falg].y= pSdo->acc_data[1];
+                    gxyz_t_queue[right_falg].z= pSdo->acc_data[2];
+                }    
+                else 
+                {
+                    //left_falg++;
+                    right_falg++;
+                    right_falg=right_falg%GSENSON_MSG_COUNT;
+                    //left_falg=left_falg%16;
+                    num_falg++;
+                    gxyz_t_queue[right_falg].x= pSdo->acc_data[0];
+                    gxyz_t_queue[right_falg].y= pSdo->acc_data[1];
+                    gxyz_t_queue[right_falg].z= pSdo->acc_data[2];
+                    //CWM_OS_dbgPrintf("write: x=%d, y=%d, z=%d\n", gxyz_t_queue[right_falg].x ,gxyz_t_queue[right_falg].y ,gxyz_t_queue[right_falg].z);
+                
+                }
+            } 
 
-        AccdataCount++;
-        AccdataCount = AccdataCount % GSENSON_MSG_COUNT;
-    }                     
+            AccdataCount++;
+        }     
+    }     
 }
 
 #endif
@@ -1441,6 +1442,7 @@ int cwm_lib_process(void)   //运行算法
 	if (ret < 0) {
 		CWM_OS_dbgPrintf("FATAL ERROR! ret=%d\n", ret);
 	}
+    AccdataCount = 0;
     return 0;
 }
 
-- 
2.17.1