修复读取角度，偶现不正常现象

Device/Imu.c

@@ -37,6 +37,7 @@
 #include "crc16.h"
 #include "uart.h"
 #include "string.h"
+#include <math.h>
 
 #define UARTx   UART2
 #define UARTx_IRQn  UART2_IRQn
@@ -56,7 +57,11 @@ static uint8_t init_finished = 0;
 static float roll_angle = 0;
 static float pitch_angle = 0;
 static float yaw_angle = 0;
-//static int16_t  tmp =0;
+
+static uint8_t imu_dataError_count = 0;
+
+
+//static int16_t  print_times =0;
 
 //static uint32_t printfBuff[16];
 //static uint16_t printfLen = 0;
@@ -76,6 +81,8 @@ static int TransmitData(uint8_t *pdata, uint16_t length);
 static void UartEventCallback(void *device, uint32_t wpara, uint32_t lpara)
 {
     int16_t tmp = 0;
+		uint8_t sum = 0;
+		uint8_t i =0;
     UART_Type *uart_Device = (UART_Type *)device;
     /*rx interrupt*/
     if ((uart_Device->IER & UART_IER_ERXNE_Msk) && (wpara & UART_LSR0_DR_Msk))
@@ -86,24 +93,50 @@ static void UartEventCallback(void *device, uint32_t wpara, uint32_t lpara)
         if (g_imuRxDataIndex >= 11)
         {
             g_imuRxDataIndex = 0;
+					
+						for( i=0; i<10; i++){
+						   sum += g_imuRxDataBuff[i];
+						}
+			
 						if(1 == init_finished){
 							
-							tmp = g_imuRxDataBuff[3];
-							tmp = (tmp<<8)|g_imuRxDataBuff[2];
-							roll_angle = 1.0*tmp/32768*180;
+							if((sum == g_imuRxDataBuff[10]) && (0x55 == g_imuRxDataBuff[0]) && (0x53 == g_imuRxDataBuff[1])){
+													
+								tmp = g_imuRxDataBuff[3];
+								tmp = (tmp<<8)|g_imuRxDataBuff[2];
+								roll_angle = 1.0*tmp/32768*180;
+
+								
+								tmp = g_imuRxDataBuff[5];
+								tmp = (tmp<<8)|g_imuRxDataBuff[4];
+								pitch_angle = 1.0*tmp/32768*180;
+
 							
-							tmp = g_imuRxDataBuff[5];
-							tmp = (tmp<<8)|g_imuRxDataBuff[4];
-							pitch_angle = 1.0*tmp/32768*180;
+								tmp = g_imuRxDataBuff[7];
+								tmp = (tmp<<8)|g_imuRxDataBuff[6];
+								yaw_angle = 1.0*tmp/32768*180;
+								
+								
+								imu_dataError_count = 0;	
+								
+								
+							}else{	
+								
+								imu_dataError_count++;
+								if(imu_dataError_count >= 5){
+									 imu_dataError_count = 3;
+									
+									roll_angle = -300;
+									pitch_angle = -300;
+									yaw_angle = -300;
+									
+								}
 							
-							tmp = g_imuRxDataBuff[7];
-							tmp = (tmp<<8)|g_imuRxDataBuff[6];
-							yaw_angle = 1.0*tmp/32768*180;
+							}
 							
-//							printfLen = snprintf((char*)printfBuff, 64, "Imu roll: %f, pitch:%f, yaw:%f \r\n", roll_angle, pitch_angle, yaw_angle);
-//							rs485_TransmitData((uint8_t*)printfBuff, printfLen);
 								
 						}
+						
         }
     }
 		
@@ -132,8 +165,8 @@ static void uart_Initialize(void)
     UART_ConfigType  uartConfig = {0};
     
     //set pin mux
-//    GPIO_SetFunc(GPIOB, GPIO_PIN9, GPIO_FUN3);
-//    GPIO_SetFunc(GPIOB, GPIO_PIN10, GPIO_FUN3);
+		//GPIO_SetFunc(GPIOB, GPIO_PIN9, GPIO_FUN3);
+		//GPIO_SetFunc(GPIOB, GPIO_PIN10, GPIO_FUN3);
     
     
     uartConfig.baudrate = 9600;
@@ -179,6 +212,8 @@ void Imu_Init(void)
 {
 		uint16_t crc;
 		uart_Initialize();
+		
+		mdelay(30);
 	
 		//设置参数
 		imucmd_buffer[0] = 0xFF;
@@ -192,8 +227,9 @@ void Imu_Init(void)
 		imucmd_buffer[5] = (uint8_t)((crc>>8) & 0x00ff);
 		imucmd_buffer[6] = (uint8_t)(crc & 0x00ff);
 		TransmitData(imucmd_buffer, 7);
+		mdelay(100);
 		
-		//step2  设置输出内容 
+		//step2  设置输出内容 ，仅输出角度
 		imucmd_buffer[2] = 0x02;
 		imucmd_buffer[3] = 0x08;
 		imucmd_buffer[4] = 0x00;
@@ -202,6 +238,7 @@ void Imu_Init(void)
 		imucmd_buffer[5] = (uint8_t)((crc>>8) & 0x00ff);
 		imucmd_buffer[6] = (uint8_t)(crc & 0x00ff);
 		TransmitData(imucmd_buffer, 7);
+		mdelay(100);
 		
 		//step3  设置单次回传 
 		imucmd_buffer[2] = 0x03;
@@ -230,9 +267,9 @@ uint8_t Imu_MR(void)
 		crc = crc16(imucmd_buffer, 5);
 		imucmd_buffer[5] = (uint8_t)((crc>>8) & 0x00ff);
 		imucmd_buffer[6] = (uint8_t)(crc & 0x00ff);
-		TransmitData(imucmd_buffer, 7);
 	
 		g_imuRxDataIndex = 0;
+		TransmitData(imucmd_buffer, 7);
 	
 		return 0;
 }
@@ -240,10 +277,14 @@ uint8_t Imu_MR(void)
 
 uint8_t Imu_GetAngle(float* roll, float* pitch, float* yaw)
 {
+	UART_SetRXNEInterrupt(UARTx, DISABLE);
+	
 	*roll = roll_angle;
 	*pitch = pitch_angle;
 	*yaw = yaw_angle;
 	
+	UART_SetRXNEInterrupt(UARTx, ENABLE);
+	
 		return 0;
 }
 

Device/Radar.c

@@ -41,7 +41,7 @@
 #define LaDarUARTx   UART0
 #define LaDarUARTx_IRQn  UART0_IRQn
 
-#define RADAR_RX_BUFF_LENGTH   7
+#define RADAR_RX_BUFF_LENGTH   		7
 #define RADAR_MAX_BUFFERSIZE   	 32
 
 #define RadarCmd_NULL   			(0)
@@ -366,3 +366,10 @@ void Radar_DeInit(void)
 
 }
 
+void Radar_Timer1_Callback(void)
+{
+		if(waiting_time_ladar < 0xFF){
+				waiting_time_ladar++;
+		}
+}
+

Device/timer.c

@@ -59,6 +59,9 @@ extern uint8_t  waiting_time_ladar;
 
 /*************<prototype>**************/
 void Timer_Callback(void *device, uint32_t wpara, uint32_t lpara);
+extern void rs485_Timer1_Callback(void);
+extern void Process_Timer1_Callback(void);
+extern void Radar_Timer1_Callback(void);
 
 
 /**
@@ -134,49 +137,9 @@ void Timer_Callback(void *device, uint32_t wpara, uint32_t lpara)
 				//g_timer0Cnt++;
 		}else if(TIMER_CHANNEL1 == device){
 			
-			if(g_blinkLedTime < 0xFFFF)
-			{
-					g_blinkLedTime++;
-			}
-			
-			if(rs485_info.dmasend_count < 0xFF){
-					rs485_info.dmasend_count++;
-			}
-			
-			if(g_mr_Interval < 0xFF){
-					g_mr_Interval++;
-			}
-			
-			if(g_print_interval < 0xFFFF){
-					g_print_interval++;
-			}
-			
-			if(waiting_time_ladar < 0xFF){
-					waiting_time_ladar++;
-			}
-			
-			
-			/*
-			
-			if(g_detectTime < 0xFF){
-					g_detectTime++;
-			}
-			
-			if (g_flashWrRdRdy)
-			{
-				g_flashWrRdTime++;
-			}
-			
-			if(g_runReady){
-				g_runTime++;
-			}
-			
-			if(g_period1000ms < 0xFFFF)
-			{
-				g_period1000ms++;
-			}
-	
-			*/
+			rs485_Timer1_Callback();
+			Radar_Timer1_Callback();
+			Process_Timer1_Callback();
 	
 		}
 

Device/uart.c

@@ -236,4 +236,11 @@ void rs485_RecvData(void)
 		UART_ReceiveDMA(UART485, DMA0_CHANNEL1, rs485_info.recv_buffer, UART_RECV_DATA_POOL_COUNT, UartRxDMAEventCallback); 
 }
 
+void rs485_Timer1_Callback(void)
+{
+	if(rs485_info.dmasend_count < 0xFF){
+			rs485_info.dmasend_count++;
+	}
+}
+
 

Project/AirControlValve.uvoptx

@@ -120,7 +120,7 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name>d</Name>
+          <Name></Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -149,6 +149,28 @@
         </SetRegEntry>
       </TargetDriverDllRegistry>
       <Breakpoint/>
+      <WatchWindow1>
+        <Ww>
+          <count>0</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>g_imuRxDataBuff</ItemText>
+        </Ww>
+        <Ww>
+          <count>1</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>raw_yaw</ItemText>
+        </Ww>
+        <Ww>
+          <count>2</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>raw_pitch</ItemText>
+        </Ww>
+        <Ww>
+          <count>3</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>raw_roll</ItemText>
+        </Ww>
+      </WatchWindow1>
       <Tracepoint>
         <THDelay>0</THDelay>
       </Tracepoint>

Project/AirControlValve.uvprojx

@@ -49,7 +49,7 @@
             <InvalidFlash>1</InvalidFlash>
           </TargetStatus>
           <OutputDirectory>.\Objects\</OutputDirectory>
-          <OutputName>SmartMHcover_20241018</OutputName>
+          <OutputName>SmartMHcover_20241112</OutputName>
           <CreateExecutable>1</CreateExecutable>
           <CreateLib>0</CreateLib>
           <CreateHexFile>1</CreateHexFile>

User/process.c

@@ -10,6 +10,7 @@ uint16_t	g_blinkLedTime;		/*LED
 uint16_t	g_blinkLedTgtTime;	/*LED目标闪烁频率*/
 
 uint8_t		g_mr_Interval = 0;			/*  压力传感器更新周期  */
+uint8_t		g_imumr_Interval = 0;			/*  imu 读取周期  */
 uint16_t  g_print_interval = 0;   /* 打印周期 */
 
 #define STATUS_DETECTINTERVAL  (40)  /*60ms * 8 == 320ms 去抖动 */
@@ -45,6 +46,27 @@ static void update_state()
 
 }
 
+void Process_Timer1_Callback(void)
+{
+		if(g_blinkLedTime < 0xFFFF)
+		{
+				g_blinkLedTime++;
+		}
+		
+		if(g_mr_Interval < 0xFF){
+				g_mr_Interval++;
+		}
+		
+		if(g_imumr_Interval < 0xFF){
+				g_imumr_Interval++;
+		}
+		
+		if(g_print_interval < 0xFFFF){
+				g_print_interval++;
+		}
+
+}
+
 void Process_Init(void)
 {
 		/*初始化控制变量.*/
@@ -97,22 +119,26 @@ void Process_RunPeriod(void)
 	//压力传感器测量请求  
 	if(g_mr_Interval >= 50){
 		g_mr_Interval=0;
-		//g_exception = PressureSensor_MR();
-		Imu_MR();
+
 		Radar_CheckResponse();
 		
 		update_state();
 		
 	}
 	
+	if(g_imumr_Interval >= 100){
+		g_imumr_Interval=0;
+		
+		//Imu_MR();
+	}
+	
 	
 	
 	if(g_print_interval >= 500){
 		g_print_interval=0;
 		
 		Radar_SendRequest();
-		
-		//Imu_MR();
+		Imu_MR();
 		
 		//printTempPress();
 		

User/process.h

@@ -43,10 +43,10 @@ extern "C" {
 #include "ac780x_gpio.h"
 
 /*************<extern>*****************/	
-extern uint16_t		g_blinkLedTime;		/*LED闪烁频率控制时间*/
-extern uint16_t		g_blinkLedTgtTime;	/*LED目标闪烁频率*/
-extern uint8_t		g_mr_Interval;			/*  压力传感器更新周期  */
-extern uint16_t  	g_print_interval;   /* 打印周期 */
+//extern uint16_t		g_blinkLedTime;		/*LED闪烁频率控制时间*/
+//extern uint16_t		g_blinkLedTgtTime;	/*LED目标闪烁频率*/
+//extern uint8_t		g_mr_Interval;			/*  压力传感器更新周期  */
+//extern uint16_t  	g_print_interval;   /* 打印周期 */
 
 
 #define BLINK_LED_MINT		(100)	/*闪烁LED灯最小时间间隔*/
@@ -74,6 +74,7 @@ uint8_t Process_RRadarTransData(uint8_t *pBuf, uint8_t read_len);
 
 uint8_t Process_SetBias(float temp_bias, float press_bias);
  
+
 #ifdef __cplusplus
 }
 #endif

User/protocol.c

@@ -6,12 +6,13 @@
 
 
 #ifdef IS_BOOTLOADER
-uint32_t Firmware_Version[4] = {1, 1, 2, 20241018};
+uint32_t Firmware_Version[4] = {1, 1, 4, 20241112};
 #else
-uint32_t Firmware_Version[4] = {1, 1, 2, 20241018};
+uint32_t Firmware_Version[4] = {1, 1, 4, 20241112};
 #endif
 
 
+
 uint16_t Read_FirmwareVersion(uint8_t *pBuf, uint16_t buf_len)
 {
 	int i;
@@ -205,9 +206,9 @@ uint16_t Read_Height(uint8_t *pBuf, uint16_t buf_len)
 
 uint16_t Read_Angle(uint8_t *pBuf, uint16_t buf_len)
 {
-	float roll =0;
-	float pitch =0;
-	float yaw =0;
+	static float roll =0;
+	static float pitch =0;
+	static float yaw =0;
 	
 	if( buf_len < 12){
 		return 0;