新增MMC5983磁传感器驱动模块及Keil工程配置更新

2026年3月24日，兴先达设备调试版本，校准算法初版
通过高度校准应力初版
2026-06-22 10:15:34 +08:00 · 2026-03-30 14:46:47 +08:00 · 2026-03-26 10:11:46 +08:00 · 2026-03-13 20:00:20 +08:00 · 2026-02-27 15:11:26 +08:00 · 2025-11-03 16:44:59 +08:00
14 changed files with 2153 additions and 435 deletions
@@ -103,7 +103,7 @@
        <bEvRecOn>1</bEvRecOn>
        <bSchkAxf>0</bSchkAxf>
        <bTchkAxf>0</bTchkAxf>
-        <nTsel>3</nTsel>
+        <nTsel>12</nTsel>
        <sDll></sDll>
        <sDllPa></sDllPa>
        <sDlgDll></sDlgDll>
@@ -117,6 +117,11 @@
        <pMon>BIN\CMSIS_AGDI.dll</pMon>
      </DebugOpt>
      <TargetDriverDllRegistry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>JL2CM3</Key>
+          <Name>-U12345678 -O78 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8027 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO31 -FD1FFF8000 -FC1000 -FN2 -FF0HC32F460_512K.FLM -FS00 -FL080000 -FP0($$Device:HC32F460JETA$FlashARM\HC32F460_512K.FLM) -FF1HC32F460_otp.FLM -FS13000C00 -FL13FC -FP1($$Device:HC32F460JETA$FlashARM\HC32F460_otp.FLM)</Name>
+        </SetRegEntry>
        <SetRegEntry>
          <Number>0</Number>
          <Key>ARMRTXEVENTFLAGS</Key>
@@ -208,7 +213,102 @@
        <Ww>
          <count>11</count>
          <WinNumber>1</WinNumber>
-          <ItemText>DcMotorDispEn</ItemText>
+          <ItemText>App.StopCollDelay1mSCnt,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>12</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.SlopeCollDelay1mSCnt,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>13</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.EmergencyDelay1mSCnt,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>14</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.MotorPara.LU_HeightStress,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>15</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.MotorPara.LL_HeightStress,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>16</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.MotorPara.RU_HeightStress,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>17</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.MotorPara.RL_HeightStress,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>18</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.TD1mSDelayCnt,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>19</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.RxUart3Data.RxBuff,0x10</ItemText>
+        </Ww>
+        <Ww>
+          <count>20</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.RxUart3Data.RxLen,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>21</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.Status</ItemText>
+        </Ww>
+        <Ww>
+          <count>22</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.MotorPara.CaliNum,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>23</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.CailUp1mSCnt,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>24</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>Oru_diff1,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>25</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>Orl_diff1,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>26</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.ADTrack.Stop_VPT,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>27</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>ru_ad[0],0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>28</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.MotorPara.RestFlag,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>29</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.MotorPara.Current_height,0x0A</ItemText>
+        </Ww>
+        <Ww>
+          <count>30</count>
+          <WinNumber>1</WinNumber>
+          <ItemText>App.HeightContRolCnt,0x0A</ItemText>
        </Ww>
      </WatchWindow1>
      <Tracepoint>
@@ -1018,7 +1118,7 @@

  <Group>
    <GroupName>CS1237</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
    <tvExpOptDlg>0</tvExpOptDlg>
    <cbSel>0</cbSel>
    <RteFlg>0</RteFlg>
@@ -10,13 +10,14 @@
      <TargetName>Debug</TargetName>
      <ToolsetNumber>0x4</ToolsetNumber>
      <ToolsetName>ARM-ADS</ToolsetName>
-      <pCCUsed>5060960::V5.06 update 7 (build 960)::.\ARMCC</pCCUsed>
+      <pArmCC>5060960::V5.06 update 7 (build 960)::ARMCC</pArmCC>
+      <pCCUsed>5060960::V5.06 update 7 (build 960)::ARMCC</pCCUsed>
      <uAC6>0</uAC6>
      <TargetOption>
        <TargetCommonOption>
          <Device>HC32F460JETA</Device>
          <Vendor>HDSC</Vendor>
-          <PackID>HDSC.HC32F460.1.0.11</PackID>
+          <PackID>HDSC.HC32F460.1.0.12</PackID>
          <PackURL>https://raw.githubusercontent.com/hdscmcu/pack/master/</PackURL>
          <Cpu>IRAM(0x1FFF8000,0x2F000) IRAM2(0x200F0000,0x1000) IROM(0x00000000,0x80000) IROM2(0x03000C00,0x003FC) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
          <FlashUtilSpec></FlashUtilSpec>
@@ -694,7 +695,7 @@
        <TargetCommonOption>
          <Device>HC32F460PETB</Device>
          <Vendor>HDSC</Vendor>
-          <PackID>HDSC.HC32F460.1.0.11</PackID>
+          <PackID>HDSC.HC32F460.1.0.12</PackID>
          <PackURL>https://raw.githubusercontent.com/hdscmcu/pack/master/</PackURL>
          <Cpu>IROM(0x00000000,0x80000) IROM2(0x03000C00,0x3FC) IRAM2(0x200F0000,0x1000) IROM(0x00000000,0x80000) CPUTYPE("Cortex-M4") FPU2 CLOCK(12000000) ELITTLE</Cpu>
          <FlashUtilSpec></FlashUtilSpec>
@@ -120,10 +120,10 @@ static void Set_CS1237_Config(uint8_t ad_reg)
    }
    /* 8: clk46 */
 	 CS1237_Clock();
-    CS1237_DOUT_H();
+   CS1237_DOUT_H();
 	 CS1237_DOUT_IN();
 	 CS1237IntEnable();
-    return ;
+   return ;
 }

 /**读CS1237寄存器*/
@@ -65,6 +65,9 @@ void DCMOTORDBGOnOff(uint8_t OnOff)
 #define TIMER4_PWM_H_PIN                (Pin09)
 #define TIMER4_PWM_L_PORT               (PortB)         /* TIM4_1_OUL_B:PE8   TIM4_1_OVL_B:PE10   TIM4_1_OWL_B:PE12 */
 #define TIMER4_PWM_L_PIN                (Pin14)
+
+en_timer4_oco_ch_t enOcoLowCh = (en_timer4_oco_ch_t)(TIMER4_OCO_HIGH_CH + 1);
+
 #endif


@@ -355,10 +358,10 @@ void AdcLoopHander(void)
            DMA_Cmd(M4_DMA1, Disable);
            calc_adc_val(g_adc_val);                  /* 计算ADC的平均值 */
            
-            DCMOTOR_DBG_LOG("ADC1_IN1 value %d.\t Temp:%.1fC \r\n", g_adc_val[0u], get_temp((float)g_adc_val[0]*Adc));
-            DCMOTOR_DBG_LOG("ADC2_IN2 value %d.\t Valtage:%.1fV \r\n", g_adc_val[1u], (float)g_adc_val[1]*ADC2VBUS);
-            temp_c = (g_adc_val[2]-init_adc_val)*ADC2CURT;
-            DCMOTOR_DBG_LOG("ADC3_IN3 value %d.\t Current:%.1fmA \r\n", g_adc_val[2u], temp_c);
+//            DCMOTOR_DBG_LOG("ADC1_IN1 value %d.\t Temp:%.1fC \r\n", g_adc_val[0u], get_temp((float)g_adc_val[0]*Adc));
+//            DCMOTOR_DBG_LOG("ADC2_IN2 value %d.\t Valtage:%.1fV \r\n", g_adc_val[1u], (float)g_adc_val[1]*ADC2VBUS);
+//            temp_c = (g_adc_val[2]-init_adc_val)*ADC2CURT;
+//            DCMOTOR_DBG_LOG("ADC3_IN3 value %d.\t Current:%.1fmA \r\n", g_adc_val[2u], temp_c);
            
 //            g_motor_data.current = temp_c;
            
@@ -367,7 +370,7 @@ void AdcLoopHander(void)
            {
                g_motor_data.current = 0.0;
            }
-            DCMOTOR_DBG_LOG("\t\t\t g_motor_data.current:%.1fmA \r\n", g_motor_data.current);
+ //           DCMOTOR_DBG_LOG("\t\t\t g_motor_data.current:%.1fmA \r\n", g_motor_data.current);
            DMA_Cmd(M4_DMA1, Enable);
        }
      }
@@ -375,7 +378,7 @@ void AdcLoopHander(void)



-
+#if(0)
 void Timer41_Init(uint8_t dir)
 {
    uint8_t E_OC_Port = 0;
@@ -516,9 +519,131 @@ void Timer41_Init(uint8_t dir)
    TIMER4_CNT_Start(TIMER4_UNIT);
    
 }
+#endif


+#if(1)

+void Timer41_Init(void)
+{
+    stc_timer4_cnt_init_t stcCntInit;
+    stc_timer4_oco_init_t stcOcoInit;
+    stc_timer4_pwm_init_t stcPwmInit;
+    stc_oco_low_ch_compare_mode_t stcLowChCmpMode;
+    stc_oco_high_ch_compare_mode_t stcHighChCmpMode;
+    uint16_t u8OcoOccrVal = TIMER4_CNT_CYCLE_VAL/10u;
+    en_timer4_oco_ch_t enOcoLowCh = (en_timer4_oco_ch_t)(TIMER4_OCO_HIGH_CH + 1);
+
+    /* Clear structures */
+    MEM_ZERO_STRUCT(stcCntInit);
+    MEM_ZERO_STRUCT(stcOcoInit);
+    MEM_ZERO_STRUCT(stcLowChCmpMode);
+    MEM_ZERO_STRUCT(stcHighChCmpMode);
+    MEM_ZERO_STRUCT(stcPwmInit);
+
+    /* Enable peripheral clock */
+    PWC_Fcg2PeriphClockCmd(PWC_FCG2_PERIPH_TIM41, Enable);
+
+    /* Timer4 CNT : Initialize CNT configuration structure */
+    stcCntInit.enBufferCmd = Disable;
+    stcCntInit.enClk = Timer4CntPclk;
+    stcCntInit.enClkDiv = Timer4CntPclkDiv4;  /* CNT clock divide */
+    stcCntInit.u16Cycle = TIMER4_CNT_CYCLE_VAL;
+//    stcCntInit.enCntMode = Timer4CntSawtoothWave;           //锯齿波模式
+    stcCntInit.enCntMode = Timer4CntTriangularWave;           //三角波模式Timer4CntTriangularWave
+    stcCntInit.enZeroIntCmd = Disable;
+    stcCntInit.enPeakIntCmd = Disable;
+    stcCntInit.enZeroIntMsk = Timer4CntIntMask0;
+    stcCntInit.enPeakIntMsk = Timer4CntIntMask0;
+    TIMER4_CNT_Init(TIMER4_UNIT, &stcCntInit); /* Initialize CNT */
+
+    /* Timer4 OCO : Initialize OCO configuration structure */
+    stcOcoInit.enOccrBufMode = OccrBufDisable;
+    stcOcoInit.enOcmrBufMode = OcmrBufDisable;
+    stcOcoInit.enPortLevel = OcPortLevelHigh;            /*输出高电平到OC端口*/
+    stcOcoInit.enOcoIntCmd = Disable;
+    TIMER4_OCO_Init(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, &stcOcoInit); /* Initialize OCO high channel */
+    TIMER4_OCO_Init(TIMER4_UNIT, enOcoLowCh, &stcOcoInit);         /* Initialize OCO low channel */
+
+    if (!(TIMER4_OCO_HIGH_CH % 2))
+    {
+        /* ocmr[15:0] = 0x225F */
+        stcHighChCmpMode.enCntZeroNotMatchOpState = OcoOpOutputHold;            ///< b15~b14
+        stcHighChCmpMode.enCntPeakNotMatchOpState = OcoOpOutputLow;             ///< b13~b12
+        stcHighChCmpMode.enCntZeroMatchOpState = OcoOpOutputHold;               ///< b11~b10
+        stcHighChCmpMode.enCntUpCntMatchOpState = OcoOpOutputLow;               ///< b9~b8
+        stcHighChCmpMode.enCntPeakMatchOpState = OcoOpOutputHigh;               ///< b7~b6
+        stcHighChCmpMode.enCntDownCntMatchOpState = OcoOpOutputHigh;            ///< b5~b4
+
+        stcHighChCmpMode.enCntZeroMatchOcfState = OcoOcfSet;                    ///< b3
+        stcHighChCmpMode.enCntUpCntMatchOcfState = OcoOcfSet;                   ///< b2
+        stcHighChCmpMode.enCntPeakMatchOcfState = OcoOcfSet;                    ///< b1
+        stcHighChCmpMode.enCntDownCntMatchOcfState = OcoOcfSet;                 ///< b0
+
+        stcHighChCmpMode.enMatchConditionExtendCmd = Disable;
+
+        TIMER4_OCO_SetHighChCompareMode(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, &stcHighChCmpMode);  /* Set OCO high channel compare mode */
+    }
+
+    /*************Timer4 OCO ocmr1[31:0] = 0x0FF0 0FFF*****************************/
+    if (enOcoLowCh % 2)
+    {
+        /* OCMR[31:0] Ox 2250 225F    0000 1111 1111 0000   0000 1111 1111 1111 */
+        stcLowChCmpMode.enCntZeroLowMatchHighMatchLowChOpState = OcoOpOutputHold;         /* bit[27:26]  00 */
+        stcLowChCmpMode.enCntZeroLowMatchHighNotMatchLowChOpState = OcoOpOutputHold;      /* bit[11:10]  00 */
+        stcLowChCmpMode.enCntZeroLowNotMatchHighMatchLowChOpState = OcoOpOutputHold;         /* bit[31:30]  00 */
+        stcLowChCmpMode.enCntZeroLowNotMatchHighNotMatchLowChOpState = OcoOpOutputHold;      /* bit[15:14]  00 */
+
+        stcLowChCmpMode.enCntUpCntLowMatchHighMatchLowChOpState = OcoOpOutputLow;        /* bit[25:24]  10 */
+        stcLowChCmpMode.enCntUpCntLowMatchHighNotMatchLowChOpState = OcoOpOutputLow;     /* bit[9:8]    10 */
+        stcLowChCmpMode.enCntUpCntLowNotMatchHighMatchLowChOpState = OcoOpOutputHold;        /* bit[19:18]  00 */
+
+        stcLowChCmpMode.enCntPeakLowMatchHighMatchLowChOpState = OcoOpOutputHigh;         /* bit[23:22]  01 */
+        stcLowChCmpMode.enCntPeakLowMatchHighNotMatchLowChOpState = OcoOpOutputHigh;      /* bit[7:6]    01 */
+        stcLowChCmpMode.enCntPeakLowNotMatchHighMatchLowChOpState = OcoOpOutputLow;         /* bit[29:28]  10 */
+        stcLowChCmpMode.enCntPeakLowNotMatchHighNotMatchLowChOpState = OcoOpOutputLow;      /* bit[13:12]  10 */
+
+        stcLowChCmpMode.enCntDownLowMatchHighMatchLowChOpState = OcoOpOutputHigh;         /* bit[21:20]  01 */
+        stcLowChCmpMode.enCntDownLowMatchHighNotMatchLowChOpState = OcoOpOutputHigh;      /* bit[5:4]    01 */
+        stcLowChCmpMode.enCntDownLowNotMatchHighMatchLowChOpState = OcoOpOutputHold;         /* bit[17:16]  00 */
+
+        stcLowChCmpMode.enCntZeroMatchOcfState = OcoOcfSet;    /* bit[3] 1 */
+        stcLowChCmpMode.enCntUpCntMatchOcfState = OcoOcfSet;   /* bit[2] 1 */
+        stcLowChCmpMode.enCntPeakMatchOcfState = OcoOcfSet;    /* bit[1] 1 */
+        stcLowChCmpMode.enCntDownCntMatchOcfState = OcoOcfSet; /* bit[0] 1 */
+
+        TIMER4_OCO_SetLowChCompareMode(TIMER4_UNIT, enOcoLowCh, &stcLowChCmpMode);  /* Set OCO low channel compare mode */
+    }
+
+//    /* Set OCO compare value */
+    TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, 0);
+    TIMER4_OCO_WriteOccr(TIMER4_UNIT, enOcoLowCh, 0);
+
+    /* Enable OCO */
+
+    TIMER4_OCO_OutputCompareCmd(TIMER4_UNIT, enOcoLowCh, Enable);
+    TIMER4_OCO_OutputCompareCmd(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, Enable);
+
+    /* Initialize PWM I/O */
+    PORT_SetFunc(TIMER4_PWM_H_PORT, TIMER4_PWM_H_PIN, Func_Tim4, Disable);
+    PORT_SetFunc(TIMER4_PWM_L_PORT, TIMER4_PWM_L_PIN, Func_Tim4, Disable);
+
+    /* Timer4 PWM: Initialize PWM configuration structure */
+    //重载定时器中断
+    stcPwmInit.enRtIntMaskCmd = Enable;
+    //计数时钟分频
+    stcPwmInit.enClkDiv = PwmPlckDiv1;      /*滤波计数器和死区计数器的计数时钟分频*/
+    //PWM输出极性控制
+    stcPwmInit.enOutputState = PwmHPwmLReverse;    /*输出PWMH信号不改变电平，输出PWML信号反转*/
+    //PWM输出模式
+    stcPwmInit.enMode = PwmThroughMode;     /*直通模式*/
+    TIMER4_PWM_Init(TIMER4_UNIT, TIMER4_PWM_CH, &stcPwmInit); /* Initialize timer4 pwm */
+
+    /* Clear && Start CNT */
+    TIMER4_CNT_Start(TIMER4_UNIT);
+}
+
+#endif

 void Timer61A_Init(void)
 {
@@ -691,10 +816,12 @@ void dcmotor_init(void)
 //	PORT_SetBits(PortA, Pin03);
 //     /* SD拉低，关闭输出 */
 //    
-//    dcmotor_stop();                 /* 停止电机 */
-//    dcmotor_dir(0);                 /* 设置正转 */
-//    dcmotor_speed(0);               /* 速度设置为0 */
-//    dcmotor_start();                /* 开启电机 */
+    Timer41_Init();
+    
+    dcmotor_stop();                 /* 停止电机 */
+    dcmotor_dir(0);                 /* 设置正转 */
+    dcmotor_speed(0);               /* 速度设置为0 */
+    dcmotor_start();                /* 开启电机 */
 }

 /**
@@ -719,7 +846,10 @@ void dcmotor_stop(void)
    #endif
    #if(TIMER == TIMER4)
    
-    TIMER4_CNT_Stop(TIMER4_UNIT);
+    TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, 0);
+    TIMER4_OCO_WriteOccr(TIMER4_UNIT, enOcoLowCh, 0u);
+    DISABLE_MOTOR;
+    
    #endif
 }

@@ -751,17 +881,19 @@ void dcmotor_dir(uint8_t para)
    #endif
    
    #if(TIMER == TIMER4)
+    TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, 0);
+    TIMER4_OCO_WriteOccr(TIMER4_UNIT, enOcoLowCh, 0u);
    if (para == 0)                /* 正转 */
    {
-
-
-        Timer41_Init(1);
-
+        TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, 0);
+        TIMER4_OCO_WriteOccr(TIMER4_UNIT, enOcoLowCh, 0u);
    }
    else if (para == 1)           /* 反转 */
    {
-        Timer41_Init(0);
+        TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, 0);
+        TIMER4_OCO_WriteOccr(TIMER4_UNIT, enOcoLowCh, 0u);
    }
+    dcmotor_start();
    #endif
 }

@@ -782,8 +914,8 @@ void dcmotor_speed(uint16_t para)
        #endif
        
        #if(TIMER == TIMER4)
-        TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, TIMER4_CNT_CYCLE_VAL - g_motorSpeed);     //正转
-        TIMER4_OCO_WriteOccr(TIMER4_UNIT, Timer4OcoOul, TIMER4_CNT_CYCLE_VAL - g_motorSpeed);         //反转
+        TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, g_motorSpeed);     //正转
+        TIMER4_OCO_WriteOccr(TIMER4_UNIT, enOcoLowCh, g_motorSpeed);         //反转
        #endif
        dcmotor_stop();
        return;
@@ -797,10 +929,7 @@ void dcmotor_speed(uint16_t para)
        #endif
        
        #if(TIMER == TIMER4)
-//        TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, TIMER4_CNT_CYCLE_VAL - g_motorSpeed);     //正转
-//        TIMER4_OCO_WriteOccr(TIMER4_UNIT, Timer4OcoOul, TIMER4_CNT_CYCLE_VAL - g_motorSpeed);         //反转
-        TIMER4_OCO_WriteOccr(TIMER4_UNIT, Timer4OcoOul, g_motorSpeed);         //反转
-        
+        TIMER4_OCO_WriteOccr(TIMER4_UNIT, enOcoLowCh, g_motorSpeed);         //反转
        #endif
        
        DCMOTOR_DBG_LOG("REVERSE\tnewSpeed = %d\r\n", g_motorSpeed);
@@ -814,7 +943,6 @@ void dcmotor_speed(uint16_t para)
        
        #if(TIMER == TIMER4)
        TIMER4_OCO_WriteOccr(TIMER4_UNIT, TIMER4_OCO_HIGH_CH, g_motorSpeed);     //正转
-//        TIMER4_OCO_WriteOccr(TIMER4_UNIT, Timer4OcoOul, TIMER4_CNT_CYCLE_VAL - g_motorSpeed);         //反转
        #endif
        DCMOTOR_DBG_LOG("FORWARD\tnewSpeed = %d\r\n", g_motorSpeed);
    }
@@ -0,0 +1,130 @@
+# ---> uVision
+# git ignore file for Keil µVision Project
+
+# µVision 5 and µVision 4 Project screen layout file
+*.uvguix.*
+*.uvgui.*
+
+# Listing Files
+*.i
+*.lst
+*.m51
+*.m66
+*.map
+
+# Object Files
+*.axf
+*.b[0-2][0-9]
+*.b3[0-1]
+*.bak
+*.build_log.htm
+*.crf
+*.d
+*.dep
+*.elf
+*.htm
+*.iex
+*.lnp
+*.o
+*.obj
+*.sbr
+
+# Firmware Files
+*.bin
+*.h86
+*.hex
+
+# Build Files
+.bat
+
+# Debugger Files
+.ini
+
+# JLink Files
+JLinkLog.txt
+
+# Other Files
+
+# ---> VisualStudioCode
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+!.vscode/*.code-snippets
+
+# Local History for Visual Studio Code
+.history/
+
+# Built Visual Studio Code Extensions
+*.vsix
+
+# ---> IAR
+# Compiled binaries
+*.o
+*.bin
+*.elf
+*.hex
+*.map
+*.out
+*.obj
+
+# Trash
+*.bak
+thumbs.db
+*.~*
+
+# IAR Settings  
+**/settings/*.crun  
+**/settings/*.dbgdt  
+**/settings/*.cspy  
+**/settings/*.cspy.*  
+**/settings/*.xcl  
+**/settings/*.dni  
+**/settings/*.wsdt  
+**/settings/*.wspos  
+
+# IAR Debug Exe  
+**/Exe/*.sim  
+
+# IAR Debug Obj  
+**/Obj/*.pbd  
+**/Obj/*.pbd.*  
+**/Obj/*.pbi  
+**/Obj/*.pbi.*  
+
+# IAR project "Debug" directory
+Debug/
+
+# IAR project "Release" directory
+Release/
+
+# IAR project settings directory
+settings/
+
+# IAR backup files
+Backup*
+
+# IAR .dep files
+*.dep
+# ---> IAR_EWARM
+# gitignore template for the IAR EWARM
+# website: https://www.iar.com/knowledge/support/technical-notes/ide/which-files-should-be-version-controlled/
+
+# Some tools will put the EWARM files
+# under a subdirectory with the same name
+# as the configuration.
+# Example
+# EWARM/Config1/Obj /List /Exe
+# EWARM/Config2/Obj /List /Exe
+EWARM/**/Obj
+EWARM/**/List
+EWARM/**/Exe
+
+# Autogenerated project files
+*.dep
+*.ewt
+
+# Autogenerated folder for debugger
+EWARM/settings
+
@@ -0,0 +1,3 @@
+# MMC5983
+
+MMC5983驱动代码
@@ -0,0 +1,343 @@
+#include "main.h"
+#include "mmc5983.h"
+#include "UartDebug.h"
+#include "i2c.h"
+#include "Algorithm.h"
+
+static int32_t max_in[XYZ];
+static int32_t min_in[XYZ];
+static bool InitFlag;
+static bool MMC5983IntFlag;
+static MMC5983Var_t MMC5983Var;
+
+/*****************************************************************************************
+* 函数名称: MMC5983_Write_Reg
+* 功能描述: MMC5983写寄存器
+* 参    数: reg 寄存器地址
+						val 寄存器数据
+* 返 回 值: 成功返回true，失败返回false
+*****************************************************************************************/
+static bool MMC5983_Write_Reg(uint8_t reg, uint8_t val)
+{
+	return I2C_MasterWriteData(M0P_I2C0, MMC5983_ADDRESS, reg, &val, 1);
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983_Read_Reg
+* 功能描述: MMC5983读寄存器
+* 参    数: reg 寄存器地址
+* 返 回 值: 返回寄存器值
+*****************************************************************************************/
+static uint8_t MMC5983_Read_Reg(uint8_t reg)
+{
+	uint8_t res;
+	if(I2C_MasterReadData(M0P_I2C0, MMC5983_ADDRESS, reg, &res, 1) == false)
+		res = 0;
+	return res;
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983_Read_Buffer
+* 功能描述: MMC5983读数据
+* 参    数: reg 寄存器地址
+						buffer 读取数据输出缓存入口
+						len 读取长度
+* 返 回 值: 成功返回true，失败返回false
+*****************************************************************************************/
+static bool MMC5983_Read_Buffer(uint8_t reg, void *buffer, uint8_t len)
+{
+	return I2C_MasterReadData(M0P_I2C0, MMC5983_ADDRESS, reg, buffer, len);
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983StartCov
+* 功能描述: MMC5983开启连续转换
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+static void MMC5983StartCov(void)
+{
+	MMC5983RegCtrl2 Ctrl2Reg;		//开启/关闭连续转换
+	Ctrl2Reg.Byte = 0x00;
+	Ctrl2Reg.Bit.Cm_freq = 0x04;		
+	Ctrl2Reg.Bit.Cmm_en = 1;		//连续设1，单次设0
+	Ctrl2Reg.Bit.Prd_set = 3;
+	Ctrl2Reg.Bit.En_prd_set = 1;	//连续设1，单次设0
+	MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983_Init
+* 功能描述: MMC5983初始化
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+static void MMC5983_Init(void)
+{
+	uint8_t id;
+	
+	id	= MMC5983_Read_Reg(MMC5983_ID);
+	//DBG_LOG("MMC5983 id=0x%x\r\n",id);
+	MMC5983Var.MMC5983Delay1mSCnt = 100;
+	while(id != 0x30){
+		if(MMC5983Var.MMC5983Delay1mSCnt <= 0) {
+			DBG_LOG("LEFT MMC5983 INIT Err...\r\n");
+			InitFlag = false;
+			return;
+		}
+	}
+	
+	MMC5983RegCtrl1 Ctrl1Reg;
+	Ctrl1Reg.Byte = 0x00;
+	Ctrl1Reg.Bit.SW_RST = 1;
+	MMC5983_Write_Reg(MMC5983_CTRL_1, Ctrl1Reg.Byte);
+	MMC5983Delay(15);
+	
+	MMC5983RegCtrl2 Ctrl2Reg;		//开启/关闭连续转换
+	Ctrl2Reg.Byte = 0x00;
+	Ctrl2Reg.Bit.Cm_freq = 0x04;		
+	Ctrl2Reg.Bit.Cmm_en = 0;		//连续设1，单次设0
+	Ctrl2Reg.Bit.Prd_set = 3;
+	Ctrl2Reg.Bit.En_prd_set = 0;	//连续设1，单次设0
+	MMC5983_Write_Reg(MMC5983_CTRL_2, Ctrl2Reg.Byte);
+	
+	MMC5983RegCtrl3 Ctrl3Reg;
+	Ctrl3Reg.Byte = 0x00;
+	Ctrl3Reg.Bit.St_enm = 1;
+	Ctrl3Reg.Bit.St_enp = 1;
+	MMC5983_Write_Reg(MMC5983_CTRL_3, Ctrl3Reg.Byte);
+	
+	MMC5983RegCtrl0 Ctrl0Reg;
+	Ctrl0Reg.Byte = 0x00;
+	Ctrl0Reg.Bit.TM_M = 1;
+	Ctrl0Reg.Bit.Auto_SR_en = 1;
+	Ctrl0Reg.Bit.NT_meas_done_en = 1;
+	MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
+	
+	if(InitFlag == false){
+		DBG_LOG("MMC5983 INIT OK...\r\n"); 
+		InitFlag = true;
+	}
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983_Read
+* 功能描述: MMC5983读取原始数据
+* 参    数: MagAdcX X轴数据
+				MagAdcY Y轴数据
+				MagAdcZ Z轴数据
+* 返 回 值: 成功返回true，失败返回false
+*****************************************************************************************/
+static bool MMC5983_Read(int32_t *MagAdcX, int32_t *MagAdcY, int32_t *MagAdcZ)
+{
+	static uint8_t buff[7];
+	
+	 if (MMC5983_Read_Buffer(MMC5983_X_OUT_H, buff, 7) == false)
+        return false;
+
+    *MagAdcX = (buff[0] << 10) | (buff[1] << 2) | ((buff[6] & 0xC0) >> 6);
+    *MagAdcY = (buff[2] << 10) | (buff[3] << 2) | ((buff[6] & 0x30) >> 4);
+    *MagAdcZ = (buff[4] << 10) | (buff[5] << 2) | ((buff[6] & 0x0C) >> 2);// Turn the 18 bits into unsigned 32-bit value
+    return true;
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983_Get_Mag
+* 功能描述: 外部获取MMC5983磁场强度数据
+* 参    数: mag 读取数据输出缓存入口
+* 返 回 值: 无
+*****************************************************************************************/
+void MMC5983_Get_Mag(float *mag)
+{
+	if(mag == NULL)
+		return;
+	
+	float ADVal[XYZ];
+	
+	ADVal[X] = MMC5983Var.Mag_Original[X] - 131072.0f; 
+	ADVal[Y] = MMC5983Var.Mag_Original[Y] - 131072.0f; 
+	ADVal[Z] = MMC5983Var.Mag_Original[Z] - 131072.0f; 
+	
+	mag[0] = (float) (ADVal[X] * MMC5983_MAG_SCALE_18BIT);
+   mag[1] = (float) (ADVal[Y] * MMC5983_MAG_SCALE_18BIT);
+	mag[2] = (float) (ADVal[Z] * MMC5983_MAG_SCALE_18BIT);
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983LoopHandler
+* 功能描述: MMC5983循环处理函数
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+extern void MmcDataProcessCallBack(float *Mag);
+
+void MMC5983LoopHandler(void)
+{
+	static int32_t mag_array[XYZ][10];
+	static uint8_t magcnt;
+	static uint8_t TimeOutMagcnt;
+	
+	switch(MMC5983Var.Status) {
+		case MMC5983_STATUS_IDLE:{
+		break;}
+		
+		case MMC5983_STATUS_START:{
+			for(int i = 0; i < 10; i++) {
+					mag_array[X][i] = 0;
+					mag_array[Y][i] = 0;
+					mag_array[Z][i] = 0;
+				}		
+			magcnt = 0;
+			TimeOutMagcnt = 0;
+			memset(&MMC5983Var, NULL, sizeof(MMC5983Var));
+			MMC5983Var.Status = MMC5983_STATUS_DELAY;
+		break;}
+		
+		case MMC5983_STATUS_DELAY:{
+			MMC5983StartRead();
+			MMC5983Var.MMC5983Delay1mSCnt = 50;
+			MMC5983Var.Status = MMC5983_STATUS_WAIT_DELAY;	
+		break;}
+		
+		case MMC5983_STATUS_WAIT_DELAY:{
+			if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
+				MMC5983Var.Status = MMC5983_STATUS_READ;	
+			}
+		break;}
+			
+		case MMC5983_STATUS_READ:{
+			MMC5983Var.MMC5983Delay1mSCnt = 50;
+			MMC5983Var.Status = MMC5983_STATUS_WAIT_READ;
+		break;}
+		
+		case	MMC5983_STATUS_WAIT_READ:{
+			if(MMC5983Var.MMC5983Delay1mSCnt <= 0){
+				MMC5983Var.Status = MMC5983_STATUS_DELAY;
+				TimeOutMagcnt++;
+			}
+			if(MMC5983IntFlag == true){
+				MMC5983IntFlag = false;
+				MMC5983_Read(&mag_array[X][magcnt], &mag_array[Y][magcnt], &mag_array[Z][magcnt]);
+				magcnt++;
+				MMC5983Var.Status = MMC5983_STATUS_DELAY;
+			}
+			if(TimeOutMagcnt >= 10){
+				TimeOutMagcnt = 0;
+				if(magcnt > 0){
+					MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
+					MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
+					MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
+					magcnt = 0;
+					MMC5983_Get_Mag(MMC5983Var.Mag);
+					MmcDataProcessCallBack(MMC5983Var.Mag);
+					MMC5983Var.Status = MMC5983_STATUS_STOP;
+				}
+				else{
+					MMC5983Var.Status = MMC5983_STATUS_STOP;
+//					DBG_LOG("LEFT MMC5983 TimeOut...\r\n"); 
+				}
+			}
+			if(magcnt >= 10){
+				MMC5983Var.Mag_Original[X] = AverageFilter_32t(mag_array[X], magcnt);
+				MMC5983Var.Mag_Original[Y] = AverageFilter_32t(mag_array[Y], magcnt);
+				MMC5983Var.Mag_Original[Z] = AverageFilter_32t(mag_array[Z], magcnt);
+				magcnt = 0;
+				MMC5983_Get_Mag(MMC5983Var.Mag);
+				MmcDataProcessCallBack(MMC5983Var.Mag);
+				MMC5983Var.Status = MMC5983_STATUS_STOP;
+			}
+		break;}
+		
+		case MMC5983_STATUS_STOP:{
+			MMC5983Stop();
+			MMC5983Var.Status = MMC5983_STATUS_IDLE;
+			//MMC5983Var.Status = MMC5983_STATUS_START;//持续工作
+		break;}
+	}
+}
+
+/*****************************************************************************************
+* 函数名称: LeftMMC59831mSRoutine
+* 功能描述: MMC59831ms时基循环处理函数
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+void MMC59831mSRoutine(void)
+{
+	if(MMC5983Var.MMC5983Delay1mSCnt > 0) 
+		MMC5983Var.MMC5983Delay1mSCnt--;
+}
+
+/*****************************************************************************************
+* 函数名称: LeftMMC5983Init
+* 功能描述: MMC5983初始化
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+void MMC5983Init(void)
+{
+	if(!InitFlag){
+		MMC5983_Init();
+		return;
+	}
+	
+	MMC5983Var.Status = MMC5983_STATUS_START;
+}
+
+/*****************************************************************************************
+* 函数名称: LeftMMC5983Start
+* 功能描述: MMC5983启动
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+void MMC5983Start(void)
+{
+	if(!InitFlag){
+		MMC5983_Init();
+		return;
+	}
+	
+	MMC5983Var.Status = MMC5983_STATUS_START;
+}
+
+/*****************************************************************************************
+* 函数名称: LeftMMC5983StartRead
+* 功能描述: MMC5983启动读取
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+void MMC5983StartRead(void)
+{	
+	MMC5983_Init();//读取时重新初始化
+}
+
+/*****************************************************************************************
+* 函数名称: MMC5983Stop
+* 功能描述: MMC5983停止
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+void MMC5983Stop(void)
+{	
+	MMC5983RegCtrl0 Ctrl0Reg;
+	Ctrl0Reg.Byte = 0x00;
+	MMC5983_Write_Reg(MMC5983_CTRL_0, Ctrl0Reg.Byte);
+	
+	MMC5983RegStatus StatusReg;
+	StatusReg.Byte = MMC5983_Read_Reg(MMC5983_STATUS);
+	if(StatusReg.Bit.Meas_M_Done == 1){
+		MMC5983_Write_Reg(MMC5983_STATUS, StatusReg.Byte);
+	}
+}
+
+/*****************************************************************************************
+* 函数名称: LeftMMC5983IntCallBack
+* 功能描述: MMC5983中断回调
+* 参    数: 无
+* 返 回 值: 无
+*****************************************************************************************/
+void MMC5983IntCallBack(void)
+{
+	MMC5983IntFlag = true;
+}
+
@@ -0,0 +1,140 @@
+/**
+  *****************************************************************************
+  * @file				ak8975.h
+  * @author				WWJ
+  * @version			v1.0
+  * @date				2019/04/09
+  * @environment		stm32f407
+  * @brief   
+  *****************************************************************************
+**/
+
+#ifndef _MMC5983_H
+#define	_MMC5983_H
+#include "bsp.h"
+
+#define	X												0
+#define	Y												1
+#define	Z												2
+#define	XYZ											3
+
+#define	MAX(A, B)								(A > B) ? A : B;	
+#define	MIN(A, B)								(A < B) ? A : B;
+
+#define MMC5983_X_OUT_H					0X00 				//Device ID = 0x48
+#define MMC5983_X_OUT_L					0X01 
+#define MMC5983_Y_OUT_H					0X02 
+#define MMC5983_Y_OUT_L					0X03 
+#define MMC5983_Z_OUT_H					0X04
+#define MMC5983_Z_OUT_L					0X05
+#define MMC5983_XYZ_OUT					0X06
+#define MMC5983_TMP_OUT					0X07
+#define MMC5983_STATUS					0X08
+#define MMC5983_CTRL_0					0X09 
+#define MMC5983_CTRL_1					0X0A 
+#define MMC5983_CTRL_2					0X0B
+#define MMC5983_CTRL_3					0X0C 
+#define MMC5983_ID		          	0X2F
+
+#define MMC5983_ADDRESS					0x30
+#define MMC5983_MAG_SCALE_16BIT		0.24414f
+#define MMC5983_MAG_SCALE_18BIT		0.061035f
+
+#define	MMC5983Delay(x)					delay_ms(x)
+
+typedef union {
+	struct {
+		uint8_t Meas_M_Done: 1;
+		uint8_t Meas_T_Done: 1;
+		uint8_t Res0: 2;
+		uint8_t OTP_Read_Done: 1;
+		uint8_t Res1: 3;
+	}Bit;
+	uint8_t Byte;
+}MMC5983RegStatus;
+
+typedef union {
+	struct {
+		uint8_t TM_M: 1;
+		uint8_t TM_T: 1;
+		uint8_t NT_meas_done_en: 1;
+		uint8_t Set: 1;
+		uint8_t Reset: 1;
+		uint8_t Auto_SR_en: 1;
+		uint8_t OTP_Read: 1;
+		uint8_t Res0: 1;
+	}Bit;
+	uint8_t Byte;
+}MMC5983RegCtrl0;
+
+typedef union {
+	struct {
+		uint8_t BW: 2;
+		uint8_t X_Inhibit: 1;
+		uint8_t YZ_Inhibit: 2;
+		uint8_t Res0: 2;
+		uint8_t SW_RST: 1;
+	}Bit;
+	uint8_t Byte;
+}MMC5983RegCtrl1;
+
+typedef union {
+	struct {
+		uint8_t Cm_freq: 3;
+		uint8_t Cmm_en: 1;
+		uint8_t Prd_set: 3;
+		uint8_t En_prd_set: 1;
+	}Bit;
+	uint8_t Byte;
+}MMC5983RegCtrl2;
+
+typedef union {
+	struct {
+		uint8_t Res0: 1;
+		uint8_t St_enp: 1;
+		uint8_t St_enm: 1;
+		uint8_t Res1: 3;
+		uint8_t Spi_3w: 1;
+		uint8_t Res2: 1;
+	}Bit;
+	uint8_t Byte;
+}MMC5983RegCtrl3;
+
+typedef enum{
+	MMC5983_STATUS_IDLE,
+	MMC5983_STATUS_START,
+	MMC5983_STATUS_DELAY,
+	MMC5983_STATUS_WAIT_DELAY,
+	MMC5983_STATUS_READ,
+	MMC5983_STATUS_WAIT_READ,
+	MMC5983_STATUS_STOP,
+}MMC5983Status_m;
+
+
+typedef struct {
+	MMC5983Status_m Status;
+	uint16_t MMC5983Delay1mSCnt;
+	float Mag[3];
+	int32_t Mag_Original[XYZ];
+}MMC5983Var_t;
+
+void MMC5983LoopHandler(void);
+void MMC59831mSRoutine(void);
+void MMC5983Init(void);
+void MMC5983Start(void);
+void MMC5983Stop(void);
+void MMC5983IntCallBack(void);
+
+void MMC5983Calib(void);
+void MMC5983GetADValue(int32_t *Mag);
+void MMC5983_Get_Mag(float *mag);
+MMC5983Status_m GetMMC5983Status(void);
+void MMC5983StartRead(void);
+bool GetMMC5983CalibStatus(void);
+#endif
+
+
+
+/* end of ak8975.h */
+
+
@@ -49,6 +49,11 @@ 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 fixWithMedianIterative(int arr[], int size, int windowSize, int threshold, int maxIterations, int verbose);
+int finalCheck(int arr[], int size, int threshold, int verbose);
+int weightedMovingAverageWithEnhance(int32_t arr[], int size, int windowSize,
+                                      int enhanceStart, int enhanceEnd, 
+                                      double enhanceFactor, int verbose);
 #endif


@@ -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
@@ -172,16 +204,18 @@
 #define	TWIN_BIT0_PINx							(Pin12)
 #define	SET_TWIN_BIT0()						PORT_SetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉高
 #define	CLR_TWIN_BIT0()						PORT_ResetBits(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)//拉低
+#define	GET_BIT0_DOUT()						PORT_GetBit(TWIN_BIT0_PORTx, TWIN_BIT0_PINx)

 //<		BIT1
 #define	TWIN_BIT1_PORTx						(PortB)
 #define	TWIN_BIT1_PINx							(Pin13)
 #define	SET_TWIN_BIT1()						PORT_SetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉高
 #define	CLR_TWIN_BIT1()						PORT_ResetBits(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)//拉低
+#define	GET_BIT1_DOUT()						PORT_GetBit(TWIN_BIT1_PORTx, TWIN_BIT1_PINx)

 typedef struct {
-	uint32_t ControlLevel;//控制等级
-	uint32_t	DBGWaveOut;//调试波形输出
+	int32_t ControlLevel;//控制等级
+	int32_t	DBGWaveOut;//调试波形输出
 }__attribute__ ((packed))LayoutPara_t, *LayoutPara;//上位机下发的配置参数

 typedef struct {
@@ -207,9 +241,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);
@@ -237,7 +277,7 @@ uint32_t mf_showfree(uint8_t *drv);
 void TimeGet(struct tm *cTime);
 int TimeTs(void);
 void TimeShow(uint32_t dwStamp);
-void TimeSync(time_t ts) ;
+void TimeSync(time_t ts);

 void delay_5ns(uint32_t ns);

@@ -253,3 +293,6 @@ void ExtInt06IntDisable(void);



+
+
+
@@ -8,28 +8,33 @@
 #define		SDCARD_BLOCK_MAX								(10000)

 #define 	OSC_CNT    			40
-#define 	SLOPE_CNT			  30
+#define 	SLOPE_CNT			  20

 #define 	SLOPE_VPT				1000
-#define		FOLLOW_VPT			20
-#define		SHIELD_VPT			2500
-#define		EME_STOP_VPT		10000
+#define		PRESS_VPT				3
+#define		FOLLOW_VPT			10
+#define		SHIELD_VPT			5000
+#define		EME_STOP_VPT		4000
+#define		CENTRE_VPT			2000

-#define 	STOP_TIME_MS		0
+#define 	STOP_TIME_MS		500
 #define 	RUN_TIME_MS			0
-#define		EME_START_MS		3000
-#define		EME_STOP_MS			3000
+#define		EME_START_MS		0
+#define		EME_STOP_MS			2000
+#define		SLOPE_START_MS	3000

 #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())
-//#define	BDC_DOWN()			(SET_TWIN_BIT1(),CLR_TWIN_BIT0())
+#define	BDC_STOP()			(SET_TWIN_BIT0(),SET_TWIN_BIT1())
+#define BDC_UP()				(SET_TWIN_BIT0(),CLR_TWIN_BIT1())
+#define	BDC_DOWN()			(SET_TWIN_BIT1(),CLR_TWIN_BIT0())

-#define	BDC_STOP()				Motor_Control(MOTOR_CMD_STOP)
-#define BDC_UP()					Motor_Control(MOTOR_CMD_FORWARD)
-#define	BDC_DOWN()				Motor_Control(MOTOR_CMD_REVERSE)
+//#define	BDC_STOP()				Motor_Control(MOTOR_CMD_STOP)
+//#define BDC_UP()					Motor_Control(MOTOR_CMD_FORWARD)
+//#define	BDC_DOWN()				Motor_Control(MOTOR_CMD_REVERSE)

 /*控制等级*/
 typedef enum {
@@ -60,7 +65,18 @@ typedef struct{
 	int32_t RightUp_Org_AdZero;//右上设备原始AD零点
 	int32_t RightLow_Org_AdZero;//右下设备原始AD零点
 }ADTrack_t;
-
+typedef struct{//电机参数
+	bool InitialFlag;//初始标志位
+	bool RestFlag;
+	bool CaliFlag;
+	bool LoopOne;//循环一次标志位
+	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,
@@ -81,10 +97,13 @@ typedef enum {
 	APP_STATUS_IDLE,
 	APP_STATUS_ON,
 	APP_STATUS_OFF,
-	APP_STATUS_UP1,
-	APP_STATUS_DOWN,
-	APP_STATUS_UP2,
-	APP_STATUS_WAIT,
+	APP_STATUS_MOTOR_INIT,
+	APP_STATUS_WAIT_MOTOR_INIT,
+	APP_STATUS_WAIT_MOTOR_CALI_UP,
+	APP_STATUS_WAIT_MOTOR_CALI_DOWN,
+	APP_STATUS_WAIT_STEADY,
+	APP_STATUS_ZERO_MARKING,
+	APP_STATUS_HEIGHT_CONTROL,
 }AppStatus_m;

 typedef struct{
@@ -94,21 +113,36 @@ 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;
+	uint32_t SlopeCollDelay1mSCnt;
+	uint32_t CailUp1mSCnt;
+	uint32_t HeightContRolCnt;
 	uint8_t FeedDogDlyCnt;
 	
+	bool CaliZeroFlag;//校零标志位
+	bool SideFlag;//侧压标志位
 	bool UpFlag;//上升标志位
 	bool DownFlag;//下降标志位
 	bool StopFlag;//停止标志位
@@ -1,7 +1,281 @@
+#include <stdio.h>
+#include <stdlib.h>
 #include "bsp.h"
 #include "Algorithm.h"
 #include "Debug.h"
 												 
+/**
+ * 加强特定区域的平滑处理（使用更小的权重衰减，更平滑的效果）
+ * 
+ * @param arr 待处理的int32_t数组
+ * @param size 数组的大小
+ * @param windowSize 滑动窗口大小
+ * @param enhanceStart 加强区域的起始索引（包含）
+ * @param enhanceEnd 加强区域的结束索引（包含）
+ * @param enhanceFactor 加强因子（>1表示更强的平滑，建议2-5）
+ * @param verbose 是否打印详细调试信息
+ * @return 返回修正的次数
+ */
+int weightedMovingAverageWithEnhance(int32_t arr[], int size, int windowSize,
+                                      int enhanceStart, int enhanceEnd, 
+                                      double enhanceFactor, int verbose) {
+    if (size <= 1 || windowSize < 2) return 0;
+    
+    int32_t *temp = (int32_t*)malloc(size * sizeof(int32_t));
+    if (temp == NULL) {
+        DBG_LOG("内存分配失败！\n");
+        return 0;
+    }
+    
+    int halfWindow = windowSize / 2;
+    int fixCount = 0;
+    
+    if (verbose) {
+        DBG_LOG("加权移动平均平滑 (窗口大小: %d)\n", windowSize);
+        DBG_LOG("加强平滑区域: [%d - %d], 加强因子: %.1f\n", enhanceStart, enhanceEnd, enhanceFactor);
+        DBG_LOG("========================================\n");
+    }
+    
+    for (int i = 0; i < size; i++) {
+        // 判断是否在加强区域内
+        int isEnhanced = (i >= enhanceStart && i <= enhanceEnd);
+        
+        // 计算窗口边界
+        int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow);
+        int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow);
+        
+        // 计算加权平均值
+        double weightedSum = 0;
+        double weightSum = 0;
+        
+        for (int j = start; j <= end; j++) {
+            // 权重：距离越近权重越大
+            int distance = abs(j - i);
+            double weight = 1.0 / (distance + 1);
+            
+            // 如果在加强区域内，且当前点不是中心点，可以调整权重
+            if (isEnhanced && j != i) {
+                // 加强区域内的参考点权重提高，使平滑效果更强
+                weight *= enhanceFactor;
+            }
+            
+            weightedSum += arr[j] * weight;
+            weightSum += weight;
+        }
+        
+        int newValue = (int)(weightedSum / weightSum + 0.5);
+        
+        if (newValue != arr[i]) {
+            if (verbose && fixCount < 50) {
+                DBG_LOG("位置 [%d]: %d -> %d %s\n", i, arr[i], newValue, 
+                       isEnhanced ? "[加强区域]" : "");
+            }
+            temp[i] = newValue;
+            fixCount++;
+        } else {
+            temp[i] = arr[i];
+        }
+    }
+    
+    // 写回原数组
+    memcpy(arr, temp, size * sizeof(int32_t));
+    free(temp);
+    
+    if (verbose) {
+        DBG_LOG("========================================\n");
+        DBG_LOG("共修正 %d 个点（加强区域修正了更多点）\n", fixCount);
+    }
+    
+    return fixCount;
+}
+int finalCheck(int arr[], int size, int threshold, int verbose) {
+    int problemCount = 0;
+    
+    if (verbose) {
+        DBG_LOG("\n========== 最终检查 ==========\n");
+    }
+    
+    for (int i = 1; i < size - 1; i++) {
+        int diffPrev = abs(arr[i] - arr[i-1]);
+        int diffNext = abs(arr[i+1] - arr[i]);
+        
+        if (diffPrev > threshold && diffNext > threshold) {
+            problemCount++;
+            if (verbose) {
+                DBG_LOG("警告: 位置 [%d] 仍有突变问题 (前差%d, 后差%d)\n", i, diffPrev, diffNext);
+            }
+        }
+    }
+    
+    // 检查首尾
+    if (size >= 2) {
+        if (abs(arr[0] - arr[1]) > threshold * 2) {
+            problemCount++;
+            if (verbose) {
+                DBG_LOG("警告: 位置 [0] 仍有突变问题 (与第二元素差%d)\n", abs(arr[0] - arr[1]));
+            }
+        }
+        if (abs(arr[size-1] - arr[size-2]) > threshold * 2) {
+            problemCount++;
+            if (verbose) {
+                DBG_LOG("警告: 位置 [%d] 仍有突变问题 (与前元素差%d)\n", size-1, abs(arr[size-1] - arr[size-2]));
+            }
+        }
+    }
+    
+    if (verbose) {
+        if (problemCount == 0) {
+            DBG_LOG("检查通过！没有发现突变点\n");
+        } else {
+            DBG_LOG("发现 %d 个问题点\n", problemCount);
+        }
+        DBG_LOG("========================================\n");
+    }
+    
+    return problemCount;
+}
+/**
+ * 使用滑动窗口修正突变值，并打印修改信息
+ * 
+ * @param arr 待处理的int类型数组
+ * @param size 数组的大小
+ * @param windowSize 滑动窗口大小（建议奇数）
+ * @param threshold 突变阈值
+ * @return 返回修正的次数
+ */
+int fixWithMedianIterative(int arr[], int size, int windowSize, int threshold, int maxIterations, int verbose) {
+    if (arr == NULL || size <= 2 || windowSize < 3) {
+        return 0;
+    }
+    
+    int totalFixCount = 0;
+    int iteration = 0;
+    int changed = 1;
+    int halfWindow = windowSize / 2;
+    
+    int *temp = (int*)malloc(size * sizeof(int));
+    if (temp == NULL) {
+        DBG_LOG("内存分配失败！\n");
+        return 0;
+    }
+    
+    if (verbose) {
+        DBG_LOG("========== 开始迭代修正 (中位数窗口) ==========\n");
+        DBG_LOG("数组大小: %d, 窗口大小: %d, 阈值: %d, 最大迭代: %d\n", 
+               size, windowSize, threshold, maxIterations);
+        DBG_LOG("========================================\n");
+    }
+    
+    while (changed && iteration < maxIterations) {
+        changed = 0;
+        int iterFixCount = 0;
+        memcpy(temp, arr, size * sizeof(int));
+        
+        if (verbose) {
+            DBG_LOG("\n第 %d 次迭代:\n", iteration + 1);
+            DBG_LOG("----------------------------------------\n");
+        }
+        
+        // 处理所有点
+        for (int i = 0; i < size; i++) {
+            int isAbnormal = 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) {
+                    isAbnormal = 1;
+                }
+            } else if (i == 0 && size >= 2) {
+                // 首元素：检查与第二个元素的差异
+                if (abs(arr[0] - arr[1]) > threshold * 2) {
+                    isAbnormal = 1;
+                }
+            } else if (i == size - 1 && size >= 2) {
+                // 尾元素：检查与倒数第二个元素的差异
+                if (abs(arr[size-1] - arr[size-2]) > threshold * 2) {
+                    isAbnormal = 1;
+                }
+            }
+            
+            if (isAbnormal) {
+                // 计算窗口边界
+                int start = (i - halfWindow) < 0 ? 0 : (i - halfWindow);
+                int end = (i + halfWindow) >= size ? (size - 1) : (i + halfWindow);
+                
+                // 收集窗口内的值（排除自身）
+                int windowActualSize = end - start + 1;
+                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];
+                    }
+                }
+                
+                int correctedValue;
+                if (idx > 0) {
+                    // 排序找中位数
+                    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;
+                            }
+                        }
+                    }
+                    correctedValue = window[idx / 2];
+                } else {
+                    // 没有参考值，使用相邻点平均
+                    if (i > 0 && i < size - 1) {
+                        correctedValue = (arr[i-1] + arr[i+1]) / 2;
+                    } else if (i == 0) {
+                        correctedValue = arr[1];
+                    } else {
+                        correctedValue = arr[size-2];
+                    }
+                }
+                
+                if (correctedValue != arr[i]) {
+                    if (verbose) {
+                        DBG_LOG("位置 [%d]: %d -> %d (窗口[%d-%d], 中位数:%d)\n", 
+                               i, arr[i], correctedValue, start, end, correctedValue);
+                    }
+                    temp[i] = correctedValue;
+                    iterFixCount++;
+                    changed = 1;
+                }
+                
+                free(window);
+            }
+        }
+        
+        if (changed) {
+            memcpy(arr, temp, size * sizeof(int));
+            totalFixCount += iterFixCount;
+            if (verbose) {
+                DBG_LOG("本次修正了 %d 个点\n", iterFixCount);
+            }
+        }
+        
+        iteration++;
+    }
+    
+    free(temp);
+    
+    if (verbose) {
+        DBG_LOG("========================================\n");
+        DBG_LOG("迭代完成！共进行了 %d 次迭代，总修正 %d 个点\n", iteration, totalFixCount);
+        DBG_LOG("========================================\n");
+    }
+    
+    return totalFixCount;
+}
 /*****************************************************************************************
 * 函数名称: check_peaks_valleys_ratio
 * 功能描述: 检查数据的峰值是否大于0，谷值是否小于0（忽略0值）
@@ -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;
@@ -449,6 +521,8 @@ static void GPIO_Config(void)
 	stcPortInit.enPullUp = Enable;//上拉
 	PORT_Init(TWIN_BIT0_PORTx, TWIN_BIT0_PINx, &stcPortInit);
 	PORT_Init(TWIN_BIT1_PORTx, TWIN_BIT1_PINx, &stcPortInit);
+	SET_TWIN_BIT0();
+	SET_TWIN_BIT1();
 }
 /*********************************左上传感器
 *******************************************************************************
@@ -1356,16 +1430,19 @@ void BSP_Init(void)
 	ExtiCh06_Init();
 	ExtiCh04_Init();
 	
-	DbgUart_Config(500000);
+	DbgUart_Config(700000);
+#if(USE_USART3 == 1)
+	Uart3_Config(9600);
+#endif
 	//RS485Uart_Config(9600);
 	
 	//Rtc_Config();
 	//SdioInit();
 	//Spi_Config();
   // Spi3_Config();
-    dcmotor_init();
-	Adc1_Config();
-	Dma_Config();
+//    dcmotor_init();
+//	Adc1_Config();
+//	Dma_Config();
 	//FeedDog();

 	SysTick_Init(1000u);
@@ -1429,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();
@@ -1467,11 +1547,8 @@ __WEAK void RS485_RxPinIntCallBack(void)
 {
    
 }
-
-
-
-
 #endif
+#if(USE_DEBUG == 1)
 __WEAK void DBGUsartErrIrqCallback(void)
 {
 	if(DBG_USART_CH->SR_f.ORE) {
@@ -1494,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中断回调
Author	SHA1	Message	Date
YuanHongbin	f7f83ebf38	新增MMC5983磁传感器驱动模块及Keil工程配置更新	2026-06-22 10:15:34 +08:00
YuanHongbin	18c48e59a1	2026年3月24日，兴先达设备调试版本，校准算法初版	2026-03-30 14:46:47 +08:00
YuanHongbin	2657c1ca27	通过高度校准应力初版	2026-03-26 10:11:46 +08:00
YuanHongbin	0e05817f63	复样后的双柱支架调试 1、修复了晃动后支架乱跳的bug 2、修复了从底部到顶部后控制力变差的bug 3、控制方式改为以上面两个传感器为主 4、控制完成后设备还未校零，马上进行侧面推拉会产生误控	2026-03-13 20:00:20 +08:00
YuanHongbin	4b1eb44562	传感器尺寸改小后的调试代码，采用四个传感器	2026-02-27 15:11:26 +08:00
YuanHongbin	89939cf1e1	1、修复了上班去传感器灵敏度低的问题	2025-11-03 16:44:59 +08:00
YuanHongbin	7dfff142c2	算法优化	2025-10-30 16:28:23 +08:00
YuanHongbin	cde156a030	双柱改良版调试完成	2025-10-30 15:52:37 +08:00
YuanHongbin	445e67f234	1、修复了小概率采集到错误ad值的bug 2、优化了中间抬升下降的bug	2025-10-27 17:50:31 +08:00
cui635	291a41e850	Merge branch 'Motor_Test' into 双柱双电机S型传感器独立分支 # Conflicts: # Project/HC32F460/MDK/HC32F460.uvoptx	2025-10-25 19:25:03 +08:00
cui635	8f2b61e436	1、修改定时器4初始化代码，修复上升下降控制速度异常BUG。	2025-10-25 19:22:55 +08:00