优化gsensor fifo输入

diff --git a/code/sdk/drivers/peripheral/cywee/src/main_dml.c b/code/sdk/drivers/peripheral/cywee/src/main_dml.c
index ceda6f6..455a5e1 100644 (file)
--- 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;
 }