YuanHongbin/DoubleColumn_Trolley
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

优化上电复位到底检测与碰撞反弹判定：新增每秒高度采样日志、反弹300ms持续确认滤除急停抖动毛刺、上电先延时2秒等控制盒初始化、延时期间排除堵转判定并增加已位移前置条件；报错帧按错误码沿触发处理防停顿无限续期

Browse Source
This commit is contained in:
DESKTOP-UB3FFU5\袁洪彬
2026-09-11 10:17:22 +08:00
parent 24f8231308
commit 4efe6d2a50
6 changed files with 418 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Project/HC32F460/MDK/HC32F460.uvoptx
+15
View File
@@ -290,6 +290,21 @@
<WinNumber>1</WinNumber>
<ItemText>App.MotorPara.Current_height,0x0A</ItemText>
</Ww>
<Ww>
<count>27</count>
<WinNumber>1</WinNumber>
<ItemText>App.RStatus</ItemText>
</Ww>
<Ww>
<count>28</count>
<WinNumber>1</WinNumber>
<ItemText>App.MotorPara.Current_height,0x0A</ItemText>
</Ww>
<Ww>
<count>29</count>
<WinNumber>1</WinNumber>
<ItemText>App.PwRstLastHeight,0x0A</ItemText>
</Ww>
</WatchWindow1>
<WatchWindow2>
<Ww>
Project/HC32F460/source/Module/dc_motor/dc_motor.c
+1 -1
View File
@@ -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;
}
}
Project/HC32F460/source/User/Inc/Debug.h
+2
View File
@@ -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
Project/HC32F460/source/User/Inc/main.h
+21
View File
@@ -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;//下降标志位
Project/HC32F460/source/User/Src/Debug.c
+61 -10
View File
@@ -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;
}
Project/HC32F460/source/User/Src/main.c
+318 -46
View File
@@ -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;