优化心率取gsensor数据

diff --git a/code/sdk/zhihui/drivers/cywee/dml/cywee_dml.c b/code/sdk/zhihui/drivers/cywee/dml/cywee_dml.c
index 8836546..e32b8c8 100644 (file)
--- a/code/sdk/zhihui/drivers/cywee/dml/cywee_dml.c
+++ b/code/sdk/zhihui/drivers/cywee/dml/cywee_dml.c
@@ -79,6 +79,8 @@ static uint8_t acc_check_cnt = 0;
 
 static uint8_t abs_solute_time = 90;
 
+static osal_mutex g_mutex = { 0 };
+
 /**
  * @brief 更新燃脂时间
  * @param current_mhr_zone 当前心率区间 (2表示燃脂区间)
@@ -1390,37 +1392,82 @@ void cywee_activity_mode_change(int mode)
     }
 }
 
-void cywee_get_gsensor_data(uint8_t cnt,gsenson_t t[])
+// 缓存最近一次的有效GSensor数据（最多9组，适配1~9组取数）
+static gsenson_t g_gsensor_history[9] = {0};
+// 标记缓存是否有效（避免初始状态复用空缓存）
+static uint8_t g_history_valid = 0;
+
+void cywee_get_gsensor_data(uint8_t cnt, gsenson_t t[])
 {
-    if(t == NULL)
+    if(t == NULL || cnt < 1 || cnt > 9) {
+        CWM_OS_dbgPrintf("[GET_DATA] 参数异常 - cnt=%d\n", cnt);
+        // 参数异常时，复用历史数据（避免返回0值）
+        if(t != NULL && g_history_valid) {
+            memcpy(t, g_gsensor_history, sizeof(gsenson_t) * cnt);
+        }
         return;
+    }
+
+    // 初始化输出数组（避免脏数据）
+    memset(t, 0, sizeof(gsenson_t) * cnt);
+    // if(g_mutex.mutex != NULL)
+        osal_mutex_lock(&g_mutex);
+    uint8_t actual_read = 0; // 实际读取的新数据组数
+    // CWM_OS_dbgPrintf("[GET_DATA] 取数前 - num_flag=%d, left_flag=%d, cnt=%d\n", num_flag, left_flag, cnt);
 
-    for(int i=0;i<cnt;i++)
+    // 1. 读取可用的新数据（最多cnt组）
+    for(int i=0; i<cnt && num_flag > 0; i++)
     {
-        // if(osal_mutex_trylock(&mutex_gsenson_queue)==0)
-        // {
-        //     CWM_OS_dbgPrintf("get_gsonsen_xyz_fail\n");
-        //     return;
-        // }
-        //CWM_OS_dbgPrintf("x=%.3f, y=%.3f, z=%.3f\n", gxyz_t_queue[left_flag].x, gxyz_t_queue[left_flag].y, gxyz_t_queue[left_flag].z);
-        t[i].x= gxyz_t_queue[left_flag].x;
-        t[i].y= gxyz_t_queue[left_flag].y;
-        t[i].z= gxyz_t_queue[left_flag].z;
+        t[i].x = gxyz_t_queue[left_flag].x;
+        t[i].y = gxyz_t_queue[left_flag].y;
+        t[i].z = gxyz_t_queue[left_flag].z;
+        
+        // CWM_OS_dbgPrintf("[GET_DATA] 第%d组（新数据）- left_flag=%d, 数据(x=%.3f,y=%.3f,z=%.3f)\n",
+        //                  i, left_flag, t[i].x, t[i].y, t[i].z);
 
+        // 清空已读数据
         gxyz_t_queue[left_flag].x = 0.0f;
         gxyz_t_queue[left_flag].y = 0.0f;
         gxyz_t_queue[left_flag].z = 0.0f;
         
-        if(num_flag)
-        {
-            left_flag++;
-            left_flag=left_flag%GSENSON_MSG_COUNT;
-            num_flag--;
+        left_flag = (left_flag + 1) % GSENSON_MSG_COUNT;
+        num_flag--;
+        actual_read++;
+    }
+    // if(g_mutex.mutex != NULL)
+        osal_mutex_unlock(&g_mutex);
+
+    // 2. 数据不足时，复用历史数据补全
+    if(actual_read < cnt) {
+        // CWM_OS_dbgPrintf("[GET_DATA] 新数据不足（仅%d组），复用历史数据补全剩余%d组\n", actual_read, cnt - actual_read);
+        
+        // 有有效历史数据：补全剩余组数
+        if(g_history_valid) {
+            for(int i=actual_read; i<cnt; i++) {
+                // 复用历史数据（循环复用，保证数据连续性）
+                t[i] = g_gsensor_history[i % cnt];
+                // CWM_OS_dbgPrintf("[GET_DATA] 第%d组（历史数据）- 数据(x=%.3f,y=%.3f,z=%.3f)\n",
+                //                  i, t[i].x, t[i].y, t[i].z);
+            }
+        }
+        // 无历史数据（初始状态）：用最后一条新数据补全（避免0值）
+        else if(actual_read > 0) {
+            for(int i=actual_read; i<cnt; i++) {
+                t[i] = t[actual_read - 1];
+                // CWM_OS_dbgPrintf("[GET_DATA] 第%d组（补全数据）- 数据(x=%.3f,y=%.3f,z=%.3f)\n",
+                //                  i, t[i].x, t[i].y, t[i].z);
+            }
+        }
+    }
 
-        }   
-        // osal_mutex_unlock(&mutex_gsenson_queue);
+    // 3. 更新历史数据缓存（仅当有新数据时更新）
+    if(actual_read > 0) {
+        memcpy(g_gsensor_history, t, sizeof(gsenson_t) * cnt);
+        g_history_valid = 1; // 标记缓存有效
+        // CWM_OS_dbgPrintf("[GET_DATA] 更新历史数据缓存（%d组）\n", cnt);
     }
 
+    // CWM_OS_dbgPrintf("[GET_DATA] 取数完成 - 实际读取新数据%d组，返回总数%d组\n", actual_read, cnt);
 }
 
 //static int flag=32;
@@ -1913,54 +1960,82 @@ struct AccelerationData {
 };
 static struct AccelerationData rawdata[25];  //自定义buffer，用来缓存sensor数据
 static int AccdataCount = 0;                           //fifo长度
-
 void CWM_APSensorDataOutput_listen(const dml_sensorData_t * pSdo) 
 {
+
+    
     if(AccdataCount < 25)
     {
+        // if(g_mutex.mutex != NULL)
+            osal_mutex_lock(&g_mutex);
+
         if (pSdo->validData & DML_SENSOR_DATA_VALID_DT)
         {
-            //CWM_OS_dbgPrintf("dt: %d us\n", pSdo->dt_us);
+            // 保留原有dt_us存储逻辑
             rawdata[AccdataCount].dt = pSdo->dt_us;
         }      
         if (pSdo->validData & DML_SENSOR_DATA_VALID_ACC)
         {
-            if(cywee_factory_log)
-                CWM_OS_dbgPrintf("acc: x=%.3f, y=%.3f, z=%.3f\n",pSdo->acc_data[0], pSdo->acc_data[1], pSdo->acc_data[2]);
+            // // 保留原有原始数据打印
+            // CWM_OS_dbgPrintf("[WRITE_DATA] 原始数据 - x=%.3f,y=%.3f,z=%.3f\n",
+            //                  pSdo->acc_data[0], pSdo->acc_data[1], pSdo->acc_data[2]);
+            
+            // // 保留原有写入前状态打印
+            // CWM_OS_dbgPrintf("[WRITE_DATA] 写入前 - num_flag=%d, left_flag=%d, right_flag=%d\n",
+            //                  num_flag, left_flag, right_flag);
+
+            // 保留原有rawdata存储逻辑
             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(num_flag==GSENSON_MSG_COUNT)
+            // ========== 改造点1：统一指针边界校验，适配环形队列 ==========
+            if(num_flag == GSENSON_MSG_COUNT)
             {
-                left_flag++;
-                right_flag++;
-                right_flag=right_flag%GSENSON_MSG_COUNT;
-                left_flag=left_flag%GSENSON_MSG_COUNT;
+                // 优化：用%做边界校验，避免left_flag/right_flag越界（比如64→0）
+                left_flag = (left_flag + 1) % GSENSON_MSG_COUNT;
+                right_flag = (right_flag + 1) % GSENSON_MSG_COUNT;
                 gxyz_t_queue[right_flag].x= pSdo->acc_data[0];
                 gxyz_t_queue[right_flag].y= pSdo->acc_data[1];
                 gxyz_t_queue[right_flag].z= pSdo->acc_data[2];
+
+                // // 保留原有队列满打印
+                // CWM_OS_dbgPrintf("[WRITE_DATA] 队列满 - 写入right_flag=%d, left_flag同步至%d\n",
+                //                  right_flag, left_flag);
             }    
             else 
             {
-                //left_flag++;
-                right_flag++;
-                right_flag=right_flag%GSENSON_MSG_COUNT;
-                //left_flag=left_flag%16;
+                // 优化：用%做边界校验，统一指针操作逻辑
+                right_flag = (right_flag + 1) % GSENSON_MSG_COUNT;
                 num_flag++;
                 gxyz_t_queue[right_flag].x= pSdo->acc_data[0];
                 gxyz_t_queue[right_flag].y= pSdo->acc_data[1];
                 gxyz_t_queue[right_flag].z= pSdo->acc_data[2];
-                //CWM_OS_dbgPrintf("write: x=%d, y=%d, z=%d\n", gxyz_t_queue[right_flag].x ,gxyz_t_queue[right_flag].y ,gxyz_t_queue[right_flag].z);
-            
+
+                // 保留原有队列未满打印
+                // CWM_OS_dbgPrintf("[WRITE_DATA] 队列未满 - 写入right_flag=%d, num_flag=%d\n",
+                //                  right_flag, num_flag);
             }
             
+            // ========== 改造点2：新增环形绕回日志，便于排查错位问题 ==========
+            // if(right_flag < left_flag) {
+            //     CWM_OS_dbgPrintf("[WRITE_DATA] 环形绕回 - right_flag=%d < left_flag=%d, 新数据积压在队列头部\n",
+            //                      right_flag, left_flag);
+            // }
 
+            // // 保留原有写入后状态打印
+            // CWM_OS_dbgPrintf("[WRITE_DATA] 写入后 - num_flag=%d, left_flag=%d, right_flag=%d\n",
+            //                  num_flag, left_flag, right_flag);
+            
+            // 保留原有AccdataCount自增逻辑
             AccdataCount++;
         }     
-    }     
-}
+        // if(g_mutex.mutex != NULL)
+            osal_mutex_unlock(&g_mutex);
+    }    
+    
 
+}
 #endif
 
 int cywee_start(void) 
@@ -2294,6 +2369,11 @@ int cywee_start(void)
     cywee_oncharging_update(get_charge_stat()); //初始化时检查充电状态
 
     g_sport_timer = osTimerNew((osTimerFunc_t)cywee_service_check_timer_callback, osTimerPeriodic, NULL, NULL);
+
+    int ret = osal_mutex_init(&g_mutex);
+    if (ret != 0) {
+        printf("osal_mutex_init g_mutex failed\n");
+    }
     //cywee_inactivity_mode_change(INACTIVITY_MODE_SEDENTARY_AND_NAP);
 
     //cywee_inactivity_mode_change(INACTIVITY_MODE_SLEEPING);

diff --git a/code/sdk/zhihui/drivers/ppg/hx3695/hx3695.c b/code/sdk/zhihui/drivers/ppg/hx3695/hx3695.c
index 9fe4874..4b9cf59 100644 (file)
--- a/code/sdk/zhihui/drivers/ppg/hx3695/hx3695.c
+++ b/code/sdk/zhihui/drivers/ppg/hx3695/hx3695.c
@@ -426,6 +426,11 @@ uint16_t hx3695_read_fifo_data(ppg_sensor_data_t *ppg_sensor_dat)
 \r
     // printf("fifo_out_count:%d\n",fifo_out_count);\r
 \r
+    if(fifo_out_count > 9)\r
+    {\r
+        fifo_out_count = 9;\r
+    }\r
+\r
     return fifo_out_count;\r
 }\r
 \r
@@ -1622,8 +1627,8 @@ void hx3695_ppg_Int_handle(ppg_sensor_data_t *ppg_sensor_dat)
 \r
        // for(ii=0;ii<count;ii++)\r
        // {\r
-       //      printf("%d/%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d\r\n", ii+1, count, ppg_sensor_dat->led_data[0].data_val[ii], ppg_sensor_dat->led_data[1].data_val[ii],ppg_sensor_dat->led_data[2].data_val[ii], ppg_sensor_dat->led_data[3].data_val[ii],  \\r
-       //      ppg_sensor_dat->led_data[0].led_current, ppg_sensor_dat->led_data[0].offset_idac, ppg_sensor_dat->led_data[1].led_current, ppg_sensor_dat->led_data[1].offset_idac,ppg_sensor_dat->led_data[2].led_current, ppg_sensor_dat->led_data[2].offset_idac);\r
+       //      printf("%d/%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d,%d,%d\r\n", ii+1, count, ppg_sensor_dat->led_data[0].data_val[ii], ppg_sensor_dat->led_data[1].data_val[ii],ppg_sensor_dat->led_data[2].data_val[ii], ppg_sensor_dat->led_data[3].data_val[ii],  \\r
+       //      ppg_sensor_dat->led_data[0].led_current, ppg_sensor_dat->led_data[0].offset_idac, ppg_sensor_dat->led_data[1].led_current, ppg_sensor_dat->led_data[1].offset_idac,ppg_sensor_dat->led_data[2].led_current, ppg_sensor_dat->led_data[2].offset_idac,gsen_data_x_fifo[ii], gsen_data_y_fifo[ii], gsen_data_z_fifo[ii]);\r
        // }\r
 }\r
 \r

diff --git a/code/sdk/zhihui/drivers/ppg/service/ppg_service.cpp b/code/sdk/zhihui/drivers/ppg/service/ppg_service.cpp
index 07c93b3..cf3beb6 100644 (file)
--- a/code/sdk/zhihui/drivers/ppg/service/ppg_service.cpp
+++ b/code/sdk/zhihui/drivers/ppg/service/ppg_service.cpp
@@ -229,7 +229,7 @@ errcode_t uapi_ppg_service_init(void)
     taskcfg.create_queue_handle = create_ppg_queue;
     taskcfg.attr.name="ppg_service";
 
-    taskcfg.attr.priority = osPriorityAboveNormal3;
+    taskcfg.attr.priority = osPriorityAboveNormal2;
 
     taskcfg.attr.stack_size = PPG_SERVICE_TASK_SIZE;
     taskcfg.func = ppg_service_check_task;