5 changed files with 206 additions and 134 deletions
@ -0,0 +1,182 @@ |
|||
/******************************************************************************
|
|||
|
|||
版权所有 (C), 2018-2099, Radkil |
|||
|
|||
****************************************************************************** |
|||
文 件 名 : msp_Timer.c |
|||
版 本 号 : 初稿 |
|||
作 者 : radkil |
|||
生成日期 : 2026年9月17日 |
|||
最近修改 : |
|||
功能描述 : 硬件定时器任务 |
|||
|
|||
修改历史 : |
|||
1.日 期 : 2026年9月17日 |
|||
作 者 : radkil |
|||
修改内容 : 创建文件 |
|||
|
|||
******************************************************************************/ |
|||
#include "BHBF.h" |
|||
#include "msg_center.h" |
|||
#include "rd_time.h" |
|||
#include "tim.h" |
|||
|
|||
/*----------------------------------------------*
|
|||
* 外部变量说明 * |
|||
*----------------------------------------------*/ |
|||
extern volatile int32_t g_RF_Angle_Roll; |
|||
/*----------------------------------------------*
|
|||
* 外部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
double Angle_Tune_PID(double CurrentAngle, double TargetAngle, double Position_KP, |
|||
double Position_KI, double Position_KD, double MaxValue); |
|||
/*----------------------------------------------*
|
|||
* 内部函数原型说明 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 全局变量 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 模块级变量 * |
|||
*----------------------------------------------*/ |
|||
static uint32_t g_uiTimModuleID = 0; |
|||
/*----------------------------------------------*
|
|||
* 常量定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
/*----------------------------------------------*
|
|||
* 宏定义 * |
|||
*----------------------------------------------*/ |
|||
|
|||
static void Timer_ModuleHandler(const Msg_t *pstMsg) |
|||
{ |
|||
if (NULL == pstMsg) |
|||
{ |
|||
return; |
|||
} |
|||
|
|||
static int aiMotorSpeed[2] = {0}; |
|||
|
|||
static int g_dletAngle = 0; //PID最终计算补偿值
|
|||
static uint32_t uiLastSendTick = 0; //上一次时间戳，用于控制直线行驶距离
|
|||
static int bTimerStarted = 0; //直线行驶距离单次触发标志
|
|||
switch (pstMsg->m_uiMsgID) |
|||
{ |
|||
case TIMER_CMD_RESET_STRAIGHT: |
|||
{ |
|||
uiLastSendTick = 0; |
|||
bTimerStarted = 0; |
|||
break; |
|||
} |
|||
case TIMER_CMD_STRAIGHT_DRIVE: |
|||
{ |
|||
BHBF_straight_drive_Cmd stCmd = {0}; |
|||
if (pstMsg->m_uiDataLen >= sizeof(stCmd)) |
|||
{ |
|||
RD_MEMCPY(&stCmd, pstMsg->m_aucData, sizeof(stCmd)); |
|||
|
|||
if (stCmd.m_iTime > 0 && 0 == bTimerStarted) |
|||
{ |
|||
uiLastSendTick = Rd_GetTime(); |
|||
bTimerStarted = 1; // 开始计时
|
|||
} |
|||
|
|||
if (0 == stCmd.m_iMode) |
|||
{ |
|||
break; |
|||
} |
|||
|
|||
if (abs(g_RF_Angle_Roll - stCmd.m_iAngle) <= g_stCV.PID_mid.PID_Angle) |
|||
{ |
|||
if (abs(g_RF_Angle_Roll - stCmd.m_iAngle) < g_stCV.PID_low.PID_Angle) |
|||
{ |
|||
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, stCmd.m_iAngle, |
|||
g_stCV.PID_low.Kp, g_stCV.PID_low.Ki, g_stCV.PID_low.Kd, 10); |
|||
} |
|||
else |
|||
{ |
|||
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, stCmd.m_iAngle, |
|||
g_stCV.PID_mid.Kp, g_stCV.PID_mid.Ki, g_stCV.PID_mid.Kd, 10); |
|||
} |
|||
if (stCmd.m_iMode > 0) |
|||
{ |
|||
aiMotorSpeed[0] = (g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ; |
|||
aiMotorSpeed[1] = (g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ; |
|||
} |
|||
else |
|||
{ |
|||
aiMotorSpeed[0] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) - g_dletAngle ; |
|||
aiMotorSpeed[1] = -(g_stCV.Lane_Change_Speed_m_per_min * 10) + g_dletAngle ; |
|||
} |
|||
} |
|||
else |
|||
{ |
|||
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, stCmd.m_iAngle, |
|||
g_stCV.PID_high.Kp, g_stCV.PID_high.Ki, g_stCV.PID_high.Kd,50); |
|||
aiMotorSpeed[0] = -g_dletAngle ; |
|||
aiMotorSpeed[1] = g_dletAngle ; |
|||
} |
|||
|
|||
if (stCmd.m_iTime > 0 && (int32_t)(Rd_GetTime() - uiLastSendTick) >= stCmd.m_iTime)// 到时间停止
|
|||
{ |
|||
aiMotorSpeed[0] = 0; |
|||
aiMotorSpeed[1] = 0; |
|||
} |
|||
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); |
|||
|
|||
if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0) |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_STRAIGHT_DRIVE, &stCmd, sizeof(stCmd)); |
|||
} |
|||
} |
|||
break; |
|||
} |
|||
case TIMER_CMD_TURN_ANGLE: |
|||
{ |
|||
int iTargetAngle = 0; |
|||
if (pstMsg->m_uiDataLen >= sizeof(int)) |
|||
{ |
|||
RD_MEMCPY(&iTargetAngle, pstMsg->m_aucData, sizeof(int)); |
|||
if (abs(g_RF_Angle_Roll - iTargetAngle) <= 50) |
|||
{ |
|||
aiMotorSpeed[0] = 0; |
|||
aiMotorSpeed[1] = 0; |
|||
} |
|||
else if (abs(g_RF_Angle_Roll - iTargetAngle) <= 1000) |
|||
{ |
|||
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, iTargetAngle, 1, 0,0.5, 10); |
|||
aiMotorSpeed[0] = -g_dletAngle ; |
|||
aiMotorSpeed[1] = g_dletAngle; |
|||
} |
|||
else |
|||
{ |
|||
g_dletAngle = Angle_Tune_PID((double)g_RF_Angle_Roll, iTargetAngle, 2, 0, 0.5, 85); |
|||
aiMotorSpeed[0] = -g_dletAngle ; |
|||
aiMotorSpeed[1] = g_dletAngle ; |
|||
} |
|||
MsgCenter_SendTo(MODULE_NAME_MOTOR, MOTOR_SET_SPEED, aiMotorSpeed, sizeof(aiMotorSpeed)); |
|||
|
|||
if (aiMotorSpeed[0] == 0 && aiMotorSpeed[1] == 0) |
|||
{ |
|||
MsgCenter_SendTo(MODULE_NAME_CUSTOM, CUSTOM_CMD_TURN_ANGLE, &iTargetAngle, sizeof(iTargetAngle)); |
|||
} |
|||
} |
|||
break; |
|||
} |
|||
default: |
|||
break; |
|||
} |
|||
} |
|||
|
|||
void Timer_Task(void) |
|||
{ |
|||
MsgCenter_Process(g_uiTimModuleID); |
|||
} |
|||
|
|||
void Timer_Init(void) |
|||
{ |
|||
HAL_TIM_Base_Start_IT(&htim8); |
|||
g_uiTimModuleID = MsgCenter_Register(MODULE_NAME_TIMER, Timer_ModuleHandler); |
|||
} |
|||
Loading…
Reference in new issue