通过高度校准应力初版

Project/HC32F460/MDK/HC32F460.uvoptx

@@ -228,57 +228,52 @@
         <Ww>
           <count>14</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.ADTrack.Slope_VPT,0x0A</ItemText>
+          <ItemText>App.MotorPara.LU_HeightStress,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>15</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText>
+          <ItemText>App.MotorPara.LL_HeightStress,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>16</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.ADTrack.Stop_VPT,0x0A</ItemText>
+          <ItemText>App.MotorPara.RU_HeightStress,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>17</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Olu_diff1,0x0A</ItemText>
+          <ItemText>App.MotorPara.RL_HeightStress</ItemText>
         </Ww>
         <Ww>
           <count>18</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Oru_diff1,0x0A</ItemText>
+          <ItemText>App.TD1mSDelayCnt,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>19</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Olu_diff2,0x0A</ItemText>
+          <ItemText>App.RxUart3Data.RxBuff,0x10</ItemText>
         </Ww>
         <Ww>
           <count>20</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Oru_diff2,0x0A</ItemText>
+          <ItemText>App.RxUart3Data.RxLen,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>21</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Olu_diff3,0x0A</ItemText>
+          <ItemText>App.MotorPara.Current_height,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>22</count>
           <WinNumber>1</WinNumber>
-          <ItemText>Oru_diff3,0x0A</ItemText>
+          <ItemText>App.Status</ItemText>
         </Ww>
         <Ww>
           <count>23</count>
           <WinNumber>1</WinNumber>
-          <ItemText>App.LeftUpCS1237_SlopeArray[0],0x0A</ItemText>
+          <ItemText>App.MotorPara.CaliNum,0x0A</ItemText>
-        </Ww>
-        <Ww>
-          <count>24</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>App.RightUpCS1237_SlopeArray[0],0x0A</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>

Project/HC32F460/source/User/Inc/Algorithm.h

@@ -49,6 +49,7 @@ bool count_smaller(int32_t *arr, uint8_t size, int32_t target);
 uint16_t CRC_Modbus(uint16_t wBase, uint8_t *para, uint16_t length);
 uint8_t CRC_Sum(uint8_t *_pbuff, uint16_t _cmdLen);
 bool isAllZero(uint8_t *arr, int size);
+int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold);
 #endif
 
 

Project/HC32F460/source/User/Inc/bsp.h

@@ -21,13 +21,14 @@
 #define  USE_I2C1					  	0					//I2C1开关，0、关闭，1、启用
 #define  USE_SPI1					  	0					//SPI1开关，0、关闭，1、启用
 #define  USE_SPI3					  	0					//SPI3开关，0、关闭，1、启用
+#define	 USE_USART3						1
 
 #define  USE_BOOTLOADER			  	0					//BOOTLOADER开关，0、关闭，1、启用
 
 #define	USE_CS1237_LEFTUP			1					//左上CS1237开关，0、关闭，1、启用
 #define	USE_CS1237_LEFTLOW		1					//左下CS1237开关，0、关闭，1、启用
 #define	USE_CS1237_RIGHTUP		1					//右上CS1237开关，0、关闭，1、启用
-#define	USE_CS1237_RIGHTLOW		1					//右上CS1237开关，0、关闭，1、启用
+#define	USE_CS1237_RIGHTLOW		1					//右下CS1237开关，0、关闭，1、启用
 
 #define	USE_AGC						1					//算法控制开关，0、关闭，1、启用
 //<<----------------------------	MCU  -------------------------------->>
@@ -50,7 +51,64 @@
 #define	DBG_RX_IRQn								(Int000_IRQn)
 #define	DBG_ER_IRQn								(Int001_IRQn)
 #endif
+//<			USART3
+//#if(USE_USART3 == 1)
+//#define	USAR3_CH							(M4_USART3)
+//#define	USAR3_RX_PORT					(PortH)
+//#define	USAR3_RX_PIN					(Pin02)
+//#define	USAR3_TX_PORT					(PortC)
+//#define	USAR3_TX_PIN						(Pin13)
+//#define	USAR3_RX_FUNC						(Func_Usart3_Rx)
+//#define 	USAR3_TX_FUNC       	(Func_Usart3_Tx)
+//#define 	USAR3_RI_NUM						(INT_USART3_RI)
+//#define 	USAR3_EI_NUM						(INT_USART3_EI)
+//#define 	USAR3_TI_NUM						(INT_USART3_TI)
+//#define 	USAR3_TCI_NUM						(INT_USART3_TCI)
+//#define	USAR3_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART3)
+//#define	USAR3_RXPIN_EXIT_CH						(ExtiCh14)
+//#define	USAR3_RXPIN_EXIT_SRC					(INT_PORT_EIRQ14)
+//#define	USAR3_RX_IRQn								(Int006_IRQn)
+//#define	USAR3_ER_IRQn								(Int007_IRQn)
+//#endif
+#if(USE_USART3 == 1)
+#define	USART3_CH							(M4_USART3)
+#define	USART3_RX_PORT						(PortH)
+#define	USART3_RX_PIN						(Pin02)
+#define	USART3_RX_FUNC						(Func_Usart3_Rx)
+#define USART3_USART_RI_NUM						(INT_USART3_RI)
+#define USART3_USART_EI_NUM						(INT_USART3_EI)
+#define	USART3_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART3)
+#define	USART3_RX_IRQn								(Int006_IRQn)
+#define	USART3_ER_IRQn								(Int007_IRQn)
+//#define	USART3_RXPIN_IRQn					Int002_IRQn
+#endif
 
+//			RS485
+#if(USE_RS485 == 1)
+#define	RS485_USART_CH							(M4_USART1)
+//#define	RS485_USART_RX_PORT						(PortB)
+//#define	RS485_USART_RX_PIN						(Pin08)
+//#define	RS485_USART_TX_PORT						(PortB)
+//#define	RS485_USART_TX_PIN						(Pin09)
+#define	RS485_USART_RX_PORT						(PortA)
+#define	RS485_USART_RX_PIN						(Pin15)
+#define	RS485_USART_TX_PORT						(PortA)
+#define	RS485_USART_TX_PIN						(Pin12)
+#define	RS485_USART_RX_FUNC						(Func_Usart1_Rx)
+#define 	RS485_USART_TX_FUNC       	(Func_Usart1_Tx)
+#define 	RS485_USART_RI_NUM						(INT_USART1_RI)
+#define 	RS485_USART_EI_NUM						(INT_USART1_EI)
+#define 	RS485_USART_TI_NUM						(INT_USART1_TI)
+#define 	RS485_USART_TCI_NUM						(INT_USART1_TCI)
+#define	RS485_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART1)
+#define	RS485_RXPIN_EXIT_CH						(ExtiCh14)
+#define	RS485_RXPIN_EXIT_SRC					(INT_PORT_EIRQ14)
+
+#define 	RS485_CTRL_PORT                    (PortC)
+#define 	RS485_CTRL_PIN                     (Pin13)
+#define	RS485_TX()								PORT_SetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
+#define	RS485_RX()								PORT_ResetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
+#endif
 //			SPI3
 #if(USE_SPI3 == 1)
 /* SPI3_SCK Port/Pin definition */
@@ -79,32 +137,6 @@
 #define SPI3_UNIT                        (M4_SPI3)
 #define SPI3_UNIT_CLOCK                  (PWC_FCG1_PERIPH_SPI3)
 #endif
-//			RS485
-#if(USE_RS485 == 1)
-#define	RS485_USART_CH							(M4_USART1)
-//#define	RS485_USART_RX_PORT						(PortB)
-//#define	RS485_USART_RX_PIN						(Pin08)
-//#define	RS485_USART_TX_PORT						(PortB)
-//#define	RS485_USART_TX_PIN						(Pin09)
-#define	RS485_USART_RX_PORT						(PortA)
-#define	RS485_USART_RX_PIN						(Pin15)
-#define	RS485_USART_TX_PORT						(PortA)
-#define	RS485_USART_TX_PIN						(Pin12)
-#define	RS485_USART_RX_FUNC						(Func_Usart1_Rx)
-#define 	RS485_USART_TX_FUNC       	            (Func_Usart1_Tx)
-#define 	RS485_USART_RI_NUM						(INT_USART1_RI)
-#define 	RS485_USART_EI_NUM						(INT_USART1_EI)
-#define 	RS485_USART_TI_NUM						(INT_USART1_TI)
-#define 	RS485_USART_TCI_NUM						(INT_USART1_TCI)
-#define	RS485_FCG1_PERIPH						(PWC_FCG1_PERIPH_USART1)
-#define	RS485_RXPIN_EXIT_CH						(ExtiCh14)
-#define	RS485_RXPIN_EXIT_SRC					(INT_PORT_EIRQ14)
-
-#define 	RS485_CTRL_PORT                    (PortC)
-#define 	RS485_CTRL_PIN                     (Pin13)
-#define	RS485_TX()								PORT_SetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
-#define	RS485_RX()								PORT_ResetBits(RS485_CTRL_PORT, RS485_CTRL_PIN)
-#endif
 //<		RTC
 #if(USE_RTC == 1)
 #define	RTC_IRQn									Int001_IRQn
@@ -207,9 +239,15 @@ void RS485UsartErrIrqCallback(void);
 void RS485_RxPinIntCallBack(void);
 void RS485UsartRxIrqCallback(void);
 void RS485UartSend(uint8_t *sData, uint16_t sLen);
-
 void RS485_1msRoutine(void);
 
+uint8_t Uart3Rec(void);
+void Usart3ErrIrqCallback(void);
+void Usart3_RxPinIntCallBack(void);
+void Usart3RxIrqCallback(void);
+void Usart3Send(uint8_t *sData, uint16_t sLen);
+void Uart3_Config(uint32_t BaudRate);
+
 void DbgUsartRxIrqCallback(void);
 void DBGUsartErrIrqCallback(void);
 void DBG_RxPinIntCallBack(void);
@@ -253,3 +291,6 @@ void ExtInt06IntDisable(void);
 
 
 
+
+
+

Project/HC32F460/source/User/Inc/main.h

@@ -11,8 +11,8 @@
 #define 	SLOPE_CNT			  20
 
 #define 	SLOPE_VPT				1000
-#define		PRESS_VPT				10
+#define		PRESS_VPT				3
-#define		FOLLOW_VPT			20
+#define		FOLLOW_VPT			10
 #define		SHIELD_VPT			5000
 #define		EME_STOP_VPT		4000
 #define		CENTRE_VPT			2000
@@ -25,6 +25,8 @@
 
 #define		TRAVEL_TIME_MS	30000
 
+#define		PEAK						1200		//1200mm
+#define		VALLEY					720			//720mm
 
 #define	BDC_STOP()			(SET_TWIN_BIT0(),SET_TWIN_BIT1())
 #define BDC_UP()				(SET_TWIN_BIT0(),CLR_TWIN_BIT1())
@@ -63,7 +65,16 @@ typedef struct{
 	int32_t RightUp_Org_AdZero;//右上设备原始AD零点
 	int32_t RightLow_Org_AdZero;//右下设备原始AD零点
 }ADTrack_t;
-
+typedef struct{//电机参数
+	bool InitialFlag;//初始标志位
+	bool CaliFlag;
+	int16_t Current_height;//当前高度mm
+	int16_t CaliNum;//标校数量
+	int32_t LU_HeightStress[488];//左上传感器对应高度应力
+	int32_t LL_HeightStress[488];//左下传感器对应高度应力
+	int32_t RU_HeightStress[488];//右上传感器对应高度应力
+	int32_t RL_HeightStress[488];//右上传感器对应高度应力
+}MotorPara_t;
 /*数据读取状态机*/
 typedef enum {
 	READ_STATUS_IDLE,
@@ -84,10 +95,12 @@ typedef enum {
 	APP_STATUS_IDLE,
 	APP_STATUS_ON,
 	APP_STATUS_OFF,
-	APP_STATUS_UP1,
+	APP_STATUS_MOTOR_INIT,
-	APP_STATUS_DOWN,
+	APP_STATUS_WAIT_MOTOR_INIT,
-	APP_STATUS_UP2,
+	APP_STATUS_WAIT_MOTOR_CALI_UP,
-	APP_STATUS_WAIT,
+	APP_STATUS_WAIT_MOTOR_CALI_DOWN,
+	APP_STATUS_WAIT_STEADY,
+	APP_STATUS_ZERO_MARKING,
 }AppStatus_m;
 
 typedef struct{
@@ -97,16 +110,26 @@ typedef struct{
 	uint8_t RxBuff[10];
 }Uart1Rx_t;
 
+typedef struct{
+	__IO uint16_t rPayLen;
+	__IO bool HeaderFrame;
+	__IO uint16_t RxLen;
+	uint8_t RxBuff[4];
+}Uart3Rx_t;
+
 typedef struct {
 	AppStatus_m Status;//状态
 	ReadStatus_m RStatus;
 	AppPara_T Para;
 	Uart1Rx_t RxUart1Data;
+	Uart3Rx_t RxUart3Data;
 
 	ADTrack_t ADTrack;
+	MotorPara_t	MotorPara;
 	uint32_t TD1mSDelayCnt;
 	uint32_t Read1mSDelayCnt;
 	uint32_t Uart1Delay1mSCnt;
+	uint32_t Uart3RxTimeOut1mSCnt;
 	uint32_t StopCollDelay1mSCnt;
 	uint32_t RunCollDelay1mSCnt;
 	uint32_t EmergencyDelay1mSCnt;

Project/HC32F460/source/User/Src/Algorithm.c

@@ -1,7 +1,115 @@
+#include <stdio.h>
+#include <stdlib.h>
 #include "bsp.h"
 #include "Algorithm.h"
 #include "Debug.h"
 
+/**
+ * 使用滑动窗口修正突变值，并打印修改信息
+ * 
+ * @param arr 待处理的int类型数组
+ * @param size 数组的大小
+ * @param windowSize 滑动窗口大小（建议奇数）
+ * @param threshold 突变阈值
+ * @return 返回修正的次数
+ */
+int fixWithMedianWindow(int arr[], int size, int windowSize, int threshold) {
+    if (size <= 2 || windowSize < 3) {
+        return 0;
+    }
+    
+    int fixCount = 0;
+    int *temp = (int*)malloc(size * sizeof(int));
+    if (temp == NULL) {
+        DBG_LOG("内存分配失败！\n");
+        return 0;
+    }
+    
+    memcpy(temp, arr, size * sizeof(int));
+    
+    DBG_LOG("开始检测并修正突变点 (使用中位数窗口)...\n");
+    DBG_LOG("数组大小: %d, 窗口大小: %d, 阈值: %d\n", size, windowSize, threshold);
+    DBG_LOG("========================================\n");
+    
+    // 检测并修正
+    for (int i = 0; i < size; i++) {
+        // 计算窗口边界
+        int halfWindow = windowSize / 2;
+        int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow);
+        int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow);
+        int windowActualSize = end - start + 1;
+        
+        // 检查当前点是否为突变点
+        int isMutated = 0;
+        if (i > 0 && i < size - 1) {
+            int diffPrev = abs(arr[i] - arr[i-1]);
+            int diffNext = abs(arr[i+1] - arr[i]);
+            if (diffPrev > threshold && diffNext > threshold) {
+                isMutated = 1;
+            }
+        }
+        
+        if (isMutated || (i > 0 && abs(arr[i] - arr[i-1]) > threshold * 2)) {
+            // 收集窗口内的值
+            int *window = (int*)malloc(windowActualSize * sizeof(int));
+            if (window == NULL) continue;
+            
+            int idx = 0;
+            for (int j = start; j <= end; j++) {
+                if (j != i) {  // 排除自身
+                    window[idx++] = arr[j];
+                }
+            }
+            
+            // 如果窗口内有效值太少，使用前后值平均
+            if (idx < 2) {
+                if (i > 0 && i < size - 1) {
+                    int correctedValue = (arr[i-1] + arr[i+1]) / 2;
+                    DBG_LOG("位置 [%d]: 原值 %d -> 新值 %d (窗口内无足够参考值)\n", 
+                           i, arr[i], correctedValue);
+                    temp[i] = correctedValue;
+                    fixCount++;
+                }
+                free(window);
+                continue;
+            }
+            
+            // 排序找中位数（简单冒泡排序，因为窗口通常不大）
+            for (int m = 0; m < idx - 1; m++) {
+                for (int n = 0; n < idx - m - 1; n++) {
+                    if (window[n] > window[n+1]) {
+                        int tmp = window[n];
+                        window[n] = window[n+1];
+                        window[n+1] = tmp;
+                    }
+                }
+            }
+            
+            // 取中位数
+            int correctedValue = window[idx / 2];
+            
+            DBG_LOG("位置 [%d]: 原值 %d -> 新值 %d\n", i, arr[i], correctedValue);
+            DBG_LOG("        窗口范围: [%d-%d], 使用中位数: %d\n", start, end, correctedValue);
+            
+            temp[i] = correctedValue;
+            fixCount++;
+            free(window);
+        }
+    }
+    
+    // 将修正后的值写回原数组
+    memcpy(arr, temp, size * sizeof(int));
+    free(temp);
+    
+    if (fixCount == 0) {
+        DBG_LOG("未发现突变点\n");
+    } else {
+        DBG_LOG("========================================\n");
+        DBG_LOG("共修正了 %d 个突变点\n", fixCount);
+    }
+    
+    return fixCount;
+}
 /*****************************************************************************************
 * 函数名称: check_peaks_valleys_ratio
 * 功能描述: 检查数据的峰值是否大于0，谷值是否小于0（忽略0值）

Project/HC32F460/source/User/Src/bsp.c

@@ -429,6 +429,78 @@ static void DBGRxWakeupInit(void)
 	enIntWakeupEnable(Extint2WU);
 }
 #endif
+#if(USE_USART3 == 1)
+static void Uart3_Config(uint32_t BaudRate)
+{
+	stc_usart_uart_init_t stcInitCfg;
+	stc_irq_regi_conf_t stcIrqRegiCfg;
+	en_result_t enRet;
+	stc_port_init_t stcPortInit;
+
+	MEM_ZERO_STRUCT(stcPortInit);
+	
+	stcPortInit.enPinOType = Pin_OType_Cmos;
+	PORT_Init(USART3_RX_PORT, USART3_RX_PIN, &stcPortInit);
+	
+	stcInitCfg.enClkMode = UsartIntClkCkNoOutput;
+	stcInitCfg.enClkDiv = UsartClkDiv_16;
+	stcInitCfg.enDataLength = UsartDataBits8;
+	stcInitCfg.enDirection = UsartDataLsbFirst;
+	stcInitCfg.enStopBit = UsartOneStopBit;
+	stcInitCfg.enParity = UsartParityNone;
+	stcInitCfg.enSampleMode = UsartSampleBit8;
+	stcInitCfg.enDetectMode = UsartStartBitFallEdge;
+	stcInitCfg.enHwFlow = UsartRtsEnable;
+	
+	PWC_Fcg1PeriphClockCmd(USART3_FCG1_PERIPH, Enable);
+	
+	PORT_SetFunc(USART3_RX_PORT, USART3_RX_PIN, USART3_RX_FUNC, Disable);
+	
+	enRet = USART_UART_Init(USART3_CH, &stcInitCfg);
+	if(enRet != Ok) {
+		Error_Handler();
+	}
+	enRet = USART_SetBaudrate(USART3_CH, BaudRate);
+	if(enRet != Ok) {
+		Error_Handler();
+	}
+	
+	stcIrqRegiCfg.enIRQn = USART3_RX_IRQn;
+	stcIrqRegiCfg.pfnCallback = &Usart3RxIrqCallback;
+	stcIrqRegiCfg.enIntSrc = USART3_USART_RI_NUM;
+	enIrqRegistration(&stcIrqRegiCfg);
+	NVIC_SetPriority(stcIrqRegiCfg.enIRQn, DDL_IRQ_PRIORITY_DEFAULT);
+	NVIC_ClearPendingIRQ(stcIrqRegiCfg.enIRQn);
+	NVIC_EnableIRQ(stcIrqRegiCfg.enIRQn);
+
+
+	/* Set USART RX error IRQ */
+	stcIrqRegiCfg.enIRQn = USART3_ER_IRQn;
+	stcIrqRegiCfg.pfnCallback = &Usart3ErrIrqCallback;
+	stcIrqRegiCfg.enIntSrc = USART3_USART_EI_NUM;
+	enIrqRegistration(&stcIrqRegiCfg);
+	NVIC_SetPriority(stcIrqRegiCfg.enIRQn, DDL_IRQ_PRIORITY_DEFAULT);
+	NVIC_ClearPendingIRQ(stcIrqRegiCfg.enIRQn);
+	NVIC_EnableIRQ(stcIrqRegiCfg.enIRQn);
+
+	USART_FuncCmd(USART3_CH, UsartRx, Enable);
+	USART_FuncCmd(USART3_CH, UsartRxInt, Enable);
+}
+
+void Uart3Send(uint8_t *sData, uint16_t sLen)
+{
+	for(int i = 0; i < sLen; i++) {
+		USART_SendData(USART3_CH, sData[i]);
+		while(USART_GetStatus(USART3_CH, UsartTxComplete) == Reset);
+	}
+}
+
+uint8_t Uart3Rec(void)
+{
+	uint16_t u16Data = USART_RecData(USART3_CH);
+	return (uint8_t)u16Data;
+}
+#endif
 static void GPIO_Config(void)
 {
 	stc_port_init_t stcPortInit;
@@ -1359,15 +1431,18 @@ void BSP_Init(void)
 	ExtiCh04_Init();
 	
 	DbgUart_Config(700000);
+#if(USE_USART3 == 1)
+	Uart3_Config(9600);
+#endif
 	//RS485Uart_Config(9600);
 	
 	//Rtc_Config();
 	//SdioInit();
 	//Spi_Config();
    // Spi3_Config();
-    dcmotor_init();
+//    dcmotor_init();
-	Adc1_Config();
+//	Adc1_Config();
-	Dma_Config();
+//	Dma_Config();
 	//FeedDog();
 
 	SysTick_Init(1000u);
@@ -1431,7 +1506,10 @@ void Wakeup(void)
 	PWC_ClkRecover();
 	//SystemClockConfig();
 	//Spi3_Config();
-	DbgUart_Config(5000000);
+	DbgUart_Config(700000);
+#if(USE_USART3 == 1)
+	Uart3_Config(9600);
+#endif
    // RS485Uart_Config(9600);
 	//GPIO_Config();
 	//SysTick_Resume();
@@ -1469,11 +1547,8 @@ __WEAK void RS485_RxPinIntCallBack(void)
 {
     
 }
-
-
-
-
 #endif
+#if(USE_DEBUG == 1)
 __WEAK void DBGUsartErrIrqCallback(void)
 {
 	if(DBG_USART_CH->SR_f.ORE) {
@@ -1496,7 +1571,32 @@ __WEAK void DbgUsartRxIrqCallback(void)
 __WEAK void DBG_RxPinIntCallBack(void)
 {
 }
+#endif
+#if(USE_USART3 == 1)
+void Usart3ErrIrqCallback(void)
+{
+	if(USART3_CH->SR_f.ORE) {
+		USART3_CH->CR1_f.CORE = 1;
+		USART_RecData(USART3_CH);
+	}
 	
+	if(USART3_CH->SR_f.PE)
+		USART3_CH->CR1_f.CPE = 1;
+	
+	if(USART3_CH->SR_f.FE)
+		USART3_CH->CR1_f.CFE = 1;
+}
+
+
+__WEAK void Usart3RxIrqCallback(void)
+{
+	
+}
+
+__WEAK void Usart3_RxPinIntCallBack(void)
+{
+}
+#endif
 /*****************************************************************************************
 * 函数名称: RtcPeriod_IrqCallback
 * 功能描述: RTC中断回调

Project/HC32F460/source/User/Src/main.c

@@ -40,12 +40,23 @@ void LEFTUP_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.LeftUpCS1237_TrendArray[0];
 //	}
-	show(1,AD);
+	if(App.MotorPara.CaliFlag == true)
+	{
+		AD = AD + App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720];
 		for(int i = OSC_CNT-1; i > 0; i--)
 		{
 			App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
 		}
 		App.LeftUpCS1237_TrendArray[0] = AD;
+	}
+	else{
+//		for(int i = OSC_CNT-1; i > 0; i--)
+//		{
+//			App.LeftUpCS1237_TrendArray[i] = App.LeftUpCS1237_TrendArray[i - 1];
+//		}
+		App.LeftUpCS1237_TrendArray[0] = AD;
+	}
+	show(1,AD);
 	App.LeftUpCS1237ReadEndFlag = true;
 }
 void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
@@ -55,12 +66,24 @@ void LEFTLOW_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.LeftLowCS1237_TrendArray[0];
 //	}
-	show(2,AD);
+	if(App.MotorPara.CaliFlag == true)
+	{
+		AD = AD + App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720];
 		for(int i = OSC_CNT-1; i > 0; i--)
 		{
 			App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
 		}
 		App.LeftLowCS1237_TrendArray[0] = AD;
+	}
+	else
+	{
+//		for(int i = OSC_CNT-1; i > 0; i--)
+//		{
+//			App.LeftLowCS1237_TrendArray[i] = App.LeftLowCS1237_TrendArray[i - 1];
+//		}
+		App.LeftLowCS1237_TrendArray[0] = AD;
+	}
+	show(2,AD);
 	App.LeftLowCS1237ReadEndFlag = true;
 }
 void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
@@ -69,12 +92,24 @@ void RIGHTUP_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.RightUpCS1237_TrendArray[0];
 //	}
-	show(3,AD);
+	if(App.MotorPara.CaliFlag == true)
+	{
+		AD = AD + App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720];
 		for(int i = OSC_CNT-1; i > 0; i--)
 		{
 			App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
 		}
 		App.RightUpCS1237_TrendArray[0] = AD;
+	}
+	else
+	{
+//		for(int i = OSC_CNT-1; i > 0; i--)
+//		{
+//			App.RightUpCS1237_TrendArray[i] = App.RightUpCS1237_TrendArray[i - 1];
+//		}
+		App.RightUpCS1237_TrendArray[0] = AD;
+	}
+	show(3,AD);
 	App.RightUpCS1237ReadEndFlag = true;
 }
 void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
@@ -83,12 +118,24 @@ void RIGHTLOW_CS1237AinADataProcessCallBack(int32_t AD)
 //	{
 //		AD = App.RightLowCS1237_TrendArray[0];
 //	}
-	show(4,AD);
+	if(App.MotorPara.CaliFlag == true)
+	{
+		AD = AD + App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720];
 		for(int i = OSC_CNT-1; i > 0; i--)
 		{
 			App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
 		}
 		App.RightLowCS1237_TrendArray[0] = AD;
+	}
+	else
+	{
+//		for(int i = OSC_CNT-1; i > 0; i--)
+//		{
+//			App.RightLowCS1237_TrendArray[i] = App.RightLowCS1237_TrendArray[i - 1];
+//		}
+		App.RightLowCS1237_TrendArray[0] = AD;
+	}
+	show(4,AD);
 	App.RightLowCS1237ReadEndFlag = true;
 }
 void GradeControl(ControlLevel_m level)
@@ -222,8 +269,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 	
 	int32_t LeftSlopeSum = abs(App.LeftUpCS1237_SlopeArray[0]) + abs(App.LeftLowCS1237_SlopeArray[0]) + abs(App.LeftUpCS1237_SlopeArray[1]) + abs(App.LeftLowCS1237_SlopeArray[1]) + abs(App.LeftUpCS1237_SlopeArray[2]) + abs(App.LeftLowCS1237_SlopeArray[2]);
 	int32_t RightSlopeSum = abs(App.RightUpCS1237_SlopeArray[0]) + abs(App.RightLowCS1237_SlopeArray[0]) + abs(App.RightUpCS1237_SlopeArray[1]) + abs(App.RightLowCS1237_SlopeArray[1]) + abs(App.RightUpCS1237_SlopeArray[2]) + abs(App.RightLowCS1237_SlopeArray[2]);
-	int32_t LeftDiffSum = abs(lu_diff[0]) + abs(lu_diff[1]) + abs(lu_diff[2]);
+	int32_t LeftDiffSum = abs(lu_diff[0]) + abs(lu_diff[1]) + abs(lu_diff[2]) + abs(ll_diff[0]) + abs(ll_diff[1]) + abs(ll_diff[2]);
-	int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[2]);
+	int32_t RightDiffSum = abs(ru_diff[0]) + abs(ru_diff[1]) + abs(ru_diff[2]) + abs(rl_diff[0]) + abs(rl_diff[1]) + abs(rl_diff[2]);
 	int32_t LeftDiff = abs(Oll_diff1) + abs(Olu_diff1) + abs(Oll_diff2) + abs(Olu_diff2) + abs(Oll_diff3) + abs(Olu_diff3);
 	int32_t RightDiff = abs(Orl_diff1) + abs(Oru_diff1) + abs(Orl_diff2) + abs(Oru_diff2) + abs(Orl_diff3) + abs(Oru_diff3);
 	int32_t LuSideDiff = lu_ad[0] - lu_ad[5];
@@ -288,7 +335,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.LeftRunFalg = true;
 				if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT)
 				&& ((Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT))
-				&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)))
+				&& ((Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
+				)
+				{
+					if(App.MotorPara.Current_height < 1195)
 					{
 						BDC_UP();
 						App.CaliZeroFlag = false;
@@ -298,9 +348,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						App.EmeFlag = false;
 						App.RunCollDelay1mSCnt = RUN_TIME_MS;
 					}
+				}
 				if((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT)
 				&& (Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT)
-				&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
+				&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)
+				)
+				{
+					if(App.MotorPara.Current_height > 725)
 					{
 						BDC_DOWN();
 						App.CaliZeroFlag = false;
@@ -311,12 +365,16 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						App.RunCollDelay1mSCnt = RUN_TIME_MS;
 					}
 				}
+			}
 			else if(RightDiff > LeftDiff)
 			{
 				App.RightRunFalg = true;
 				if((Olu_diff1+Oru_diff1) < -(App.ADTrack.Stop_VPT)
 				&& (Olu_diff2+Oru_diff2) < -(App.ADTrack.Stop_VPT)
-				&& (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT))
+				&& (Olu_diff3+Oru_diff3) < -(App.ADTrack.Stop_VPT)
+				)
+				{
+					if(App.MotorPara.Current_height < 1195)
 					{
 						BDC_UP();
 						App.CaliZeroFlag = false;
@@ -326,9 +384,13 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						App.EmeFlag = false;
 						App.RunCollDelay1mSCnt = RUN_TIME_MS;
 					}
+				}
 				if((Olu_diff1+Oru_diff1) > (App.ADTrack.Stop_VPT)
 				&& (Olu_diff2+Oru_diff2) > (App.ADTrack.Stop_VPT)
-				&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT))
+				&& (Olu_diff3+Oru_diff3) > (App.ADTrack.Stop_VPT)
+				)
+				{
+					if(App.MotorPara.Current_height > 725)
 					{
 						BDC_DOWN();
 						App.CaliZeroFlag = false;					
@@ -340,6 +402,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 					}
 				}
 			}
+		}
 		else if((LeftSlopeSum > RightSlopeSum && LeftDiffSum > RightDiffSum))//判断左边操作
 		{
 			App.LeftRunFalg = true;
@@ -353,7 +416,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (App.LeftLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) || (
 			   (Olu_diff1 >= (App.ADTrack.LeftUpVPT*5/6))	//通过差值判断上下拉方向
 			&& (Olu_diff2 >= (App.ADTrack.LeftUpVPT*5/6))
-			&& (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6))))
+			&& (Olu_diff3 >= (App.ADTrack.LeftUpVPT*5/6)))
+			&& (Oll_diff1 <= -(App.ADTrack.LeftLowVPT*5/6))
+			&& (Oll_diff2 <= -(App.ADTrack.LeftLowVPT*5/6))
+			&& (Oll_diff3 <= -(App.ADTrack.LeftLowVPT*5/6)))
 			{
 				if((App.LeftUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
 				&& (App.LeftUpCS1237_SlopeArray[1] <= -PRESS_VPT) 
@@ -369,16 +435,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
 						&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
 						{
-							BDC_DOWN();
+							if(App.MotorPara.Current_height > 725)
-							App.CaliZeroFlag = false;
-							App.UpFlag = false;
-							App.DownFlag = true;
-							App.StopFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
-						}
-					}
-					else
 							{
 									BDC_DOWN();
 									App.CaliZeroFlag = false;
@@ -390,6 +447,21 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 							}
 						}
 					}
+					else
+					{
+						if(App.MotorPara.Current_height > 725)
+						{
+								BDC_DOWN();
+								App.CaliZeroFlag = false;
+								App.UpFlag = false;
+								App.DownFlag = true;
+								App.StopFlag = false;
+								App.EmeFlag = false;
+								App.RunCollDelay1mSCnt = RUN_TIME_MS;
+						}
+					}
+				}
+			}
 			else if((
 //				(App.SlopeCollDelay1mSCnt <= 0)
 //			&& (App.LeftUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)  //通过斜率判断上下拉方向
@@ -400,7 +472,11 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (App.LeftLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT)) || (
 				 (Olu_diff1 <= -(App.ADTrack.LeftUpVPT*5/6))//通过差值判断上下拉方向
 			&& (Olu_diff2 <= -(App.ADTrack.LeftUpVPT*5/6))
-			&& (Olu_diff3 <= -(App.ADTrack.LeftUpVPT*5/6))))
+			&& (Olu_diff3 <= -(App.ADTrack.LeftUpVPT*5/6))
+			&& (Oll_diff1 >= (App.ADTrack.LeftLowVPT*5/6))
+			&& (Oll_diff2 >= (App.ADTrack.LeftLowVPT*5/6))
+			&& (Oll_diff3 >= (App.ADTrack.LeftLowVPT*5/6)))
+			)
 			{
 				if((App.LeftUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
 				&& (App.LeftUpCS1237_SlopeArray[1] >= PRESS_VPT) 
@@ -416,16 +492,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (abs(Oll_diff1) > abs(Oru_diff1) + App.ADTrack.LeftLowShieldVPT)
 						&& (abs(Olu_diff1) > abs(Oru_diff1) + App.ADTrack.LeftUpShieldVPT))
 						{
-							BDC_UP();
+							if(App.MotorPara.Current_height < 1195)
-							App.CaliZeroFlag = false;
-							App.UpFlag = true;
-							App.DownFlag = false;
-							App.StopFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
-						}
-					}
-					else
 							{
 								BDC_UP();
 								App.CaliZeroFlag = false;
@@ -437,6 +504,21 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 							}
 						}
 					}
+					else
+					{
+							if(App.MotorPara.Current_height < 1195)
+							{
+								BDC_UP();
+								App.CaliZeroFlag = false;
+								App.UpFlag = true;
+								App.DownFlag = false;
+								App.StopFlag = false;
+								App.EmeFlag = false;
+								App.RunCollDelay1mSCnt = RUN_TIME_MS;
+							}
+					}
+				}
+			}
 		}
 		else if(LeftSlopeSum < RightSlopeSum && LeftDiffSum < RightDiffSum)//判断右边操作
 		{
@@ -451,7 +533,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (App.RightLowCS1237_SlopeArray[2] >= App.ADTrack.Slope_VPT)) || (
 				 (Oru_diff1 >= (App.ADTrack.RightUpVPT*5/6))				//通过差值判断上下拉方向
 			&& (Oru_diff2 >= (App.ADTrack.RightUpVPT*5/6))
-			&& (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6))))
+			&& (Oru_diff3 >= (App.ADTrack.RightUpVPT*5/6)))
+			&& (Orl_diff1 <= -(App.ADTrack.RightLowVPT*5/6))
+			&& (Orl_diff2 >= -(App.ADTrack.RightLowVPT*5/6))
+			&& (Orl_diff3 <= -(App.ADTrack.RightLowVPT*5/6)))
 			{
 				if((App.RightUpCS1237_SlopeArray[0] <= -PRESS_VPT) //判断是否不是正面压
 				&& (App.RightUpCS1237_SlopeArray[1] <= -PRESS_VPT) 
@@ -467,16 +552,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
 						&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
 						{
-							BDC_DOWN();		
+							if(App.MotorPara.Current_height > 725)
-							App.CaliZeroFlag = false;
-							App.DownFlag = true;
-							App.StopFlag = false;
-							App.UpFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
-						}
-					}
-					else
 							{
 								BDC_DOWN();		
 								App.CaliZeroFlag = false;
@@ -488,6 +564,21 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 							}
 						}
 					}
+					else
+					{
+						if(App.MotorPara.Current_height > 725)
+						{
+							BDC_DOWN();		
+							App.CaliZeroFlag = false;
+							App.DownFlag = true;
+							App.StopFlag = false;
+							App.UpFlag = false;
+							App.EmeFlag = false;
+							App.RunCollDelay1mSCnt = RUN_TIME_MS;
+						}
+					}
+				}
+			}
 			else if((
 //				(App.SlopeCollDelay1mSCnt <= 0)
 //			&& (App.RightUpCS1237_SlopeArray[0] >= App.ADTrack.Slope_VPT)  //通过斜率判断上下拉方向
@@ -498,7 +589,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 //			&& (App.RightLowCS1237_SlopeArray[2] <= -App.ADTrack.Slope_VPT))|| (
 			 (Oru_diff1 <= -(App.ADTrack.RightUpVPT*5/6))		//通过差值判断上下拉方向
 			&& (Oru_diff2 <= -(App.ADTrack.RightUpVPT*5/6))
-			&& (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6))))
+			&& (Oru_diff3 <= -(App.ADTrack.RightUpVPT*5/6)))
+			&& (Orl_diff1 >= (App.ADTrack.RightLowVPT*5/6))
+			&& (Orl_diff2 >= (App.ADTrack.RightLowVPT*5/6))
+			&& (Orl_diff3 >= (App.ADTrack.RightLowVPT*5/6)))
 			{
 				if((App.RightUpCS1237_SlopeArray[0] >= PRESS_VPT) //判断是否不是正面压
 				&& (App.RightUpCS1237_SlopeArray[1] >= PRESS_VPT) 
@@ -514,16 +608,7 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 						&& (abs(Orl_diff1) > abs(Olu_diff1) + App.ADTrack.RightLowShieldVPT)
 						&& (abs(Oru_diff1) > abs(Olu_diff1) + App.ADTrack.RightUpShieldVPT))
 						{
-							BDC_UP();
+							if(App.MotorPara.Current_height < 1195)
-							App.CaliZeroFlag = false;
-							App.UpFlag = true;
-							App.StopFlag = false;
-							App.DownFlag = false;
-							App.EmeFlag = false;
-							App.RunCollDelay1mSCnt = RUN_TIME_MS;
-						}
-					}
-					else
 							{
 								BDC_UP();
 								App.CaliZeroFlag = false;
@@ -535,6 +620,21 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 							}
 						}
 					}
+					else
+					{
+						if(App.MotorPara.Current_height < 1195)
+						{
+							BDC_UP();
+							App.CaliZeroFlag = false;
+							App.UpFlag = true;
+							App.StopFlag = false;
+							App.DownFlag = false;
+							App.EmeFlag = false;
+							App.RunCollDelay1mSCnt = RUN_TIME_MS;
+						}
+					}
+				}
+			}
 		}
 	}
 	else if(App.DownFlag == true && App.RunCollDelay1mSCnt <= 0)
@@ -542,7 +642,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		if(App.LeftRunFalg == true)
 		{
 			if(((App.CentreRunFalg == false ) && (lu_slope > App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -557,8 +658,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((App.CentreRunFalg == true) 
 				&& ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
 				&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
-				&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))
+				&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
-				))
+				|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -569,23 +670,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
 				App.EmergencyDelay1mSCnt = EME_START_MS;
 			}
-			else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) 
+//			else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) 
-				&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) 
+//				&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) 
-				&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) 
+//				&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) 
-				&& (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) 
+//				&& (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) 
-				&& (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) 
+//				&& (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) 
-				&& (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) 
+//				&& (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) 
-				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
+//				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
-			{
+//			{
-				BDC_STOP();
+//				BDC_STOP();
-				App.EmeFlag = true;
+//				App.EmeFlag = true;
-				App.EmergencyDelay1mSCnt = EME_STOP_MS;
+//				App.EmergencyDelay1mSCnt = EME_STOP_MS;
-			}
+//			}
 		}
 		else if(App.RightRunFalg == true)
 		{
 			if(((App.CentreRunFalg == false ) && (ru_slope > App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -600,8 +702,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((App.CentreRunFalg == true) 
 				&& ((Olu_diff1 + Oru_diff1) < (App.ADTrack.Stop_VPT*2/3))
 				&& ((Olu_diff2 + Oru_diff2) < (App.ADTrack.Stop_VPT*2/3))
-				&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3))
+				&& ((Olu_diff3 + Oru_diff3) < (App.ADTrack.Stop_VPT*2/3)))
-				))
+				|| (App.MotorPara.Current_height <= 725))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -612,18 +714,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
 				App.EmergencyDelay1mSCnt = EME_START_MS;
 			}
-			else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) 
+//			else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) 
-				&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) 
+//				&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) 
-				&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) 
+//				&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) 
-				&& abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) 
+//				&& abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) 
-				&& abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) 
+//				&& abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) 
-				&& abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) 
+//				&& abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) 
-				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
+//				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
-			{
+//			{
-				BDC_STOP();
+//				BDC_STOP();
-				App.EmeFlag = true;
+//				App.EmeFlag = true;
-				App.EmergencyDelay1mSCnt = EME_STOP_MS;
+//				App.EmergencyDelay1mSCnt = EME_STOP_MS;
-			}
+//			}
 		}
 	}
 	else if(App.UpFlag == true && App.RunCollDelay1mSCnt <= 0)
@@ -631,7 +733,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		if(App.LeftRunFalg == true)
 		{
 			if(((App.CentreRunFalg == false ) && (lu_slope < -App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -646,8 +749,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((App.CentreRunFalg == true) 
 				&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
 				&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
-				&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))
+				&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
-				))
+				|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -658,23 +761,24 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
 				App.EmergencyDelay1mSCnt = EME_START_MS;
 			}
-			else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) 
+//			else if((abs(Oll_diff1) + EME_STOP_VPT) < abs(Orl_diff1) 
-				&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) 
+//				&& (abs(Oll_diff2) + EME_STOP_VPT) < abs(Orl_diff2) 
-				&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) 
+//				&& (abs(Oll_diff3) + EME_STOP_VPT) < abs(Orl_diff3) 
-				&& (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) 
+//				&& (abs(Olu_diff1) + EME_STOP_VPT) < abs(Oru_diff1) 
-				&& (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) 
+//				&& (abs(Olu_diff2) + EME_STOP_VPT) < abs(Oru_diff2) 
-				&& (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) 
+//				&& (abs(Olu_diff3) + EME_STOP_VPT) < abs(Oru_diff3) 
-				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
+//				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
-			{
+//			{
-				BDC_STOP();
+//				BDC_STOP();
-				App.EmeFlag = true;
+//				App.EmeFlag = true;
-				App.EmergencyDelay1mSCnt = EME_STOP_MS;
+//				App.EmergencyDelay1mSCnt = EME_STOP_MS;
-			} 
+//			} 
 		}
 		else if(App.RightRunFalg == true)
 		{
 			if(((App.CentreRunFalg == false ) && (ru_slope < -App.ADTrack.Slope_VPT))
-			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0)))
+			|| ((App.CentreRunFalg == true ) && ((App.LeftUpCS1237_SlopeArray[0] + App.RightUpCS1237_SlopeArray[0]) > App.ADTrack.Slope_VPT) && (App.SlopeCollDelay1mSCnt <= 0))
+			|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -689,8 +793,8 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				|| ((App.CentreRunFalg == true) 
 				&& ((Olu_diff1 + Oru_diff1) > -(App.ADTrack.Stop_VPT*2/3))
 				&& ((Olu_diff2 + Oru_diff2) > -(App.ADTrack.Stop_VPT*2/3))
-				&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3))
+				&& ((Olu_diff3 + Oru_diff3) > -(App.ADTrack.Stop_VPT*2/3)))
-				))
+				|| (App.MotorPara.Current_height >= 1195))
 			{
 				BDC_STOP();
 				App.StopFlag = true;
@@ -701,18 +805,18 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 				App.SlopeCollDelay1mSCnt = SLOPE_START_MS;
 				App.EmergencyDelay1mSCnt = EME_START_MS;
 			}
-			else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) 
+//			else if(abs(Oll_diff1) > (abs(Orl_diff1) + EME_STOP_VPT) 
-				&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) 
+//				&& abs(Oll_diff2) > (abs(Orl_diff2) + EME_STOP_VPT) 
-				&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) 
+//				&& abs(Oll_diff3) > (abs(Orl_diff3) + EME_STOP_VPT) 
-				&& abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) 
+//				&& abs(Olu_diff1) > (abs(Oru_diff1) + EME_STOP_VPT) 
-				&& abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) 
+//				&& abs(Olu_diff2) > (abs(Oru_diff2) + EME_STOP_VPT) 
-				&& abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) 
+//				&& abs(Olu_diff3) > (abs(Oru_diff3) + EME_STOP_VPT) 
-				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
+//				&& App.EmergencyDelay1mSCnt <= 0 && App.CentreRunFalg == false)
-			{
+//			{
-				BDC_STOP();
+//				BDC_STOP();
-				App.EmeFlag = true;
+//				App.EmeFlag = true;
-				App.EmergencyDelay1mSCnt = EME_STOP_MS;
+//				App.EmergencyDelay1mSCnt = EME_STOP_MS;
-			}
+//			}
 		}
 	}
 	
@@ -750,7 +854,10 @@ void SupportControlHandle(int32_t *lu_ad, int32_t *ll_ad, int32_t *ru_ad, int32_
 		&& App.RightLowCS1237_SlopeArray[2] < FOLLOW_VPT
 		&& App.RightLowCS1237_SlopeArray[2] > -FOLLOW_VPT) 
 	{
-		if(App.StopFlag == true && App.StopCollDelay1mSCnt <= 0){
+		if(App.StopFlag == true 
+		&& App.StopCollDelay1mSCnt <= 0 
+		&& App.MotorPara.Current_height <= 1995
+		&& App.MotorPara.Current_height >= 725){
 			App.ADTrack.LeftUp_Org_AdZero = lu_ad[0];
 			App.ADTrack.LeftLow_Org_AdZero = ll_ad[0];
 			App.ADTrack.RightUp_Org_AdZero = ru_ad[0];
@@ -793,8 +900,10 @@ static void AppInit(void)
 	
 	BDC_STOP();
 	
-	App.Status = APP_STATUS_WAIT;
+//	App.Status = APP_STATUS_WAIT;
-	App.TD1mSDelayCnt = 1000;
+	App.Status = APP_STATUS_MOTOR_INIT;
+	App.MotorPara.InitialFlag = false;
+//	App.TD1mSDelayCnt = 1000;
 //	App.Status = APP_STATUS_UP1;
 //	App.TD1mSDelayCnt = TRAVEL_TIME_MS;
 	App.RStatus = READ_STATUS_START;
@@ -915,88 +1024,40 @@ static void AppLoopHandler(void)
 			App.Status = APP_STATUS_IDLE;
 		break;}
 		
-		case APP_STATUS_UP1:{
+		case APP_STATUS_MOTOR_INIT:{
-			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			{
 			BDC_UP();
-			}
+			Ddl_Delay1ms(1000);
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
+			BDC_DOWN();
-			{
+			App.DownFlag = true;
-				BDC_STOP();
+			App.StopFlag = false;
-			}
+			App.Status = APP_STATUS_WAIT_MOTOR_INIT;
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
-			{
-				BDC_UP();
-			}
-			else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
-			{
-				BDC_STOP();
-				App.Status = APP_STATUS_DOWN;
 			App.TD1mSDelayCnt = TRAVEL_TIME_MS;
-			}
 		break;}
 		
-		case APP_STATUS_DOWN:{
+		case APP_STATUS_WAIT_MOTOR_INIT:{
-			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			{
-				BDC_DOWN();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-1500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
-			{
-				BDC_STOP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-1500))
-			{
-				BDC_DOWN();
-			}
-			else if(App.TD1mSDelayCnt >= (TRAVEL_TIME_MS-26100) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-26000))
-			{
-				BDC_STOP();
-			}
-			else if(App.TD1mSDelayCnt > 0 && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-27100))
-			{
-				BDC_DOWN();
-			}
-			else if(App.TD1mSDelayCnt <= 0)
-			{
-				BDC_STOP();
-				App.Status = APP_STATUS_UP2;
-				App.TD1mSDelayCnt = TRAVEL_TIME_MS;
-			}
-		break;}
-		
-		case APP_STATUS_UP2:{
-			if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-500))
-			{
-				BDC_UP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-2500) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-500))
-			{
-				BDC_STOP();
-			}
-			else if(App.TD1mSDelayCnt > (TRAVEL_TIME_MS-25000) && App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-3000))
-			{
-				BDC_UP();
-			}
-			else if(App.TD1mSDelayCnt <= (TRAVEL_TIME_MS-25000))
-			{
-				BDC_STOP();
-				App.Status = APP_STATUS_WAIT;
-				App.TD1mSDelayCnt = 1000;
-			}
-		break;}
-				
-		case APP_STATUS_WAIT:{
 			if(App.TD1mSDelayCnt <= 0)
 			{
-				App.WaitFlag = true;
+				BDC_STOP();
-				App.SideFlag = false;
-				App.CaliZeroFlag = false;
-				App.UpFlag = false;
-				App.DownFlag = false;
-				App.EmeFlag = false;
 				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				App.Status = APP_STATUS_IDLE;
+			}
+			if(App.MotorPara.Current_height <= 721 && App.MotorPara.Current_height >= 720)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				App.MotorPara.CaliFlag = false;
+				App.TD1mSDelayCnt = 1000;
+				App.Status = APP_STATUS_ZERO_MARKING;
+			}
+		break;}
 		
+		case APP_STATUS_ZERO_MARKING:{
+			if(App.TD1mSDelayCnt <= 0)
+			{				
 				App.ADTrack.LeftUp_Org_AdZero = App.LeftUpCS1237_TrendArray[0];
 				App.ADTrack.LeftLow_Org_AdZero = App.LeftLowCS1237_TrendArray[0];
 				App.ADTrack.RightUp_Org_AdZero = App.RightUpCS1237_TrendArray[0];
@@ -1007,8 +1068,98 @@ static void AppLoopHandler(void)
 				App.ADTrack.RightUp_Org_AdZero,
 				App.ADTrack.RightLow_Org_AdZero);
 				
+				App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
+				App.MotorPara.InitialFlag = true;
+				App.MotorPara.CaliNum = 0;
+				App.TD1mSDelayCnt = TRAVEL_TIME_MS;
+				App.StopFlag = false;
+				App.UpFlag = true;
+				BDC_UP();
+					//App.Status = APP_STATUS_WAIT_STEADY;
+			}
+		break;}
+				
+		case APP_STATUS_WAIT_MOTOR_CALI_UP:{
+			if(App.MotorPara.Current_height >= 1204)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				Ddl_Delay1ms(1000);
+				App.DownFlag = true;
+				App.StopFlag = false;
+				App.Status = APP_STATUS_WAIT_MOTOR_CALI_DOWN;
+				BDC_DOWN();
+			}
+			else if(App.MotorPara.CaliNum >= 488)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				App.MotorPara.InitialFlag = false;
+				App.MotorPara.CaliFlag = true;
+				DBG_LOG("===================LU Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.LU_HeightStress,sizeof(App.MotorPara.LU_HeightStress)/4,7,2000);
+				DBG_LOG("===================LL Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.LL_HeightStress,sizeof(App.MotorPara.LL_HeightStress)/4,7,2000);
+				DBG_LOG("===================RU Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.RU_HeightStress,sizeof(App.MotorPara.RU_HeightStress)/4,7,2000);
+				DBG_LOG("===================RL Data monitoring====================");
+				fixWithMedianWindow(App.MotorPara.RL_HeightStress,sizeof(App.MotorPara.RL_HeightStress)/4,7,2000);
+				DBG_LOG("Motor Cali Ok\r\n");
+				App.TD1mSDelayCnt = 1000;
+				App.Status = APP_STATUS_WAIT_STEADY;
+			}
+		break;}
+		
+		case APP_STATUS_WAIT_MOTOR_CALI_DOWN:{ 
+			if(App.MotorPara.Current_height <= 721 && App.MotorPara.Current_height >= 720)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				Ddl_Delay1ms(1000);
+				App.UpFlag = true;
+				App.StopFlag = false;
+				App.Status = APP_STATUS_WAIT_MOTOR_CALI_UP;
+				BDC_UP();
+			}
+			else if(App.MotorPara.CaliNum >= 488)
+			{
+				BDC_STOP();
+				App.StopFlag = true;
+				App.DownFlag = false;
+				App.UpFlag = false;
+				App.MotorPara.InitialFlag = false;
+				App.MotorPara.CaliFlag = true;
+				DBG_LOG("===================LU Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.LU_HeightStress,sizeof(App.MotorPara.LU_HeightStress)/4,7,2000);
+				DBG_LOG("===================LL Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.LL_HeightStress,sizeof(App.MotorPara.LL_HeightStress)/4,7,2000);
+				DBG_LOG("===================RU Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.RU_HeightStress,sizeof(App.MotorPara.RU_HeightStress)/4,7,2000);
+				DBG_LOG("===================RL Data monitoring====================\r\n");
+				fixWithMedianWindow(App.MotorPara.RL_HeightStress,sizeof(App.MotorPara.RL_HeightStress)/4,7,2000);
+				DBG_LOG("Motor Cali Ok\r\n");
+				App.TD1mSDelayCnt = 1000;
+				App.Status = APP_STATUS_WAIT_STEADY;
+			}
+		break;}
+		
+		case APP_STATUS_WAIT_STEADY:{
+			if(App.TD1mSDelayCnt <= 0)
+			{
+				App.SideFlag = false;
+				App.CaliZeroFlag = false;
+				App.UpFlag = false;
+				App.DownFlag = false;
+				App.EmeFlag = false;
+				App.StopFlag = true;
+				App.WaitFlag = true;
 				App.Status = APP_STATUS_IDLE;
-				App.TD1mSDelayCnt = 0;
 			}
 		break;}
 	}
@@ -1021,6 +1172,8 @@ static void App1mSRoutine(void)
 		App.Read1mSDelayCnt--;
 	if(App.Uart1Delay1mSCnt > 0)
 		App.Uart1Delay1mSCnt--;
+	if(App.Uart3RxTimeOut1mSCnt > 0)
+		App.Uart3RxTimeOut1mSCnt--;
 	if(App.StopCollDelay1mSCnt > 0)
 		App.StopCollDelay1mSCnt--;
 	if(App.RunCollDelay1mSCnt > 0)
@@ -1065,7 +1218,6 @@ int main(void)
 		AppLoopHandler();
 	}
 }
-
 /*****************************************************************************************
 * 函数名称: SysTick_IrqHandler
 * 功能描述: 系统滴答中断
@@ -1098,6 +1250,71 @@ void DBG_RxPinIntCallBack(void)
 
 
 
+#if(USE_USART3 == 1)
+// 定义函数Usart3RxIrqCallback，用于处理串口接收中断
+void Usart3RxIrqCallback(void)
+{
+	// 如果1分钟超时计数器为0，则清空RxLen
+	if(App.Uart3RxTimeOut1mSCnt == 0) 
+	{
+		App.RxUart3Data.HeaderFrame = false;
+		App.RxUart3Data.RxLen = 0;
+	}
+	App.Uart3RxTimeOut1mSCnt = 5;
+	// 读取串口接收到的数据
+	uint8_t Data = Uart3Rec();
+	if(App.RxUart3Data.HeaderFrame == false)
+	{
+		if(App.RxUart3Data.RxLen == 0 && Data == 0x01)
+		{
+			App.RxUart3Data.HeaderFrame = true;
+			App.RxUart3Data.RxLen = 0;
+			App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data;
+		}
+	}
+	else
+	{
+		if(App.RxUart3Data.RxLen == 1 && Data != 0x01)
+		{
+			App.RxUart3Data.HeaderFrame = false;
+			return;
+		}
+		 // 如果RxLen小于128，则将数据添加到RxBuff中
+		if(App.RxUart3Data.RxLen < 128) {
+				App.RxUart3Data.RxBuff[App.RxUart3Data.RxLen++] = Data;
+		}
+		if(App.RxUart3Data.RxLen >= 4)
+		{
+			if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x01)
+			{
+				int16_t h = ((App.RxUart3Data.RxBuff[2]<<8) & 0xff00) + (App.RxUart3Data.RxBuff[3] & 0x00ff);
+				if(h >=720 && h <= 1208)
+				{
+					App.MotorPara.Current_height = h;
+				}
+				if(App.MotorPara.InitialFlag == true && h >=720 && h <= 1210)
+				{//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿
+					if(App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] == 0
+					|| App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] == 0
+					|| App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] == 0
+					|| App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] == 0)
+					{
+						App.MotorPara.LU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftUp_Org_AdZero - App.LeftUpCS1237_TrendArray[0];
+						App.MotorPara.LL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.LeftLow_Org_AdZero - App.LeftLowCS1237_TrendArray[0];
+						App.MotorPara.RU_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightUp_Org_AdZero - App.RightUpCS1237_TrendArray[0];
+						App.MotorPara.RL_HeightStress[(App.MotorPara.Current_height)-720] = App.ADTrack.RightLow_Org_AdZero - App.RightLowCS1237_TrendArray[0];
+						App.MotorPara.CaliNum++;
+					}
+				}
+			}
+			memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff));
+			App.RxUart3Data.RxLen = 0;
+			App.Uart3RxTimeOut1mSCnt = 0;
+			App.RxUart3Data.HeaderFrame = false;
+		}
+	}
+}
+#endif
 void RtcPeriod_IrqCallback(void)
 {
     //PORT_Toggle(LED_Green_PORT, LED_Green_PIN);