From 4efe6d2a50dc1dabdd9fde46924c6ff42f8cd0a9 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?DESKTOP-UB3FFU5=5C=E8=A2=81=E6=B4=AA=E5=BD=AC?=
<975559679@qq.com>
Date: Fri, 11 Sep 2026 10:17:22 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E4=B8=8A=E7=94=B5=E5=A4=8D?=
=?UTF-8?q?=E4=BD=8D=E5=88=B0=E5=BA=95=E6=A3=80=E6=B5=8B=E4=B8=8E=E7=A2=B0?=
=?UTF-8?q?=E6=92=9E=E5=8F=8D=E5=BC=B9=E5=88=A4=E5=AE=9A=EF=BC=9A=E6=96=B0?=
=?UTF-8?q?=E5=A2=9E=E6=AF=8F=E7=A7=92=E9=AB=98=E5=BA=A6=E9=87=87=E6=A0=B7?=
=?UTF-8?q?=E6=97=A5=E5=BF=97=E3=80=81=E5=8F=8D=E5=BC=B9300ms=E6=8C=81?=
=?UTF-8?q?=E7=BB=AD=E7=A1=AE=E8=AE=A4=E6=BB=A4=E9=99=A4=E6=80=A5=E5=81=9C?=
=?UTF-8?q?=E6=8A=96=E5=8A=A8=E6=AF=9B=E5=88=BA=E3=80=81=E4=B8=8A=E7=94=B5?=
=?UTF-8?q?=E5=85=88=E5=BB=B6=E6=97=B62=E7=A7=92=E7=AD=89=E6=8E=A7?=
=?UTF-8?q?=E5=88=B6=E7=9B=92=E5=88=9D=E5=A7=8B=E5=8C=96=E3=80=81=E5=BB=B6?=
=?UTF-8?q?=E6=97=B6=E6=9C=9F=E9=97=B4=E6=8E=92=E9=99=A4=E5=A0=B5=E8=BD=AC?=
=?UTF-8?q?=E5=88=A4=E5=AE=9A=E5=B9=B6=E5=A2=9E=E5=8A=A0=E5=B7=B2=E4=BD=8D?=
=?UTF-8?q?=E7=A7=BB=E5=89=8D=E7=BD=AE=E6=9D=A1=E4=BB=B6=EF=BC=9B=E6=8A=A5?=
=?UTF-8?q?=E9=94=99=E5=B8=A7=E6=8C=89=E9=94=99=E8=AF=AF=E7=A0=81=E6=B2=BF?=
=?UTF-8?q?=E8=A7=A6=E5=8F=91=E5=A4=84=E7=90=86=E9=98=B2=E5=81=9C=E9=A1=BF?=
=?UTF-8?q?=E6=97=A0=E9=99=90=E7=BB=AD=E6=9C=9F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
Project/HC32F460/MDK/HC32F460.uvoptx | 15 +
.../source/Module/dc_motor/dc_motor.c | 2 +-
Project/HC32F460/source/User/Inc/Debug.h | 2 +
Project/HC32F460/source/User/Inc/main.h | 21 +
Project/HC32F460/source/User/Src/Debug.c | 71 +++-
Project/HC32F460/source/User/Src/main.c | 364 +++++++++++++++---
6 files changed, 418 insertions(+), 57 deletions(-)
diff --git a/Project/HC32F460/MDK/HC32F460.uvoptx b/Project/HC32F460/MDK/HC32F460.uvoptx
index c2bfcaf..e6349a2 100644
--- a/Project/HC32F460/MDK/HC32F460.uvoptx
+++ b/Project/HC32F460/MDK/HC32F460.uvoptx
@@ -290,6 +290,21 @@
1
App.MotorPara.Current_height,0x0A
+
+ 27
+ 1
+ App.RStatus
+
+
+ 28
+ 1
+ App.MotorPara.Current_height,0x0A
+
+
+ 29
+ 1
+ App.PwRstLastHeight,0x0A
+
diff --git a/Project/HC32F460/source/Module/dc_motor/dc_motor.c b/Project/HC32F460/source/Module/dc_motor/dc_motor.c
index f2bb4b8..24b4fc5 100644
--- a/Project/HC32F460/source/Module/dc_motor/dc_motor.c
+++ b/Project/HC32F460/source/Module/dc_motor/dc_motor.c
@@ -343,7 +343,7 @@ void AdcLoopHander(void)
init_adc_val /= 2; /* 取平均值 */
}
}
- DCMOTOR_DBG_LOG("init_adc_val %d.\n", init_adc_val);
+ //DCMOTOR_DBG_LOG("init_adc_val %d.\n", init_adc_val);
AdcInitSta = true;
}
}
diff --git a/Project/HC32F460/source/User/Inc/Debug.h b/Project/HC32F460/source/User/Inc/Debug.h
index 69b942b..5badc16 100644
--- a/Project/HC32F460/source/User/Inc/Debug.h
+++ b/Project/HC32F460/source/User/Inc/Debug.h
@@ -33,6 +33,7 @@ typedef struct{
/* 控制盒信息帧→复位→恢复 观测打印: ISR推事件, 主循环打印完整时间线(未知帧全记录用于捕捉过热报错) */
void Uart3ErrLog_Push(uint8_t type, const uint8_t *pFrame, uint16_t height); /* type:0首次信息帧 1复位中回应 2复位完成 3未知帧 */
+void Uart3ErrLog_PushBottom(uint8_t type, uint16_t height); /* 上电复位到底判定事件(type:4堵转/5反弹, 8高度采样, 1ms例程调用) */
void Uart3ErrLogLoopHandler(void); /* 主循环调用: 打印报错/复位事件 */
void DebugUartIRQ(char Data);
@@ -46,6 +47,7 @@ void Dbg1msRoutine(void);
#define DBG_ARRAY(ARRAY,SIZE)
#define Uart3ErrLog_Push(type, pFrame, height) (0)
+#define Uart3ErrLog_PushBottom(type, height) (0)
#define Uart3ErrLogLoopHandler() (0)
#endif
diff --git a/Project/HC32F460/source/User/Inc/main.h b/Project/HC32F460/source/User/Inc/main.h
index 0dcefc4..2cbc606 100644
--- a/Project/HC32F460/source/User/Inc/main.h
+++ b/Project/HC32F460/source/User/Inc/main.h
@@ -115,6 +115,7 @@ typedef enum {
APP_STATUS_WAIT_STEADY,
APP_STATUS_END_BEEP,
APP_STATUS_HEIGHT_CONTROL,
+ APP_STATUS_POWER_RESET,//上电复位流程: 下降触发控制盒复位, 复位完成后编码器基准一致再进入校准
}AppStatus_m;
typedef struct{
@@ -152,9 +153,29 @@ typedef struct {
uint32_t CailUp1mSCnt;
uint32_t HeightContRolCnt;
uint32_t RestStop1mSCnt;//复位停顿计时: 收到复位帧后先停止电机, 计时结束再下降触发复位
+ uint32_t PwRstStop1mSCnt;//上电复位到底停顿计时: 下降到底后先停止电机, 计时结束再继续下降触发复位
+ uint16_t PwRstNoChg1mSCnt;//上电复位下降中高度无变化计时: 持续1秒无变化判定到底
+ int16_t PwRstLastHeight;//上电复位下降中上一次高度值, 用于比较高度是否变化
+ int16_t PwRstRiseRef;//上电复位反弹判定参考高度: 只跟随下降/持平更新(只跟跌不跟涨), 高度较其反升>1cm判定碰撞
+ int16_t PwRstMinHeight;//上电复位下降过程历史最低高度: 确认到底仅限接近最低点, 停在高处=被障碍物/碰撞保护托住
+ bool PwRstHtSeen;//上电复位已收到首帧有效高度标志: 高度基准就绪后才启动到底检测(防止首帧误判反弹)
+ uint8_t PwRstReboundCnt;//上电复位连续反弹计数: 首次反弹停1秒恢复下降观察, 再次反弹才转点动(到底确认时清零)
+ uint16_t PwRstInch1mSCnt;//上电复位点动下降节奏计时: 反弹后改为脉冲式下降减小触底冲击
+ uint16_t PwRstInchLastHeight;//上电复位点动周期起点高度: 周期结束时比较判断该周期是否移动
+ uint8_t PwRstInchNoMvCnt;//上电复位点动无位移周期计数: 连续2个点动周期无位移判定到底
+ bool PwRstErrValid;//上电复位报错帧已处理标志: 同一报错码连发只停顿一次, 防止连发广播把停顿无限续期
+ uint16_t PwRstErrCode;//上电复位已处理的报错码(帧[2]<<8|帧[3]): 不同报错码视为新报错重新停顿
+ uint16_t PwRstSamp1mSCnt;//上电复位高度采样计时: 每秒打印一次当前高度, 展示下降全程高度曲线
+ uint16_t PwRstRiseCandCnt;//上电复位反弹候选持续计时: 反升>1cm持续300ms才确认反弹, 滤除瞬时抖动毛刺
+ int16_t PwRstRiseCandMax;//上电复位反弹候选期间最高高度: 确认时高度须仍接近最高点(未回落)
+ uint16_t PwRstStartDlyCnt;//上电复位开始前延时: 等电机控制盒自身初始化完成(整体断电再上电时控制盒未就绪, 立即下降会因状态切换抖动误触发碰撞保护)
+ bool PwRstMovedFlag;//上电复位已发生位移标志: 高度变化过(电机已响应下降指令)后才允许堵转到底判定, 防止延时/控制盒软启动期间高度停滞误判到底
uint8_t FeedDogDlyCnt;
bool CaliZeroFlag;//校零标志位
+ bool PowerResetFlag;//上电复位流程标志: 上电复位中收到过复位帧, 收到高度帧(复位完成)后进入校准
+ bool PwRstBottomFlag;//上电复位最终到底标志: 点动下降中高度1秒无变化才确认, 之后点动下降触发复位帧
+ bool PwRstInchMode;//上电复位点动模式: 发生反弹后生效, 接近反弹高度时改脉冲式下降减小触底冲击
bool SideFlag;//侧压标志位
bool UpFlag;//上升标志位
bool DownFlag;//下降标志位
diff --git a/Project/HC32F460/source/User/Src/Debug.c b/Project/HC32F460/source/User/Src/Debug.c
index dd9c94d..b02a9be 100644
--- a/Project/HC32F460/source/User/Src/Debug.c
+++ b/Project/HC32F460/source/User/Src/Debug.c
@@ -408,20 +408,24 @@ const DBGFunType DebugFun[DEBUG_CMD_CNT] = {
* 协议: 01 04 d2 d3 = 报错帧(d2/d3为错误码, 如过热); 01 01 h_hi h_lo = 高度帧(清除复位标志)
*=========================================================================================*/
#define U3ERR_EVT_MAX 16 /* 事件缓冲条数 */
-#define U3ERR_STATUS_NUM 12 /* 主状态机状态个数 */
+#define U3ERR_STATUS_NUM 13 /* 主状态机状态个数 */
/* 事件类型 */
#define U3ERR_T_FIRST 0 /* 首次信息帧(复位标志 0→1) */
#define U3ERR_T_REPLY 1 /* 复位中回应信息帧(复位标志已置位) */
#define U3ERR_T_DONE 2 /* 复位完成(收到高度帧, 复位标志 1→0) */
#define U3ERR_T_UNKNOWN 3 /* 未知帧(非高度/信息帧, 用于捕捉过热等其它报错) */
+#define U3ERR_T_BOTTOM 4 /* 上电复位到底判定-堵转(下降中高度1秒无变化) */
+#define U3ERR_T_REBOUND 5 /* 上电复位到底判定-反弹(下降中高度不降反升, 触底震荡/碰撞反向) */
+#define U3ERR_T_TOUCH 7 /* 上电复位首次反弹-到底接触小反弹(停1秒恢复下降观察, 不转点动) */
+#define U3ERR_T_HTSAMP 8 /* 上电复位高度采样(每秒一条, 展示下降全程高度曲线) */
typedef struct{
uint8_t Type; /* 事件类型 */
uint8_t Bits; /* BIT0/BIT1 输出电平(bit0/bit1) */
uint8_t Status; /* 事件时刻主状态机状态 */
uint8_t Frame[4]; /* 原始4字节帧 */
- uint16_t Height; /* type2: 高度帧数值(mm) */
+ uint16_t Height; /* type2: 高度帧数值(mm); type4/5: 判定时刻高度 */
uint16_t Repeat; /* 连发合并的重复包数(不含首包) */
uint32_t Tick; /* 事件时刻(ms, 上电起) */
}U3ErrEvt_t;
@@ -431,12 +435,13 @@ static uint8_t m_U3ErrLastType = 0xFF; /* 上一帧类型(0xFF=无) */
static uint8_t m_U3ErrLastFrame[4]; /* 上一帧内容 */
static uint16_t m_U3ErrLastHeight = 0; /* 上一帧高度(type2) */
static uint32_t m_U3ErrLastTick = 0; /* 上一帧时刻 */
+static uint32_t m_U3ErrFirstTick = 0; /* 当前滞留帧的起始时刻(连发持续超5秒也强制落一条) */
static uint16_t m_U3ErrRepeat = 0; /* 上一帧已合并的重复包数 */
static const char *m_U3ErrStatusName[U3ERR_STATUS_NUM] = {
"IDLE", "ON", "OFF", "START_BEEP", "MOTOR_INIT", "ZERO_MARKING",
"WAIT_MOTOR_INIT", "WAIT_MOTOR_CALI_UP", "WAIT_MOTOR_CALI_DOWN",
- "WAIT_STEADY", "END_BEEP", "HEIGHT_CONTROL"
+ "WAIT_STEADY", "END_BEEP", "HEIGHT_CONTROL", "POWER_RESET"
};
static U3ErrEvt_t m_U3ErrEvt[U3ERR_EVT_MAX]; /* 事件环形缓冲 */
@@ -504,6 +509,26 @@ void Uart3ErrLog_Push(uint8_t type, const uint8_t *pFrame, uint16_t height)
m_U3ErrLastFrame[3] = pFrame[3];
m_U3ErrLastHeight = height;
m_U3ErrLastTick = tick;
+ m_U3ErrFirstTick = tick;
+}
+
+/* 函数名称: Uart3ErrLog_PushBottom
+* 功能描述: 记录上电复位"下降到底判定"事件(type:4堵转/5反弹, SysTick 1ms例程调用,
+* 立即落队不参与连发合并), 先落队滞留帧保证事件时间线顺序(临界区保护) */
+void Uart3ErrLog_PushBottom(uint8_t type, uint16_t height)
+{
+ uint8_t zero[4] = {0};
+ uint32_t primask = __get_PRIMASK();
+ __disable_irq();
+ if(m_U3ErrLastType != 0xFF){
+ Uart3ErrLog_Enqueue(m_U3ErrLastType, m_U3ErrLastFrame, m_U3ErrLastHeight, m_U3ErrLastTick, m_U3ErrRepeat);
+ m_U3ErrLastType = 0xFF;
+ m_U3ErrRepeat = 0;
+ }
+ Uart3ErrLog_Enqueue(type, zero, height, SysTick_GetTick(), 0);
+ if(primask == 0U){
+ __enable_irq();
+ }
}
/* 函数名称: Uart3ErrLog_BitsName
@@ -521,6 +546,9 @@ static const char *Uart3ErrLog_BitsName(uint8_t bits)
static const char *Uart3ErrLog_CodeName(const uint8_t *pFrame)
{
if(pFrame[0] == 0x01 && pFrame[1] == 0x02 && pFrame[2] == 0x00 && pFrame[3] == 0x10) return "(OVERHEAT)";
+ if(pFrame[0] == 0x01 && pFrame[1] == 0x02 && pFrame[2] == 0x40 && pFrame[3] == 0x00) return "(E07)";
+ if(pFrame[0] == 0x01 && pFrame[1] == 0x02 && pFrame[2] == 0x00 && pFrame[3] == 0x08) return "(E12)";
+ if(pFrame[0] == 0x01 && pFrame[1] == 0x02 && pFrame[2] == 0x02 && pFrame[3] == 0x00) return "(E02)";
if(pFrame[2] == 0x01 && pFrame[3] == 0xAA) return "(RESET)";
return "";
}
@@ -540,13 +568,16 @@ static const char *Uart3ErrLog_StatusName(uint8_t status)
void Uart3ErrLogLoopHandler(void)
{
uint8_t cnt;
- /* 连发已结束(超时500ms无同帧)且有滞留帧 → 先落队一条 */
- if((m_U3ErrLastType != 0xFF) && (m_U3ErrRepeat > 0U) &&
- ((SysTick_GetTick() - m_U3ErrLastTick) >= 500U))
- {
- Uart3ErrLog_Enqueue(m_U3ErrLastType, m_U3ErrLastFrame, m_U3ErrLastHeight, m_U3ErrLastTick, m_U3ErrRepeat);
- m_U3ErrLastType = 0xFF;
- m_U3ErrRepeat = 0;
+ /* 滞留帧落队时机: ①连发结束(500ms无同帧, 含单帧情况) ②连发持续超5秒强制落一条
+ (控制盒报错状态会持续广播报错帧, 若只等连发结束则期间日志完全静默, 看起来像没收到) */
+ if(m_U3ErrLastType != 0xFF){
+ uint32_t now = SysTick_GetTick();
+ if(((now - m_U3ErrLastTick) >= 500U) || ((now - m_U3ErrFirstTick) >= 5000U))
+ {
+ Uart3ErrLog_Enqueue(m_U3ErrLastType, m_U3ErrLastFrame, m_U3ErrLastHeight, m_U3ErrLastTick, m_U3ErrRepeat);
+ m_U3ErrLastType = 0xFF;
+ m_U3ErrRepeat = 0;
+ }
}
for(cnt = 0; cnt < 8U; cnt++){
char rep[20];
@@ -590,6 +621,26 @@ void Uart3ErrLogLoopHandler(void)
evt.Frame[0], evt.Frame[1], evt.Frame[2], evt.Frame[3],
Uart3ErrLog_CodeName(evt.Frame), rep);
break;
+ case U3ERR_T_BOTTOM: /* 上电复位: 高度2秒无变化判定到底(堵转), 停顿后持续下降触发复位 */
+ DBG_LOG("[T+%3u.%03us][%s|OUT:%s] BOTTOM-STALL: Height=%dmm no-change 2s\r\n",
+ s, ms, Uart3ErrLog_StatusName(evt.Status),
+ Uart3ErrLog_BitsName(evt.Bits), evt.Height);
+ break;
+ case U3ERR_T_REBOUND: /* 上电复位: 再次反弹判定碰撞震荡(触底震荡/碰撞反向), 转点动下降 */
+ DBG_LOG("[T+%3u.%03us][%s|OUT:%s] BOTTOM-REBOUND: Height=%dmm collision, switch to inching\r\n",
+ s, ms, Uart3ErrLog_StatusName(evt.Status),
+ Uart3ErrLog_BitsName(evt.Bits), evt.Height);
+ break;
+ case U3ERR_T_TOUCH: /* 上电复位: 首次反弹(到底接触小反弹), 停1秒恢复下降观察不转点动 */
+ DBG_LOG("[T+%3u.%03us][%s|OUT:%s] BOTTOM-TOUCH: Height=%dmm rising, pause 1s then keep descending\r\n",
+ s, ms, Uart3ErrLog_StatusName(evt.Status),
+ Uart3ErrLog_BitsName(evt.Bits), evt.Height);
+ break;
+ case U3ERR_T_HTSAMP: /* 上电复位: 高度采样(每秒), 展示下降全程高度曲线 */
+ DBG_LOG("[T+%3u.%03us][%s|OUT:%s] HEIGHT-SAMP: %dmm\r\n",
+ s, ms, Uart3ErrLog_StatusName(evt.Status),
+ Uart3ErrLog_BitsName(evt.Bits), evt.Height);
+ break;
default:
break;
}
diff --git a/Project/HC32F460/source/User/Src/main.c b/Project/HC32F460/source/User/Src/main.c
index 0774d36..24e253a 100644
--- a/Project/HC32F460/source/User/Src/main.c
+++ b/Project/HC32F460/source/User/Src/main.c
@@ -973,34 +973,59 @@ static void AppLoopHandler(void)
LED_RED_ON();
LED_GREEN_OFF();
Beeper_Start(200,200,2);
- App.TD1mSDelayCnt = 1000;
- App.Status = APP_STATUS_MOTOR_INIT;
+ App.TD1mSDelayCnt = 60000;//上电复位触发超时: 1分钟无复位帧返回则转异常状态
+ App.PwRstNoChg1mSCnt = 0;//清到底检测计数
+ App.PwRstLastHeight = App.MotorPara.Current_height;//记录初始高度用于到底检测
+ App.PwRstRiseRef = App.MotorPara.Current_height;//反弹判定参考高度初始化(只跟跌不跟涨)
+ App.PwRstHtSeen = false;//高度基准未就绪, 收到首帧有效高度后再初始化参考值并启动到底检测
+ App.PwRstReboundCnt = 0;//清连续反弹计数
+ App.PwRstMinHeight = 1430;//历史最低高度初始化为量程上限, 收到首帧有效高度时精确初始化
+ App.PwRstBottomFlag = false;
+ App.PwRstInchMode = false;
+ App.PwRstInch1mSCnt = 0;
+ App.PwRstInchNoMvCnt = 0;
+ App.PwRstErrValid = false;//清报错码处理标志, 上电后首个报错码触发一次停顿
+ App.PwRstErrCode = 0;
+ App.PwRstSamp1mSCnt = 0;//清高度采样计时
+ App.PwRstRiseCandCnt = 0;//清反弹候选计时
+ App.PwRstRiseCandMax = 0;//清反弹候选最高点
+ App.PwRstStartDlyCnt = 2000;//上电先延时2秒: 整体断电再上电时控制盒自身初始化未完成, 立即下降会因状态切换抖动误触发碰撞保护
+ App.PwRstMovedFlag = false;//未发生位移: 延时/控制盒软启动期间高度停滞不判堵转到底
+ App.Status = APP_STATUS_POWER_RESET;//上电先走复位流程, 保证换控制盒后高度基准一致, 再进入校准
break;}
-
- case APP_STATUS_MOTOR_INIT:{
- if(App.TD1mSDelayCnt <= 0)
- {
- BDC_UP();
- Ddl_Delay1ms(1000);
- BDC_DOWN();
- App.DownFlag = true;
- App.StopFlag = false;
- App.Status = APP_STATUS_WAIT_MOTOR_INIT;
- App.TD1mSDelayCnt = TRAVEL_TIME_MS;
- }
- break;}
-
- case APP_STATUS_WAIT_MOTOR_INIT:{
- if(App.TD1mSDelayCnt <= 0)
- {
+
+ case APP_STATUS_POWER_RESET:{
+ /* 上电复位流程: 不判断高度一直下降(换控制盒后编码器可能错位, 高度信息不可信)。
+ 到底检测(1ms例程): 反弹(高度较参考点反升>1cm, 控制盒碰撞保护主动反向)首次只
+ 停1秒恢复下降观察(常规到底接触的小反弹会自行稳定), 再次反弹才判定碰撞转点动
+ 下降(模拟手按, 每1000ms降200ms停800ms)一点点挪, 连续2个点动周期无位移才最终
+ 确认到底且需接近历史最低点(停在高处=被障碍物/碰撞保护托住, 转点动试探; 全程
+ 未反弹则高度2秒无变化确认)→停1秒→持续下降触发控制盒发复位
+ 帧(01 04)→停2s→再下降→控制盒完成复位发高度帧(编码器基准重置, 高度一致)→直接
+ 记录零点开始校准(去掉起步动作)。到底确认时清反弹计数, 确认后的下压反弹只停
+ 1秒继续下压不转点动。
+ 1分钟无复位帧返回则转异常状态(下降/点动有位移时自动刷新超时, 恢复流程不被打断) */
+ if(App.PwRstStartDlyCnt > 0)
+ { //上电延时2秒: 等电机控制盒自身初始化完成再开始下降(延时期间控制盒发出的
+ //首帧高度帧同步把高度基准就绪), 避免控制盒未就绪时收到下降指令产生状态切换抖动
BDC_STOP();
- App.StopFlag = true;
- App.DownFlag = false;
- App.UpFlag = false;
- App.Status = APP_STATUS_IDLE;
+ break;}
+ if(App.MotorPara.RestFlag == true)
+ { //收到复位帧: 复位流程进行中
+ App.PowerResetFlag = true;
+ if(App.RestStop1mSCnt > 0)
+ {
+ BDC_STOP();//复位停顿: 先停止电机, 让控制盒进入复位流程
+ }
+ else
+ {
+ BDC_DOWN();//停顿结束: 下降完成复位, 收到高度帧后RestFlag清除
+ }
+ App.TD1mSDelayCnt = 60000;//复位进行中, 刷新1分钟超时
}
- if(App.MotorPara.Current_height <= 1002 && App.MotorPara.Current_height >= 1000)
- {
+ else if(App.PowerResetFlag == true)
+ { //复位完成(收到高度帧, 复位标志清除): 车已在底部, 直接记录零点开始校准
+ App.PowerResetFlag = false;
BDC_STOP();
App.StopFlag = true;
App.DownFlag = false;
@@ -1009,20 +1034,57 @@ static void AppLoopHandler(void)
App.TD1mSDelayCnt = 1500;
App.Status = APP_STATUS_ZERO_MARKING;
}
+ else if(App.TD1mSDelayCnt <= 0)
+ { //1分钟无复位帧返回: 转异常状态(停机待机)
+ BDC_STOP();
+ App.StopFlag = true;
+ App.DownFlag = false;
+ App.UpFlag = false;
+ App.Status = APP_STATUS_IDLE;
+ }
+ else if(App.PwRstStop1mSCnt > 0)
+ { //停顿: 反弹后或最终确认到底后先停止电机片刻(让控制盒碰撞保护状态稳定)
+ BDC_STOP();
+ }
+ else if(App.PwRstBottomFlag == true)
+ { //最终确认到底: 和正常到底复位流程一样, 持续下降触发控制盒复位帧
+ //若再次触发碰撞保护(高度反升), 1ms例程会重新停1秒转点动
+ BDC_DOWN();
+ }
+ else if(App.PwRstInchMode == true)
+ { //点动下降(发生反弹后, 确认到底前): 模拟手按节奏(每1000ms降200ms停800ms),
+ //一点点往下挪, 冲击小不触发碰撞保护
+ if(App.PwRstInch1mSCnt < 200)
+ {
+ BDC_DOWN();
+ }
+ else
+ {
+ BDC_STOP();
+ if(App.PwRstInch1mSCnt >= 1000)
+ {
+ App.PwRstInch1mSCnt = 0;
+ }
+ }
+ }
+ else
+ { //下降中(未触发过反弹): 到底检测在App1mSRoutine(真实1ms)
+ BDC_DOWN();//一直下降, 直到确认到底
+ }
break;}
- case APP_STATUS_ZERO_MARKING:{
+ 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];
App.ADTrack.RightLow_Org_AdZero = App.RightLowCS1237_TrendArray[0];
- DBG_LOG("Frist Pack Data LeftUp Zero AD: %08d\tLeftUp Zero AD: %08d\tRightUp Zero AD: %08d\tRightLow Zero AD: %08d\r\n",
- App.ADTrack.LeftUp_Org_AdZero,
- App.ADTrack.LeftLow_Org_AdZero,
- App.ADTrack.RightUp_Org_AdZero,
- App.ADTrack.RightLow_Org_AdZero);
+// DBG_LOG("Frist Pack Data LeftUp Zero AD: %08d\tLeftUp Zero AD: %08d\tRightUp Zero AD: %08d\tRightLow Zero AD: %08d\r\n",
+// App.ADTrack.LeftUp_Org_AdZero,
+// App.ADTrack.LeftLow_Org_AdZero,
+// 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;
@@ -1058,13 +1120,13 @@ static void AppLoopHandler(void)
App.MotorPara.LoopOne = false;
App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true;
- DBG_LOG("===================LU Data monitoring====================\r\n");
+// DBG_LOG("===================LU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,431,7,341,430,4,0);
- DBG_LOG("===================LL Data monitoring====================\r\n");
+ // DBG_LOG("===================LL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,431,7,341,430,4,0);
- DBG_LOG("===================RU Data monitoring====================\r\n");
+ // DBG_LOG("===================RU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,431,7,341,430,4,0);
- DBG_LOG("===================RL Data monitoring====================\r\n");
+ // DBG_LOG("===================RL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,431,7,341,430,4,0);
DBG_LOG("Motor Cali Ok\r\n");
App.TD1mSDelayCnt = 1000;
@@ -1096,13 +1158,13 @@ static void AppLoopHandler(void)
App.MotorPara.LoopOne = false;
App.MotorPara.InitialFlag = false;
App.MotorPara.CaliFlag = true;
- DBG_LOG("===================LU Data monitoring====================\r\n");
+ //DBG_LOG("===================LU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LU_HeightStress,431,7,341,430,4,0);
- DBG_LOG("===================LL Data monitoring====================\r\n");
+ // DBG_LOG("===================LL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.LL_HeightStress,431,7,341,430,4,0);
- DBG_LOG("===================RU Data monitoring====================\r\n");
+ // DBG_LOG("===================RU Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RU_HeightStress,431,7,341,430,4,0);
- DBG_LOG("===================RL Data monitoring====================\r\n");
+ // DBG_LOG("===================RL Data monitoring====================\r\n");
weightedMovingAverageWithEnhance(App.MotorPara.RL_HeightStress,431,7,341,430,4,0);
DBG_LOG("Motor Cali Ok\r\n");
App.TD1mSDelayCnt = 1000;
@@ -1188,6 +1250,18 @@ static void App1mSRoutine(void)
App.Uart3RxTimeOut1mSCnt--;
if(App.RestStop1mSCnt > 0)
App.RestStop1mSCnt--;
+ if(App.PwRstStartDlyCnt > 0)
+ App.PwRstStartDlyCnt--;//上电复位开始前延时(等控制盒初始化)
+ if(App.PwRstStop1mSCnt > 0)
+ {
+ App.PwRstStop1mSCnt--;
+ if(App.PwRstStop1mSCnt == 0)
+ { //停顿结束: 反弹参考点重置为当前高度, 避免停顿中控制盒抬升高度导致恢复下降后立即误判反弹
+ App.PwRstRiseRef = App.MotorPara.Current_height;
+ App.PwRstRiseCandCnt = 0;//清反弹候选(停顿期间残留计时, 防止恢复后快速误确认)
+ App.PwRstRiseCandMax = App.MotorPara.Current_height;
+ }
+ }
if(App.StopCollDelay1mSCnt > 0)
App.StopCollDelay1mSCnt--;
if(App.RunCollDelay1mSCnt > 0)
@@ -1208,6 +1282,177 @@ static void App1mSRoutine(void)
{
App.HeightContRolCnt = 0;
}
+ if(App.Status == APP_STATUS_POWER_RESET && App.PwRstHtSeen == true)
+ { //上电复位高度采样: 每秒记录一次当前高度(覆盖停顿/点动/复位全程), 打印完整下降曲线,
+ //直观展示控制盒上报高度的变化趋势, 用于排查反弹是物理现象还是逻辑误判
+ if(++App.PwRstSamp1mSCnt >= 1000)
+ {
+ App.PwRstSamp1mSCnt = 0;
+ Uart3ErrLog_PushBottom(8, (uint16_t)App.MotorPara.Current_height);
+ }
+ }
+ if(App.Status == APP_STATUS_POWER_RESET
+ && App.MotorPara.RestFlag == false //复位进行中不检测
+ && App.PowerResetFlag == false //复位完成不检测
+ && App.PwRstStop1mSCnt == 0 //停顿中不检测
+ && App.PwRstStartDlyCnt == 0 //上电延时中不检测(车静止高度不变, 会误判堵转到底)
+ && App.PwRstHtSeen == true) //未收到首帧有效高度不检测(高度基准未就绪)
+ { //上电复位下降中: 到底检测(真实1ms计时, 主循环频率不定不能在此计数)
+ //控制盒换新后缓存高度偏大(1000+), 到995本该减速仍全速下降, 硬触底产生震荡,
+ //控制盒碰撞保护主动控制电机反向。策略: 首次反弹(高度较参考点反升>1cm)只停1秒
+ //恢复下降观察(常规到底接触的小反弹会自行稳定, 接着2秒堵转确认走正常流程),
+ //再次反弹才判定硬触底震荡碰撞转点动下降(模拟手按, 约1秒一次短脉冲), 连续2个
+ //点动周期无位移判定到底, 之后和正常到底复位流程一样: 停1秒→下降触发复位帧
+ //→停2s→下降→收到高度帧复位完成。到底确认时清反弹计数, 确认后下压的反弹同样
+ //只停1秒继续下压, 不会转点动
+ //未反弹时堵转判定为高度2秒无变化(碰撞悬停时高度会冻结1~2秒, 尽量避开误判)
+ //下降/点动有位移时刷新上电复位1分钟超时, 恢复流程不因总时长过长被停机
+ //历史最低高度参考: 确认到底必须接近最低点, 停在高处=被障碍物/碰撞保护托住, 转点动试探
+ if(App.MotorPara.Current_height < App.PwRstMinHeight)
+ { //记录本次复位下降过程的历史最低高度(到底只可能是最低点)
+ App.PwRstMinHeight = App.MotorPara.Current_height;
+ }
+ if(App.MotorPara.Current_height > App.PwRstRiseRef + 10)
+ { //高度较参考点反升>1cm: 碰撞反弹候选(300ms持续确认, 滤除瞬时抖动毛刺)
+ //启动/急停时机械抖动造成的高度毛刺只持续1~2帧, 直接判定会把正常高度误判为反弹
+ //参考点只跟跌不跟涨, 高度帧逐帧上升可跨帧累计检出, 不受帧率影响
+ App.PwRstNoChg1mSCnt = 0;
+ if(App.PwRstRiseCandCnt == 0)
+ {
+ App.PwRstRiseCandMax = App.MotorPara.Current_height;//新候选起点: 重置最高点
+ }
+ if(App.MotorPara.Current_height > App.PwRstRiseCandMax)
+ {
+ App.PwRstRiseCandMax = App.MotorPara.Current_height;//跟踪候选期间最高点
+ }
+ if(App.PwRstRiseCandCnt < 300)
+ {
+ App.PwRstRiseCandCnt++;//候选持续计时(封顶300ms)
+ }
+ if(App.PwRstRiseCandCnt >= 300
+ && App.MotorPara.Current_height >= App.PwRstRiseCandMax - 2)
+ { //反升持续300ms且高度未回落(仍接近候选期最高点): 确认真实反弹(控制盒碰撞保护反向)
+ App.PwRstRiseCandCnt = 0;
+ App.PwRstLastHeight = App.MotorPara.Current_height;
+ App.PwRstRiseRef = App.MotorPara.Current_height;//参考点重置, 停顿结束恢复下降后重新判定
+ App.PwRstStop1mSCnt = 1000;//先停1秒, 等控制盒碰撞保护状态稳定
+ if(App.PwRstInchMode == true)
+ { //点动中再反弹: 重置点动周期, 停1秒后继续点动
+ App.PwRstInchLastHeight = App.MotorPara.Current_height;//点动周期起点高度
+ App.PwRstInchNoMvCnt = 0;
+ Uart3ErrLog_PushBottom(5, (uint16_t)App.MotorPara.Current_height);//打印反弹判定事件
+ }
+ else if(App.PwRstReboundCnt == 0)
+ { //首次反弹: 常规到底接触的小反弹, 停1秒恢复下降观察, 不转点动
+ App.PwRstReboundCnt = 1;
+ Uart3ErrLog_PushBottom(7, (uint16_t)App.MotorPara.Current_height);//打印首次反弹观察事件
+ }
+ else
+ { //再次反弹: 确认硬触底震荡碰撞, 转点动模式(作废之前的到底确认重新确认)
+ App.PwRstReboundCnt = 0;
+ App.PwRstInchMode = true;//之后一律点动下降
+ App.PwRstBottomFlag = false;//之前确认的到底作废, 重新点动确认
+ App.PwRstInchLastHeight = App.MotorPara.Current_height;//点动周期起点高度
+ App.PwRstInchNoMvCnt = 0;
+ Uart3ErrLog_PushBottom(5, (uint16_t)App.MotorPara.Current_height);//打印反弹判定事件
+ }
+ }
+ }
+ else
+ {
+ if(App.PwRstRiseCandCnt > 0)
+ { //反升未持续300ms(瞬时毛刺/急停抖动回归): 取消候选, 参考点重置为当前高度
+ //(低毛刺帧已把参考点拉低, 不重置的话回归后的正常高度会反复进入候选)
+ App.PwRstRiseCandCnt = 0;
+ App.PwRstRiseCandMax = App.MotorPara.Current_height;
+ App.PwRstRiseRef = App.MotorPara.Current_height;
+ }
+ if(App.PwRstBottomFlag == true)
+ { //到底已确认: 不再做到底检测, 持续下降直到收到复位帧(否则会反复触发堵转停顿)
+ App.PwRstNoChg1mSCnt = 0;
+ App.PwRstLastHeight = App.MotorPara.Current_height;
+ }
+ else if(App.PwRstInchMode == true)
+ { //点动模式: 每个点动周期(1s)末尾比较该周期是否移动, 连续2周期无位移判定到底
+ if(App.PwRstInch1mSCnt == 999)
+ {
+ if(App.MotorPara.Current_height == App.PwRstInchLastHeight)
+ {
+ if(++App.PwRstInchNoMvCnt >= 2)
+ {
+ if(App.MotorPara.Current_height > App.PwRstMinHeight + 5)
+ { //点动无位移但停在高于历史最低点: 被障碍物/碰撞保护托住, 不是到底, 继续点动试探
+ App.PwRstInchNoMvCnt = 0;
+ }
+ else
+ { //连续2个点动周期无位移且接近历史最低点: 确认到底, 之后走正常到底复位流程
+ App.PwRstBottomFlag = true;
+ App.PwRstReboundCnt = 0;//到底已确认, 清反弹计数(后续下压反弹只停1秒继续压)
+ App.PwRstInchNoMvCnt = 0;
+ App.PwRstStop1mSCnt = 1000;//停1秒后下降触发复位
+ Uart3ErrLog_PushBottom(4, (uint16_t)App.MotorPara.Current_height);//打印到底确认事件
+ }
+ }
+ }
+ else
+ {
+ App.PwRstInchNoMvCnt = 0;
+ App.PwRstInchLastHeight = App.MotorPara.Current_height;
+ App.TD1mSDelayCnt = 60000;//点动脉冲有位移: 刷新上电复位超时
+ }
+ }
+ if(App.MotorPara.Current_height <= App.PwRstRiseRef)
+ {
+ App.PwRstRiseRef = App.MotorPara.Current_height;//点动脉冲下降时参考点跟随(只跟跌不跟涨)
+ }
+ }
+ else
+ {
+ if(App.MotorPara.Current_height <= App.PwRstRiseRef)
+ {
+ App.PwRstRiseRef = App.MotorPara.Current_height;//下降/持平: 参考点跟随(只跟跌不跟涨)
+ }
+ if(App.MotorPara.Current_height != App.PwRstLastHeight)
+ { //高度变化: 电机已响应下降指令, 标记已位移(此后高度稳定才可能是堵转到底)
+ App.PwRstMovedFlag = true;
+ App.PwRstNoChg1mSCnt = 0;
+ App.PwRstLastHeight = App.MotorPara.Current_height;
+ App.TD1mSDelayCnt = 60000;//高度在变(下降/反弹中): 刷新上电复位超时
+ }
+ else if(App.PwRstMovedFlag == true)
+ { //高度稳定且已发生过位移: 才允许堵转到底判定
+ if(++App.PwRstNoChg1mSCnt >= 2000)
+ {
+ if(App.MotorPara.Current_height > App.PwRstMinHeight + 5)
+ { //高度2秒无变化但停在高于历史最低点: 碰撞后控制盒把车托在障碍物上, 不是到底,
+ //转点动模式一点点试探下行
+ App.PwRstReboundCnt = 0;
+ App.PwRstInchMode = true;
+ App.PwRstInchLastHeight = App.MotorPara.Current_height;//点动周期起点高度
+ App.PwRstInchNoMvCnt = 0;
+ App.PwRstNoChg1mSCnt = 0;
+ App.PwRstStop1mSCnt = 1000;//先停1秒再开始点动
+ Uart3ErrLog_PushBottom(5, (uint16_t)App.MotorPara.Current_height);//打印碰撞托住转点动事件
+ }
+ else
+ { //2秒无高度变化且接近历史最低点: 确认到底(未触发过反弹时的堵转判定)
+ App.PwRstBottomFlag = true;
+ App.PwRstReboundCnt = 0;//到底已确认, 清反弹计数(后续下压反弹只停1秒继续压)
+ App.PwRstNoChg1mSCnt = 0;
+ App.PwRstStop1mSCnt = 1000;//停1秒后下降触发复位
+ Uart3ErrLog_PushBottom(4, (uint16_t)App.MotorPara.Current_height);//打印到底确认事件
+ }
+ }
+ }
+ else
+ { //高度稳定但电机从未响应(上电延时后控制盒软启动中): 不累积堵转计时
+ App.PwRstNoChg1mSCnt = 0;
+ }
+ }
+ }
+ }
+ if(App.PwRstInch1mSCnt < 1000)
+ App.PwRstInch1mSCnt++;
App.FeedDogDlyCnt++;
}
void FeedDogHandler(void)
@@ -1227,7 +1472,7 @@ int main(void)
FeedDogHandler();
#endif
DebugLoopHandler();
- //Uart3ErrLogLoopHandler(); //调试观测日志(复位流程已验证, 关闭打印, 需要时取消注释)
+ Uart3ErrLogLoopHandler(); //打印控制盒报错→复位→恢复事件时间线
#if(USE_CS1237_LEFTUP == 1)
LEFTUP_CS1237DataLoopCollect();
#endif
@@ -1330,20 +1575,28 @@ void Usart3RxIrqCallback(void)
{
App.RestStop1mSCnt = 2000;//首次收到复位帧: 启动停顿计时, 先停止电机让控制盒进入复位流程, 计时结束再下降触发复位
}
- //Uart3ErrLog_Push(firstErr, App.RxUart3Data.RxBuff, 0);//调试观测日志(关闭打印, 需要时取消注释)
+ Uart3ErrLog_Push(firstErr, App.RxUart3Data.RxBuff, 0);//打印复位帧事件(0:首次触发 1:复位中回应)
}
else 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(App.MotorPara.RestFlag == true)
{
- //Uart3ErrLog_Push(2, App.RxUart3Data.RxBuff, (uint16_t)h);//调试观测日志(关闭打印, 需要时取消注释)
+ Uart3ErrLog_Push(2, App.RxUart3Data.RxBuff, (uint16_t)h);//打印复位完成事件(收到高度帧)
}
App.MotorPara.RestFlag = false;
App.MotorPara.OverheatFlag = false;//收到高度帧, 确认复位完成
if(h >= 990 && h <= 1430)
{
App.MotorPara.Current_height = h;
+ if(App.Status == APP_STATUS_POWER_RESET && App.PwRstHtSeen == false)
+ { //上电复位中首次收到有效高度帧: 高度基准就绪, 以首帧高度初始化到底检测参考值
+ //(此前Current_height为0, 若用入口时的0做参考, 首帧会被误判为碰撞反弹)
+ App.PwRstHtSeen = true;
+ App.PwRstLastHeight = h;
+ App.PwRstRiseRef = h;
+ App.PwRstMinHeight = h;
+ }
}
if(App.MotorPara.InitialFlag == true && h >= 1000 && h <= 1430)
{//缓存每个高度点对于零点应力的差值，对不同高度点的应力进行补偿
@@ -1362,13 +1615,32 @@ void Usart3RxIrqCallback(void)
}
else
{ //未知帧(帧头非01 04/01 01): 过热等其它报错可能是任意组合, 全部记录供分析
- /* 过热帧(01 02 00 10): 置OverheatFlag, HEIGHT_CONTROL跳过限位纠偏, 由SupportControlHandle判断下降触发控制盒复位 */
- if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x02
- && App.RxUart3Data.RxBuff[2] == 0x00 && App.RxUart3Data.RxBuff[3] == 0x10)
+ /* 报错帧(01 02 xx xx): 置OverheatFlag, HEIGHT_CONTROL跳过限位纠偏, 由SupportControlHandle判断下降触发控制盒复位
+ 已知报错帧: 01 02 00 10(过热) 01 02 40 00(E07) 01 02 00 08(E12) 01 02 02 00(E02) */
+ if(App.RxUart3Data.RxBuff[0] == 0x01 && App.RxUart3Data.RxBuff[1] == 0x02)
{
+ uint16_t errCode = ((uint16_t)App.RxUart3Data.RxBuff[2] << 8) | App.RxUart3Data.RxBuff[3];
App.MotorPara.OverheatFlag = true;
+ if(App.Status == APP_STATUS_POWER_RESET && App.MotorPara.RestFlag == false
+ && (App.PwRstErrValid == false || App.PwRstErrCode != errCode))
+ { //上电复位中收到报错帧: 退出点动流程, 置到底标志关掉到底检测, 先停1秒再
+ //长按下降(持续下降)等复位帧。同一报错码连发只处理一次(沿触发): 控制盒报错
+ //状态会持续广播报错帧, 若每帧都重启1秒停顿则停顿被无限续期, 长按下降永远
+ //无法执行, 复位帧永远等不到; 不同报错码视为新报错重新停顿一次
+ App.PwRstErrValid = true;
+ App.PwRstErrCode = errCode;
+ App.PwRstInchMode = false;
+ App.PwRstBottomFlag = true;
+ App.PwRstInch1mSCnt = 0;
+ App.PwRstInchNoMvCnt = 0;
+ App.PwRstNoChg1mSCnt = 0;
+ App.PwRstStop1mSCnt = 1000;//先停1秒再继续下降触发复位
+ App.PwRstLastHeight = App.MotorPara.Current_height;
+ App.PwRstRiseRef = App.MotorPara.Current_height;//参考点重置, 长按下降中再反弹仍可检出
+ App.PwRstReboundCnt = 0;//清反弹计数, 长按下降中反弹只停1秒继续压不转点动
+ }
}
- //Uart3ErrLog_Push(3, App.RxUart3Data.RxBuff, 0);//调试观测日志(关闭打印, 需要时取消注释)
+ Uart3ErrLog_Push(3, App.RxUart3Data.RxBuff, 0);//打印未知帧事件(过热帧01 02 00 10, E07帧01 02 40 00等)
}
memset(App.RxUart3Data.RxBuff, 0, sizeof(App.RxUart3Data.RxBuff));
App.RxUart3Data.RxLen = 0;