From: SnickerSun <snickersun@163.com>
Date: Mon, 22 Jan 2024 16:12:47 +0000 (+0800)
Subject: 指南针timer稳定性问题解决，数据还不准
X-Git-Tag: release_V1.2.70_20241020_135402~1345
X-Git-Url: http://git.snicker.com.cn/Users/apple/Desktop/static/gitweb.css?a=commitdiff_plain;h=2f450f48c5968c65db87e541b38714cc5ce49c77;p=mirrors%2FFD230.git

指南针timer稳定性问题解决，数据还不准
---

diff --git a/code/sdk/drivers/peripheral/acc/lis2doc_STdC/examples/lis2doc_read_data_single.c b/code/sdk/drivers/peripheral/acc/lis2doc_STdC/examples/lis2doc_read_data_single.c
index e21c0500e..42a01a816 100644
--- a/code/sdk/drivers/peripheral/acc/lis2doc_STdC/examples/lis2doc_read_data_single.c
+++ b/code/sdk/drivers/peripheral/acc/lis2doc_STdC/examples/lis2doc_read_data_single.c
@@ -165,6 +165,10 @@ int lis2doc_read_data_single_init(void)
   pm.low_noise = 1;
   lis2doc_power_mode_set(&dev_ctx, pm);
 
+  //lis2doc_power_mode_set(&dev_ctx, LIS2DOC_CONT_LOW_PWR_LOW_NOISE_12bit);
+  /* Set Output Data Rate */
+  // lis2doc_data_rate_set(&dev_ctx, LIS2DOC_XL_ODR_25Hz);
+  
   lis2doc_data_rate_set(&dev_ctx, LIS2DOC_XL_SET_SW_TRIG);
   return ERRCODE_SUCC;
 }
@@ -187,7 +191,7 @@ int lis2doc_read_data_single(float *x,float *y ,float *z)
     uint8_t val = uapi_gpio_get_val(G_INT1_PIN);
     uint8_t val2 = uapi_gpio_get_val(G_INT2_PIN);
     // PRINT("G_INT1_PIN %u,G_INT2_PIN %u \r\n",val,val2);
-    if( val == 0 || val2 == 0)
+    // if( val == 0 || val2 == 0)
     {
       /* Read acceleration data */
       memset(data_raw_acceleration, 0x00, 3 * sizeof(int16_t));
diff --git a/code/sdk/drivers/peripheral/mag/CMakeLists.txt b/code/sdk/drivers/peripheral/mag/CMakeLists.txt
index 0ef2a2753..ff0f26223 100644
--- a/code/sdk/drivers/peripheral/mag/CMakeLists.txt
+++ b/code/sdk/drivers/peripheral/mag/CMakeLists.txt
@@ -7,7 +7,7 @@ set(COMPONENT_NAME "mag_sensor")
 
 if("MAG_MMC5603" IN_LIST DEFINES)
 set(SOURCES ${SOURCES}
-    ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/Customer.c
+    ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/mmc_customer.c
     ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/mmc5603_main.c
     ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/MMC5603.c
     ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/mmc5603_at_cmd.c
@@ -23,7 +23,7 @@ set(PUBLIC_DEFINES
 )
 else()
 set(SOURCES ${SOURCES}
-    ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/Customer.c
+    ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/mmc_customer.c
     ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/mmc5603_main.c
     ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/MMC5603.c
     ${CMAKE_CURRENT_SOURCE_DIR}/mmc5603/mmc5603_at_cmd.c
diff --git a/code/sdk/drivers/peripheral/mag/mmc5603/MMC5603.c b/code/sdk/drivers/peripheral/mag/mmc5603/MMC5603.c
index 8b5ab4163..900373136 100644
--- a/code/sdk/drivers/peripheral/mag/mmc5603/MMC5603.c
+++ b/code/sdk/drivers/peripheral/mag/mmc5603/MMC5603.c
@@ -15,8 +15,7 @@
  */
 
 #include "MMC5603.h"
-#include "Customer.h"
-
+#include "mmc_customer.h"
 #include "math.h"
 
 /* Indicate working mode of sensor */
@@ -450,10 +449,10 @@ void MMC5603_GetData(float *mag_out)
 	mag_out[1] = ((float)data_temp[1] - MMC5603_16BIT_OFFSET)/MMC5603_16BIT_SENSITIVITY;
 	mag_out[2] = ((float)data_temp[2] - MMC5603_16BIT_OFFSET)/MMC5603_16BIT_SENSITIVITY;
 
-	mag_out[0] = get_filter_data_x(mag_out[0]);
-	mag_out[1] = get_filter_data_y(mag_out[1]);
-	mag_out[2] = get_filter_data_z(mag_out[2]);
-	filter_index_plus();
+	// mag_out[0] = get_filter_data_x(mag_out[0]);
+	// mag_out[1] = get_filter_data_y(mag_out[1]);
+	// mag_out[2] = get_filter_data_z(mag_out[2]);
+	// filter_index_plus();
 	
 	MMC5603_Auto_Switch(data_temp);	
 			
diff --git a/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_at_cmd.c b/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_at_cmd.c
index 869c7c2ca..7f368b42d 100644
--- a/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_at_cmd.c
+++ b/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_at_cmd.c
@@ -5,7 +5,7 @@
 #include "at_cmd_api.h"
 #include "pm_control.h"
 #include "MMC5603.h"
-#include "Customer.h"
+#include "mmc_customer.h"
 #include "mag_service.h"
 
 #define ARRAY_COUNT(array) (sizeof(array) / sizeof(array[0]))
diff --git a/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_main.c b/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_main.c
index 7f3aa4fd0..50a23eae7 100644
--- a/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_main.c
+++ b/code/sdk/drivers/peripheral/mag/mmc5603/mmc5603_main.c
@@ -8,12 +8,15 @@
  
 #include "MemsicAlgo.h"
 #include "MemsicCompass.h"
-#include "Customer.h"
+#include "mmc_customer.h"
 #include "MMC5603.h" 
 
 #ifdef ACC_LIS2DOC
 #include "acc_service.h"
 #endif
+
+#include "time.h"
+
 float magnetic_field_x;
 float magnetic_field_y;
 float magnetic_field_z;
@@ -111,6 +114,12 @@ extern uint8_t is_xts_test_done(void);
 		return 0;
 #endif
 	int i;
+   	static uint32_t MILLISEC_TO_NANOSEC = 1000000;
+    static uint16_t SEC_TO_MILLISEC = 1000;
+	struct timespec time;
+    clock_gettime(CLOCK_MONOTONIC, &time);
+	uint32_t _startTimestamp = (uint32_t ) time.tv_sec * SEC_TO_MILLISEC + time.tv_nsec / MILLISEC_TO_NANOSEC;
+	
 	
 	/* This variable is used to store the accelerometer and magnetometer raw data.
 	 * please NOTE that the data is going to store here MUST has been transmitted 
@@ -147,12 +156,19 @@ extern uint8_t is_xts_test_done(void);
 	float caliMagPara[4] = {0.0f};	
 	/* convert the coordinate system */		
 	acc_coordinate_raw_to_real(A_LAYOUT, acc_raw_data, acc_real_data);			
-	
+	MMC_LOG("acc_coordinate_raw_to_real raw(%.02f,%.02f,%.02f) , real(%.02f,%.02f,%.02f)\r\n",
+	acc_raw_data[0],acc_raw_data[1],acc_raw_data[2],
+	acc_real_data[0],acc_real_data[1],acc_real_data[2]
+	);	
 	/* Get the MMC5603 mag raw data, unit is gauss */
 	MMC5603_GetData(mag_raw_data);
 
 	/* Convert the coordinate system */		
-	mag_coordinate_raw_to_real(M_LAYOUT, mag_raw_data, mag_real_data);			
+	mag_coordinate_raw_to_real(M_LAYOUT, mag_raw_data, mag_real_data);		
+	MMC_LOG("mag_coordinate_raw_to_real raw(%.02f,%.02f,%.02f) , real(%.02f,%.02f,%.02f)\r\n",
+	mag_raw_data[0],mag_raw_data[1],mag_raw_data[2],
+	mag_real_data[0],mag_real_data[1],mag_real_data[2]
+	);	
 
 	/* Below functions are algorithm interface. 
 		* input acc, mag data into algorithm
@@ -181,21 +197,33 @@ extern uint8_t is_xts_test_done(void);
 			fPitch    = caliOri[1];
 			fRoll     = caliOri[2];	
 			if(iAccuracy == 3){
-				PRINT("== Mag x %.02F,y %.02F , z %.02F\r\n",
+				MMC_LOG("== Mag x %.02F,y %.02F , z %.02F\r\n",
 				calMagX,//高斯X
 				calMagY,//高斯Y
 				calMagZ//高斯Z
 				);
-				PRINT("iAccuracy %d== Acc Azimuth %.02F,Pitch %.02F , Roll %.02F\r\n", 
+				MMC_LOG("iAccuracy %d== Acc Azimuth %.02F,Pitch %.02F , Roll %.02F\r\n", 
 				iAccuracy,//可信度
 				fAzimuth, //水平方向角，左转右转
 				fPitch,//上下俯仰角
 				fRoll//左右翻滚角
 				);
 			}else{
-				PRINT("iAccuracy %d < 3 skip\r\n", 
+				MMC_LOG("--iAccuracy %d < 3 skip\r\n", 
 				iAccuracy   //可信度
 				);
+
+				MMC_LOG("--== Mag x %.02F,y %.02F , z %.02F\r\n",
+				calMagX,//高斯X
+				calMagY,//高斯Y
+				calMagZ//高斯Z
+				);
+				MMC_LOG("--iAccuracy %d== Acc Azimuth %.02F,Pitch %.02F , Roll %.02F\r\n", 
+				iAccuracy,//可信度
+				fAzimuth, //水平方向角，左转右转
+				fPitch,//上下俯仰角
+				fRoll//左右翻滚角
+				);
 			
 			}
 	}
@@ -205,7 +233,7 @@ extern uint8_t is_xts_test_done(void);
 	if(GetMagSaturation())  
 	{
 		//MEMSIC_Magnetic_Sensor_SET();	//Do SET action
-		PRINT("Do SET action MMC5603_SET\r\n");
+		MMC_LOG("Do SET action MMC5603_SET\r\n");
 		MMC5603_SET();
 	}
 	
@@ -222,12 +250,16 @@ extern uint8_t is_xts_test_done(void);
 		/* Save the calpara buffer into the system file */
 		/* This function should be create by customer.If customer can not implement this function, should comment it out */	
 		SavePara(saveData);								
-		PRINT("save para \r\n");	
+		MMC_LOG("save para \r\n");	
 	}
 
 	/* Sampling interval is 20ms. */
 	// mmc5603_Delay_Ms(20);
+
+	clock_gettime(CLOCK_MONOTONIC, &time);
+	uint32_t _endTimestamp = (uint32_t ) time.tv_sec * SEC_TO_MILLISEC + time.tv_nsec / MILLISEC_TO_NANOSEC;
 	
+	MMC_LOG("mmc5603_check  cost %u\r\n",(_endTimestamp-_startTimestamp));
 	return 0;
 }
 
@@ -276,7 +308,7 @@ void acc_coordinate_raw_to_real(int layout, float *in, float *out)
 {
 	switch (layout) {
 	case 0:
-		out[0] =  in[0];
+		out[0] =  -in[0];
 		out[1] =  in[1];
 		out[2] =  in[2];
 		break;
@@ -354,7 +386,7 @@ void mag_coordinate_raw_to_real(int layout, float *in, float *out)
 		break;
 	case 5:
 		out[0] = -in[0];
-		out[1] =  in[1];
+		out[1] = -in[1];
 		out[2] = -in[2];
 		break;
 	case 6:
diff --git a/code/sdk/drivers/peripheral/mag/mmc5603/Customer.c b/code/sdk/drivers/peripheral/mag/mmc5603/mmc_customer.c
similarity index 95%
rename from code/sdk/drivers/peripheral/mag/mmc5603/Customer.c
rename to code/sdk/drivers/peripheral/mag/mmc5603/mmc_customer.c
index 3fb542e82..a3cdb761b 100644
--- a/code/sdk/drivers/peripheral/mag/mmc5603/Customer.c
+++ b/code/sdk/drivers/peripheral/mag/mmc5603/mmc_customer.c
@@ -1,4 +1,4 @@
-#include "Customer.h"
+#include "mmc_customer.h"
 #include "osal_timer.h"
 #include "osal_mutex.h"
 #include "pinctrl.h"
diff --git a/code/sdk/drivers/peripheral/mag/mmc5603/Customer.h b/code/sdk/drivers/peripheral/mag/mmc5603/mmc_customer.h
similarity index 93%
rename from code/sdk/drivers/peripheral/mag/mmc5603/Customer.h
rename to code/sdk/drivers/peripheral/mag/mmc5603/mmc_customer.h
index 585668424..74a3e9d76 100644
--- a/code/sdk/drivers/peripheral/mag/mmc5603/Customer.h
+++ b/code/sdk/drivers/peripheral/mag/mmc5603/mmc_customer.h
@@ -4,7 +4,10 @@
 #include <stdio.h>
 #include "debug_print.h"
 
+
+#ifndef XTS_SUPPORT
 #define MMC_DEBUG
+#endif 
 
 #ifdef MMC_DEBUG
 #define MMC_LOG(fmt, ...)                    \
diff --git a/code/sdk/drivers/peripheral/mag/service/mag_service.c b/code/sdk/drivers/peripheral/mag/service/mag_service.c
index dd008efca..0ca7728d7 100644
--- a/code/sdk/drivers/peripheral/mag/service/mag_service.c
+++ b/code/sdk/drivers/peripheral/mag/service/mag_service.c
@@ -69,6 +69,7 @@ static bool g_batter_service_initialled = false;
 #include "mmc5603_hal.h"
 #include "mag_service.h"
 
+#include "mmc_customer.h"
 
 /***
  * 其他cmd在Header里定义，对外开放
@@ -78,7 +79,7 @@ static bool g_batter_service_initialled = false;
 osTimerId_t g_mag_service_timer = NULL;
 
 static bool g_bat_timer_is_running = false;
-#define MAG_CHECK_MS 200
+#define MAG_CHECK_MS 20
 static unsigned long g_app_queue;
 
 static bool mag_service_running_stat = false;
@@ -131,7 +132,7 @@ void do_mag_service_check_msg(int cmd)
     ret = osal_msg_queue_write_copy(g_app_queue, (void *)&mail,
                                     sizeof(common_notify_mail_t), 0);
     if (ret != OSAL_SUCCESS) {
-        PRINT("Error mag Check MSG 0x%02X send failed! err=0x%x! \r\n",cmd, ret);
+        MMC_ERROR("Error mag Check MSG 0x%02X send failed! err=0x%x! \r\n",cmd, ret);
     }
     
 }
@@ -153,15 +154,20 @@ static void mag_service_check_task(void const *arg)
         if (mail.comm.app == MAG_SERVICE_CMD_TIMER_CHECK) {
             // mag_service_check_and_enable_charge();
             // mmc5603_check();
-            PRINT("MAG_SERVICE_CMD_TIMER_CHECK\r\n");
+            MMC_LOG("MAG_SERVICE_CMD_TIMER_CHECK\r\n");
         }else if (mail.comm.app == MAG_SERVICE_CMD_START_CHECK){
-            mag_service_running_stat = true;
             mmc5603_start();
+            uapi_tcxo_delay_ms(20);
+            mag_service_running_stat = true;
+            // uapi_pm_add_sleep_veto(PM_ID_SCREEN);
             mag_service_timer_start_check(MAG_CHECK_MS);
         }else if (mail.comm.app == MAG_SERVICE_CMD_STOP_CHECK){
             mag_service_running_stat = false;
             osTimerStop(g_mag_service_timer);
             mmc5603_stop();
+            uapi_tcxo_delay_ms(20);
+
+            // uapi_pm_remove_sleep_veto(PM_ID_SCREEN);
         }
     }
 }
@@ -170,7 +176,7 @@ errcode_t uapi_mag_service_init(void)
 {
     if (osal_msg_queue_create("mag_service_queue", APP_BOARD_MSG_COUNT, &g_app_queue,
                               0, sizeof(common_notify_mail_t)) != ERRCODE_SUCC) {
-        PRINT("[mag][service] app create mail fail\n");
+        MMC_ERROR("[mag][service] app create mail fail\n");
         return ERRCODE_FAIL;
     }
 
@@ -179,12 +185,12 @@ errcode_t uapi_mag_service_init(void)
                                                  "mag_service_service", APP_BOARD_TASK_SIZE);
     if (task_handle == NULL) {
         osal_msg_queue_is_full(g_app_queue);
-        PRINT("create mag service thread fail!\r\n");
+        MMC_ERROR("create mag service thread fail!\r\n");
         return ERRCODE_FAIL;
     }
 
-    // osal_kthread_set_priority(task_handle, APP_BOARD_TASK_PRIO);
-    osal_kthread_set_priority(task_handle, 10);
+    osal_kthread_set_priority(task_handle, APP_BOARD_TASK_PRIO);
+    // osal_kthread_set_priority(task_handle, 10);
 
     g_mag_service_timer = osTimerNew((osTimerFunc_t)mag_service_check_timer_callback, osTimerPeriodic, NULL, NULL);
   
@@ -194,10 +200,10 @@ errcode_t uapi_mag_service_init(void)
 
 // void mag_service_service_resume(void)
 // {
-//     PRINT("mag_service_service resume!\r\n");
+//     MMC_LOG("mag_service_service resume!\r\n");
 //     mag_service_check_timer_callback(0);
 // }
 // void mag_service_service_suspend(void)
 // {
-//     PRINT("mag_service_service suspend!\r\n");
+//     MMC_LOG("mag_service_service suspend!\r\n");
 // }
\ No newline at end of file