优化运动切换函数

diff --git a/code/sdk/drivers/peripheral/cywee/src/main_dml.c b/code/sdk/drivers/peripheral/cywee/src/main_dml.c
index d88e091..6e3d96a 100644 (file)
--- a/code/sdk/drivers/peripheral/cywee/src/main_dml.c
+++ b/code/sdk/drivers/peripheral/cywee/src/main_dml.c
@@ -532,154 +532,185 @@ void CWM_AP_dmlSensorDataOutput_listen(const dml_sensorData_t * pSdo) {
 
 int cywee_main_dml(int cmd) {
 
-    if (cywee_installized==1)
+    if (cywee_installized==0)
     {
-        CWM_OS_dbgPrintf("cywee_installized skip install\n");
-        return 0;
-
-    }
-    cywee_installized = 1;
-    int i;
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    CWM_SettingControl(SCL_GET_LIB_INFO, &scl);
-    CWM_OS_dbgPrintf("version:%d.%d.%d.%d product:%d model:%d\n", scl.iData[1], scl.iData[2], scl.iData[3], scl.iData[4], scl.iData[5], scl.iData[6]);
-    CWM_OS_dbgPrintf("[DBG] ach: %d\n", scl.iData[7]);
-#if USE_P813
-    os_api api = {
-        .GetTimeNs    = CWM_OS_GetTimeNs,
-        .dbgOutput    = CWM_OS_dbgOutput,
-        .i2cRead      = CWM_OS_i2cRead,
-        .i2cWrite     = CWM_OS_i2cWrite,
-        .uSleep       = CWM_OS_uSleep,
-        .free         = CWM_OS_free,
-        .malloc       = CWM_OS_malloc,
-        .spiRead      = NULL,
-        .spiWrite      = NULL,
-    };
-    CWM_LibPreInit(&api);
-
-#else
-    OsAPI apiOS = {
-        .GetTimeNs    = CWM_OS_GetTimeNs,
-        .dbgOutput    = CWM_OS_dbgOutput,
-        .i2cRead      = CWM_OS_i2cRead,
-        .i2cWrite     = CWM_OS_i2cWrite,
-        .uSleep       = CWM_OS_uSleep,
-        .free         = CWM_OS_free,
-        .malloc       = CWM_OS_malloc,
-        .i2cTransfer  = CWM_OS_i2cTransfer,
-        .restoreData  = CWM_OS_restoreData,
-        .saveData     = CWM_OS_saveData,
-        .spiRead      = NULL,
-        .spiWrite     = NULL,
-        .securityCodeWrite      = NULL,
-        .setGpio      = NULL,
-        .enableGpioIRQ      = NULL,
-        .disableGpioIRQ      = NULL,
-    };
-    CWM_LibPreInit(&apiOS);
-#endif
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    //set mcu_chip information, must call this before CWM_LibPostInit()
-
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[1] = 2; // 0: mcu_auto_detect 2: skip_mcu_auto_detect
-    CWM_SettingControl(SCL_CHIP_VENDOR_CONFIG, &scl);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    CWM_LibPostInit(CWM_AP_SensorListen);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    //get chip information
-
-    char chipInfo[64];
-
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[1] = 1;
-    scl.iData[2] = (int)chipInfo;
-    scl.iData[3] = sizeof(chipInfo);
-    scl.iData[4] = 0;
-    scl.iData[5] = 0;
-    scl.iData[6] = 0;
-    CWM_SettingControl(SCL_GET_CHIP_INFO, &scl);
-    CWM_OS_dbgPrintf("have_security = %d.%d ret_buff_size = %d  chipInfo = %s\n", scl.iData[5], scl.iData[6], scl.iData[4], chipInfo);
-    CWM_OS_dbgPrintf("chip_settings = %d, %d, %d\n", scl.iData[9], scl.iData[10], scl.iData[11]);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    CWM_Dml_LibInit();
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    #ifdef AP_ENABLE_DML_SDO_LISTEN
-    // This API allows customers to develop custom features.
-    // Specific settingControl commands needs to be called for this API to work.
-    // No need to enable or implement this function for general usage.
-    CWM_Dml_sensorDataOutput_register(CWM_AP_dmlSensorDataOutput_listen);
+        int i;
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        CWM_SettingControl(SCL_GET_LIB_INFO, &scl);
+        CWM_OS_dbgPrintf("version:%d.%d.%d.%d product:%d model:%d\n", scl.iData[1], scl.iData[2], scl.iData[3], scl.iData[4], scl.iData[5], scl.iData[6]);
+        CWM_OS_dbgPrintf("[DBG] ach: %d\n", scl.iData[7]);
+    #if USE_P813
+        os_api api = {
+            .GetTimeNs    = CWM_OS_GetTimeNs,
+            .dbgOutput    = CWM_OS_dbgOutput,
+            .i2cRead      = CWM_OS_i2cRead,
+            .i2cWrite     = CWM_OS_i2cWrite,
+            .uSleep       = CWM_OS_uSleep,
+            .free         = CWM_OS_free,
+            .malloc       = CWM_OS_malloc,
+            .spiRead      = NULL,
+            .spiWrite      = NULL,
+        };
+        CWM_LibPreInit(&api);
+
+    #else
+        OsAPI apiOS = {
+            .GetTimeNs    = CWM_OS_GetTimeNs,
+            .dbgOutput    = CWM_OS_dbgOutput,
+            .i2cRead      = CWM_OS_i2cRead,
+            .i2cWrite     = CWM_OS_i2cWrite,
+            .uSleep       = CWM_OS_uSleep,
+            .free         = CWM_OS_free,
+            .malloc       = CWM_OS_malloc,
+            .i2cTransfer  = CWM_OS_i2cTransfer,
+            .restoreData  = CWM_OS_restoreData,
+            .saveData     = CWM_OS_saveData,
+            .spiRead      = NULL,
+            .spiWrite     = NULL,
+            .securityCodeWrite      = NULL,
+            .setGpio      = NULL,
+            .enableGpioIRQ      = NULL,
+            .disableGpioIRQ      = NULL,
+        };
+        CWM_LibPreInit(&apiOS);
     #endif
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    CWM_SettingControl(SCL_DML_GET_LIB_INFO, &scl);
-    CWM_OS_dbgPrintf("dml version:%d.%d.%d.%d product:%d model:%d\n", scl.iData[1], scl.iData[2], scl.iData[3], scl.iData[4], scl.iData[5], scl.iData[6]);
-
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[1] = 18; //00000000000000000000000000010010  高位在前
-    // CWM_SettingControl(SCL_DML_DEBUG, &scl);
-
-    //设置acc gyro 相关参数
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[3] = 2;
-    scl.iData[4] = 2; //1 anytime 2 fifo
-    scl.iData[6] = 5; //先設定5
-    scl.iData[7] = 5;
-    scl.iData[8] = 52; // for st sensor(52Hz)
-    CWM_SettingControl(SCL_DML_DRV_AG_CONFIG, &scl);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    //设定搜寻哪种型号的传感器，以及要搜寻的属性
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[1] = 2;
-    scl.iData[6] = 3002502;
-    scl.iData[7] = (0|1|8|256|1024);
-    CWM_SettingControl(SCL_DML_DRV_HW_CONFIG, &scl);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    //搜寻是否接入传感器
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[1] = 1;
-    CWM_SettingControl(SCL_DML_DRV_INIT, &scl); 
-    CWM_OS_dbgPrintf("DML device=%d\n", scl.iData[2]);
-
-    //获取初始化 driver清单
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    CWM_SettingControl(SCL_DML_GET_INITED_LIST, &scl);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        //set mcu_chip information, must call this before CWM_LibPostInit()
+
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[1] = 2; // 0: mcu_auto_detect 2: skip_mcu_auto_detect
+        CWM_SettingControl(SCL_CHIP_VENDOR_CONFIG, &scl);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        CWM_LibPostInit(CWM_AP_SensorListen);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        //get chip information
+
+        char chipInfo[64];
+
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[1] = 1;
+        scl.iData[2] = (int)chipInfo;
+        scl.iData[3] = sizeof(chipInfo);
+        scl.iData[4] = 0;
+        scl.iData[5] = 0;
+        scl.iData[6] = 0;
+        CWM_SettingControl(SCL_GET_CHIP_INFO, &scl);
+        CWM_OS_dbgPrintf("have_security = %d.%d ret_buff_size = %d  chipInfo = %s\n", scl.iData[5], scl.iData[6], scl.iData[4], chipInfo);
+        CWM_OS_dbgPrintf("chip_settings = %d, %d, %d\n", scl.iData[9], scl.iData[10], scl.iData[11]);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        CWM_Dml_LibInit();
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        #ifdef AP_ENABLE_DML_SDO_LISTEN
+        // This API allows customers to develop custom features.
+        // Specific settingControl commands needs to be called for this API to work.
+        // No need to enable or implement this function for general usage.
+        CWM_Dml_sensorDataOutput_register(CWM_AP_dmlSensorDataOutput_listen);
+        #endif
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        CWM_SettingControl(SCL_DML_GET_LIB_INFO, &scl);
+        CWM_OS_dbgPrintf("dml version:%d.%d.%d.%d product:%d model:%d\n", scl.iData[1], scl.iData[2], scl.iData[3], scl.iData[4], scl.iData[5], scl.iData[6]);
+
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[1] = 18; //00000000000000000000000000010010  高位在前
+        // CWM_SettingControl(SCL_DML_DEBUG, &scl);
+
+        //设置acc gyro 相关参数
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[3] = 2;
+        scl.iData[4] = 2; //1 anytime 2 fifo
+        scl.iData[6] = 5; //先設定5
+        scl.iData[7] = 5;
+        scl.iData[8] = 52; // for st sensor(52Hz)
+        CWM_SettingControl(SCL_DML_DRV_AG_CONFIG, &scl);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        //设定搜寻哪种型号的传感器，以及要搜寻的属性
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[1] = 2;
+        scl.iData[6] = 3002502;
+        scl.iData[7] = (0|1|8|256|1024);
+        CWM_SettingControl(SCL_DML_DRV_HW_CONFIG, &scl);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        //搜寻是否接入传感器
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[1] = 1;
+        CWM_SettingControl(SCL_DML_DRV_INIT, &scl); 
+        CWM_OS_dbgPrintf("DML device=%d\n", scl.iData[2]);
+
+        //获取初始化 driver清单
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        CWM_SettingControl(SCL_DML_GET_INITED_LIST, &scl);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        for (i = 0; i < scl.iData[2]; i ++) {
+
+            CWM_OS_dbgPrintf("DML device[%d]: hw_id=%d hw_attr=%d\n", i, scl.iData[6 + i * 2], scl.iData[7 + i * 2]);
+        }
 
-    for (i = 0; i < scl.iData[2]; i ++) {
+        //设置抬腕功能
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        // scl.iData[1] = 5;
+        // scl.iData[2] = 5;
+        // scl.iData[3] = 5;
+        // scl.iData[4] = 5;
+        CWM_SettingControl(SCL_HAND_UPDOWN_CONFIG, &scl);
+        
+        //呼叫任一個CWM_Sensor_Enable 前呼叫此指令 设置手动或自动模式
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[1] = 1;
+        // int[2] = 3，代表開啟acc和gyro感測器  1代表开启acc
+        scl.iData[2] = 1;
+        CWM_SettingControl(SCL_DML_DRV_ENABLE, &scl);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+
+        memset(&scl, 0, sizeof(scl));
+        scl.iData[0] = 1;
+        scl.iData[3] = 3;
+        scl.iData[4] = 5;
+        // CWM_SettingControl(SCL_LOG, &scl);
+    
+        
+        CWM_Sensor_Enable(IDX_REQUEST_SENSOR);
+        // //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+        CWM_Sensor_Enable(IDX_ALGO_INACTIVITY_OUTPUT);
+        
+        CWM_Sensor_Enable(IDX_ALGO_ACTIVITY_OUTPUT);
+
+        CWM_Sensor_Enable(IDX_ACCEL);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+        // CWM_Sensor_Enable(IDX_GYRO);
+        CWM_Sensor_Enable(IDX_ALGO_NO_MOTION);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+        // CWM_Sensor_Enable(IDX_ALGO_ANY_MOTIOIN);
+        CWM_Sensor_Enable(IDX_ALGO_ABSOLUTE_STATIC);
+        //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
+        CWM_Sensor_Enable(IDX_ALGO_WATCH_HANDUP);
 
-        CWM_OS_dbgPrintf("DML device[%d]: hw_id=%d hw_attr=%d\n", i, scl.iData[6 + i * 2], scl.iData[7 + i * 2]);
     }
-
-    //设置抬腕功能
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    // scl.iData[1] = 5;
-    // scl.iData[2] = 5;
-    // scl.iData[3] = 5;
-    // scl.iData[4] = 5;
-    CWM_SettingControl(SCL_HAND_UPDOWN_CONFIG, &scl);
-
+    cywee_installized = 1;
+    if(cywee_installized == 1)
+        CWM_OS_dbgPrintf("cywee_installized skip install\n");
 /*
 int[0] = 1
 int[1] = floor_Height (cm)(設定樓層高度)(default: 380cm = 3.8m)
@@ -857,49 +888,17 @@ int[5] = release_mode (解除久坐的算法模式)(default: 行走模式)
     memset(&scl, 0, sizeof(scl));
     scl.iData[0] = 1;
     scl.iData[2] = 1;
-    // int[2] = 3，代表開啟acc和gyro感測器  1代表开启acc
     scl.iData[3] = 1;
     scl.iData[4] = 3;
     scl.iData[5] = 1;
     CWM_SettingControl(SCL_SEDENTARY, &scl);
 
 
-    //呼叫任一個CWM_Sensor_Enable 前呼叫此指令 设置手动或自动模式
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[1] = 1;
-    // int[2] = 3，代表開啟acc和gyro感測器  1代表开启acc
-    scl.iData[2] = 1;
-    CWM_SettingControl(SCL_DML_DRV_ENABLE, &scl);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-
-    memset(&scl, 0, sizeof(scl));
-    scl.iData[0] = 1;
-    scl.iData[3] = 3;
-    scl.iData[4] = 5;
-    // CWM_SettingControl(SCL_LOG, &scl);
-    
 
     g_cywee_hrs_timer = osTimerNew((osTimerFunc_t)cywee_hrs_check_timer_callback, osTimerPeriodic, NULL, NULL);
     //g_cywee_wearstatus_timer = osTimerNew((osTimerFunc_t)cywee_wearstatus_check_timer_callback, osTimerPeriodic, NULL, NULL);
     g_cywee_oncharging_timer = osTimerNew((osTimerFunc_t)cywee_oncharging_check_timer_callback, osTimerPeriodic, NULL, NULL);
 
-    CWM_Sensor_Enable(IDX_REQUEST_SENSOR);
-    // //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-    CWM_Sensor_Enable(IDX_ALGO_INACTIVITY_OUTPUT);
-    
-    CWM_Sensor_Enable(IDX_ALGO_ACTIVITY_OUTPUT);
-
-    CWM_Sensor_Enable(IDX_ACCEL);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-    // CWM_Sensor_Enable(IDX_GYRO);
-    CWM_Sensor_Enable(IDX_ALGO_NO_MOTION);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-    // CWM_Sensor_Enable(IDX_ALGO_ANY_MOTIOIN);
-    CWM_Sensor_Enable(IDX_ALGO_ABSOLUTE_STATIC);
-    //CWM_OS_dbgPrintf("[DBG] %s %d\n", __func__,__LINE__);
-    CWM_Sensor_Enable(IDX_ALGO_WATCH_HANDUP);
-
 
     cyw_mode_init(cmd);