YuanHub 源码分析【四】位置速度

关注

原创 2026-05-07 10:36:44 452 阅读 AI 写同款· GEO 优化 ›

情系淮思：五款车规MCU横向对比与VCU选型本文对比五款车规MCU中的S32K312与S32K324，聚焦核心架构、存储、通信及安全特性。S32K324为双核160MHz Cortex-M7，内存、外设（如以太网、QuadSPI、SAI）全面升级，支持高实时并行处理与复杂应用；而S32K312为单核120MHz，适用于中端车身控制。两者均满足ASIL B功能安全，具备完整安全引擎。选型建议：中端用S32K312，高端复杂系统优选S32K324。阅读详情

一、定时器外设初始化和中断

1.1 初始化

外设由 CubeMX 初始化：

根据宏定义我们可知，位置环是在 TIM5 上初始化：

#define POSITION_LOOP_TIM_HANDLE    (htim5)                     //位置环定时器
#define POSITION_LOOP_TIM_IRQ_TASK  TIM5_IRQHandler             //位置环定时器中断回调函数

下面是 TIM5 的初始化代码：

int main(void)
{
  ...
  MX_TIM5_Init();
  ...
}

/**
  * @brief TIM5 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM5_Init(void)
{

  /* USER CODE BEGIN TIM5_Init 0 */

  /* USER CODE END TIM5_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};

  /* USER CODE BEGIN TIM5_Init 1 */

  /* USER CODE END TIM5_Init 1 */
  htim5.Instance = TIM5;
  htim5.Init.Prescaler = 0;
  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim5.Init.Period = 24000-1;
  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
  if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM5_Init 2 */

  /* USER CODE END TIM5_Init 2 */

}

分类	参数项目	配置值	功能说明
计数器	计数模式 (CounterMode)	向上计数 (Up)	计数器从 0 递增到周期值（24000-1），然后溢出并重新从 0 开始
	周期 (Period)	24000-1	决定定时器的溢出周期/基础频率，即自动重装载寄存器 (ARR) 的值
	预分频 (Prescaler)	0	定时器时钟不分频（实际分频系数为 0+1=1），保持总线输入的最高时钟频率
	时钟分频 (ClockDivision)	不分频 (DIV1)	决定数字滤波器和死区时间生成的采样时钟频率，此处与定时器时钟一致
	自动重载预装载 (AutoReloadPreload)	使能 (Enable)	开启缓冲机制，运行时修改周期值会在下一个更新事件才生效，防止波形突变（毛刺）
时钟源	时钟源选择 (ClockSource)	内部时钟 (Internal)	使用微控制器内部的总线时钟（通常是 APB1）作为定时器驱动源
触发同步	主模式触发输出 (TRGO)	复位 (Reset)	定时器的复位信号（UG位产生）用作主输出触发，可用于触发级联的从定时器或其他外设
	主从模式 (MasterSlave)	禁用 (Disable)	禁用主从同步模式，定时器作为独立模块正常运行

二、中断函数

2.1 位置环中断函数

POSITION_LOOP_TIM_IRQ_TASK()

// 位置环中断任务 典型频率  10KHZ
void POSITION_LOOP_TIM_IRQ_TASK(void)
{
    POSITION_LOOP_TIM_HANDLE.Instance->SR &= ~TIM_SR_UIF;

    bsp_set_timer_record_stop(SYS_TIMER_RECORD_POSITION_LOOP_CYCLE_INDEX); // 测量位置环周期
    bsp_set_timer_record_start(SYS_TIMER_RECORD_POSITION_LOOP_CYCLE_INDEX);

    bsp_set_timer_record_start(SYS_TIMER_RECORD_POSITION_LOOP_TIME_INDEX); // 测量位置环运行时间

#ifndef VIRTUAL_MOTOR_MODEL
    if (sys_get_hardware_self_test_status() == false)
    {
        EncoderDataInit();
        HardwareSelfTestRun();
    }

    // 编码器数据读取及处理
    EncoderDataRead();
    EncoderDataProcess();

    bsp_pwm_ready_state_updata(); // 更新PWM输出准备状态
#endif // VIRTUAL_MOTOR_MODEL
    if (sys_get_hardware_self_test_status() == true)
    {
        PosSpeedLoopCtrl();
    }
    bsp_set_timer_record_stop(SYS_TIMER_RECORD_POSITION_LOOP_TIME_INDEX);
}

2.2 计时器

其使用了 DWT 时钟周期计数器来记录时间，共有：

sys_timer_init()
set_timer_record_start()
set_timer_record_stop()
get_timer_duration_records_us()

四个函数。

下面是 CURRENT_LOOP_IRQ_TASK() 所使用的计时器代码：

SysTimer_t sys_timer = {
    .sys_freq_Mhz = 0,
    .sys_freq_Mhz_inv = 0,
    .now_records = {0},
    .duration_records = {0},
};
 
void sys_timer_init(void)
{
    if (!((DEM_CR & DEM_CR_TRCENA) && DWT_CR & DWT_CR_CYCCNTENA))
    {
        DEM_CR |= DEM_CR_TRCENA;//第一步使能DWT外设
        DWT_LAR |= DWT_LAR_UNLOCK;
        DWT_CYCCNT = 0u;//第二步在使能DWT计数前先清零
        DWT_CR |= DWT_CR_CYCCNTENA;//第三步使能DWT计数
    }
    sys_timer.sys_freq_Mhz = HAL_RCC_GetSysClockFreq() / 1000000;
    sys_timer.sys_freq_Mhz_inv = 1.0f / (float)sys_timer.sys_freq_Mhz;
}
 
void set_timer_record_start(SYS_TIMER_RECORD_INDEX index)
{
    sys_timer.now_records[index] = DWT_CYCCNT;
}
 
void set_timer_record_stop(SYS_TIMER_RECORD_INDEX index)
{
    sys_timer.duration_records[index] = DWT_CYCCNT - sys_timer.now_records[index];
}
 
float get_timer_duration_records_us(SYS_TIMER_RECORD_INDEX index)
{
    return sys_timer.sys_freq_Mhz_inv * sys_timer.duration_records[index];
}

2.2 编码器初始化

这里包括了编码器初始化的内容：

    if (sys_get_hardware_self_test_status() == false)
    {
        EncoderDataInit();
        HardwareSelfTestRun();
    }

    // 编码器数据读取及处理
    EncoderDataRead();
    EncoderDataProcess();

    bsp_pwm_ready_state_updata(); // 更新PWM输出准备状态

2.3 更新PWM输出准备状态函数

在实际的电机驱动硬件中，当我们用软件下发使能 PWM 的指令时，底层的门级驱动芯片 (Gate Driver) 内部的自举电容充电、电荷泵稳压、内部逻辑复位等硬件动作都需要一定的时间，通常是几个到几十个毫秒。如果在芯片刚上电还没稳妥的时候，单片机就立刻往里面塞高频的 PWM 波形，很容易导致硬件报错甚至炸管。

bsp_pwm_ready_state_updata() 函数的功能是在使能 PWM 的时候给驱动器的自举电容充电，其充电时间由计数器宏定义 DRIVER_PWM_READY_TIME 决定。

代码如下：

/**
 * @brief 更新PWM输出准备状态
 */
void bsp_pwm_ready_state_updata(void)
{
#ifdef VIRTUAL_MOTOR_MODEL
    kBspData.pwm_ready_state = true;
#else
    if (DRIVER_PWM_READY_TIME)
    {
        if (kBspData.pwm_en_state == PWM_ENABLE)
        {
            kBspData.pwm_state_cnt++;
            if (kBspData.pwm_state_cnt >= DRIVER_PWM_READY_TIME)
            {
                kBspData.pwm_state_cnt = DRIVER_PWM_READY_TIME;
                kBspData.pwm_ready_state = true;
            }
        }
        else
        {
            kBspData.pwm_state_cnt = 0;
            kBspData.pwm_ready_state = false;
        }
    }
    else // 等待时间为0，PWM使能后立即认为PWM准备好
    {
        kBspData.pwm_ready_state = true;
    }
#endif
}

三、速度环中断

3.1 逻辑总览

3.2 位置速度环中断函数

PosSpeedLoopCtrl() 函数是位置环和速度环的控制总调度。

除其一些逻辑处理外，主要包括了 AppRun()、 ModeRun()、CurrentCtlLoopTask() 三个函数。


/**
 * @brief 位置速度环中断循环
 */
void PosSpeedLoopCtrl(void)
{
    // 位置速度环系统输入
    // 1. 读取当前实际位置  编码器当前单圈位置
    kAxis.motor_pos_sensor_input.enc_counts_now_p = bsp_get_encoder_cnt(ENCODER_ID_MOTOR);
    kAxis.load_pos_sensor_input.enc_counts_now_p = bsp_get_encoder_cnt(ENCODER_ID_LOAD);

    if (get_app_Internal_control_authority() == INTERNAL_CONTROL_APP)
    {
        AppStatusCheck();
        AppStatusScanFast();
        AppMotionPreConditionRun();
#ifdef USE_CIA402
        Cia402ErrorCodeScan();
        PDS_FSA_Run();
#endif // USE_CIA402
        AppRun(kAxis.motor_ctl_sm_input.hw_ready_state);
#ifdef USE_CIA402
        UpdateStatusword();
#endif // USE_CIA402
    }

    // 2. 执行位置速度环控制算法
    PosSpeedCtlLoopTask(&kAxis, &kAxisDw);

    // 3. 输出电机控制电压  失能状态到使能状态 上升沿使能电机
    if (kAxis.motor_ctl_sm_output.state == MOTOR_CTL_SM_STATE_ENABLE)
    {
        if (sys_get_current_calibration_status() == CURRENT_CALIBRATION_STATUS_OK)
        {
            if (kAxis.motor_ctl_sm_output.state_last == MOTOR_CTL_SM_STATE_DISABLE)
            {
                bsp_set_pwm_state(PWM_ENABLE);
            }
        }
        else
        {
            sys_current_calibration_step();
        }
    }
    else // 失能状态    其他状态都失能电机  包括错误状态
    {
        bsp_set_pwm_state(PWM_DISABLE);
    }

    // 4. 更新硬件准备状态
    if (bsp_get_pwm_state() == true &&\
        sys_get_current_calibration_status() == CURRENT_CALIBRATION_STATUS_OK)
    {
        kAxis.motor_ctl_sm_input.hw_ready_state = 1;
    }
    else
    {
        kAxis.motor_ctl_sm_input.hw_ready_state = 0;
    }
    // 记录上一次控制层状态机状态
    kAxis.motor_ctl_sm_output.state_last = kAxis.motor_ctl_sm_output.state;


    // 同步控制层变量到应用层,实现波形动态刷新和离线波形采集
    AppParamSyncUser();

    // 运行离线波形采样
    ScopeIsrHandle();
}

3.2 AppRun() 系列逻辑

3.2.1 AppRun()

在 AppRun() 中选择合适的启动模式程序，通过函数指针方式调用，所有模式的初始化、运行、启动、停止函数等均在这个表格内：

// 应用程序列表
static const AppTable app_table[APP_NUM] =
{
    {APP_PP_MODE, PpModeInit, PpModeStart, PpModeRun, PpModeStop, APP_TYPE_PDS},
    {APP_PV_MODE, PvModeInit, PvModeStart, PvModeRun, PvModeStop, APP_TYPE_PDS},
    {APP_PT_MODE, PtModeInit, PtModeStart, PtModeRun, PtModeStop, APP_TYPE_PDS},
    {APP_MIT_MODE, MitModeInit, MitModeStart, MitModeRun, MitModeStop, APP_TYPE_PDS},
    {APP_HM_MODE, HomingModeInit, HomingModeStart, HomingModeRun, HomingModeStop, APP_TYPE_PDS},
    {APP_CSP_MODE, CspModeInit, CspModeStart, CspModeRun, CspModeStop, APP_TYPE_PDS},
    {APP_CSV_MODE, CsvModeInit, CsvModeStart, CsvModeRun, CsvModeStop, APP_TYPE_PDS},
    {APP_CST_MODE, CstModeInit, CstModeStart, CstModeRun, CstModeStop, APP_TYPE_PDS},
    {APP_DIR_ID_MODE, IdDirModeInit, IdDirModeStart, IdDirModeRun, IdDirModeStop, APP_TYPE_PDS},
    {APP_ELEC_ANGLE_ID_MODE, IdElecAngleModeInit, IdElecAngleModeStart, IdElecAngleModeRun, IdElecAngleModeStop, APP_TYPE_PDS},
    {APP_ELEC_ID_MODE, IdElecModeInit, IdElecModeStart, IdElecModeRun, IdElecModeStop, APP_TYPE_PDS},
    {APP_MEC_ID_MODE, IdMecModeInit, IdMecModeStart, IdMecModeRun, IdMecModeStop, APP_TYPE_PDS},
    {APP_POLE_PAIRS_ID_MODE, IdPolePairsModeInit, IdPolePairsModeStart, IdPolePairsModeRun, IdPolePairsModeStop, APP_TYPE_PDS},
    {APP_TQ_FC_ID_MODE, IdTqFcModeInit, IdTqFcModeStart, IdTqFcModeRun, IdTqFcModeStop, APP_TYPE_PDS},
    {APP_NULL, NULL, NULL, NULL, NULL}, // 用于表示空闲状态
};

AppRun() 从中根据配置信息查找合适的函数进行调用：

// 运行应用程序
void AppRun(uint8_t hw_ready_state)
{
    // 遍历应用程序列表
    for (int i = 0; i < APP_NUM; i++)
    {
        if (kAppInfo[i].enable)
        {

            // 启动应用程序
            if (kAppInfo[i].app_state == APP_STA_READY)
            {

                if (kAppInfo[i].app_ptr->start != NULL)
                {
                    kAppInfo[i].app_result = kAppInfo[i].app_ptr->start();
                    if (kAppInfo[i].app_result == APP_RET_RUNNING)
                    { // start处于等待状态
                        kAppInfo[i].app_state = APP_STA_READY;
                    }
                    else if (kAppInfo[i].app_result == APP_RET_SUCCESS)
                    { // start成功,进入run状态
                        kAppInfo[i].app_state = APP_STA_RUNNING;
                    }
                    else if (kAppInfo[i].app_result == APP_RET_FAIL)
                    { // start失败,进入错误状态
                        kAppInfo[i].app_state = APP_STA_ERROR;
                    }
                }
            }

            // 运行应用程序
            if (kAppInfo[i].app_state == APP_STA_RUNNING)
            {
                if (kAppInfo[i].app_ptr->run != NULL)
                {
                    // 回读PWM准备好状态，准备好则运行APP 否则等待处于 READY 状态
                    if (hw_ready_state)
                    {
                        kAppInfo[i].app_result = kAppInfo[i].app_ptr->run();
                        if (kAppInfo[i].app_result == APP_RET_RUNNING)
                        {
                            kAppInfo[i].app_state = APP_STA_RUNNING;
                        }
                        else if (kAppInfo[i].app_result == APP_RET_SUCCESS)
                        {
                            kAppInfo[i].app_state = APP_STA_SUCCESS;
                        }
                        else if (kAppInfo[i].app_result == APP_RET_FAIL)
                        {
                            kAppInfo[i].app_state = APP_STA_ERROR;
                        }
                    }
                    else
                    {
                        kAppInfo[i].app_state = APP_STA_READY;
                    }
                }
            }

            // 停止应用程序
            if ((kAppInfo[i].app_state == APP_STA_SUCCESS) || (kAppInfo[i].app_state == APP_STA_ERROR))
            {
                if (kAppInfo[i].app_ptr->stop != NULL)
                {
                    kAppInfo[i].app_ptr->stop();
                }
                kAppInfo[i].enable = false;           // 禁用应用程序
                kAppInfo[i].app_state = APP_STA_IDLE; // 设置应用程序状态为空闲
            }
        }
    }
}

3.2.2 PvModeRun()

如果我们希望配置成速度模式 (PV) 经过 AppRun() 的调用，便会来到 PvModeRun() 函数中，其功能就是根据配置的速度要求传入到 SpeedTrajectoryPlanningHandle() 速度轨迹规划函数中。

AppResult PvModeRun() {
  kPvMode.now_Controlword = (APP_CONTROL_WORD)get_app_Controlword();

  if (kPvMode.now_Controlword == APP_CTRL_ENABLE) {
    if (!get_app_Emergency_brake_requested()) {
      if (kPvMode.pre_Controlword == APP_CTRL_DISABLE) {
        // 重新使能后初始化规划器
        SpeedTrajectoryPlanningInit();
      }

      kPvMode.velocity_target = get_app_Target_velocity();

      // 规划速度为0的情况：1. 暂停指令生效 2. 抱闸状态不为松闸
      if (get_app_Halt_running_cmd() == true ||
          get_app_Brake_state() != BRAKE_STATE_RELEASED) {
        kPvMode.traj.speed_tar_p = 0;
      } else {
        kPvMode.traj.speed_tar_p = kPvMode.velocity_target *
                                   get_app_Motor_rpm_2_pps() *
                                   get_app_Reduction_ratio();
      }

      // 轮廓加速度减速度切换 目标速度绝对值 大于 当前规划速度绝对值加速
      // 否则则加速
      if (MATH_ABS(kPvMode.velocity_target) >
          MATH_ABS(get_app_Velocity_demand_value())) {
        kPvMode.traj.acc = get_app_Profile_acceleration() *
                           get_app_Motor_rpm_2_pps() *
                           get_app_Reduction_ratio();
      } else {
        kPvMode.traj.acc = get_app_Profile_deceleration() *
                           get_app_Motor_rpm_2_pps() *
                           get_app_Reduction_ratio();
      }
    } else {
      set_app_Controlword(
          APP_CTRL_EMERGENCY_BRAKE); // 强制进入紧急停车（QuickStop）状态
      kPvMode.now_Controlword = APP_CTRL_EMERGENCY_BRAKE;
    }
  }

  // 紧急停车（QuickStop）处理
  if (kPvMode.now_Controlword == APP_CTRL_EMERGENCY_BRAKE) {
    kPvMode.traj.speed_tar_p = 0; // 如果处于急停状态，规划速度为0
    kPvMode.emergency_brake_mode = get_app_Quick_stop_option_code();
    if (kPvMode.emergency_brake_mode == EMERGENCY_BRAKE_MODE_QUICK_STOP_RAMP ||
        kPvMode.emergency_brake_mode ==
            EMERGENCY_BRAKE_MODE_QUICK_STOP_RAMP_ENABLE ||
        kPvMode.emergency_brake_mode == EMERGENCY_BRAKE_MODE_CURRENT_LIMIT ||
        kPvMode.emergency_brake_mode ==
            EMERGENCY_BRAKE_MODE_CURRENT_LIMIT_ENABLE) {
      kPvMode.traj.acc = get_app_Quick_stop_deceleration() *
                         get_app_Motor_rpm_2_pps() * get_app_Reduction_ratio();
    } else if (kPvMode.emergency_brake_mode != EMERGENCY_BRAKE_MODE_DISABLED) {
      // TODO: 电压限制急停先不实现按正常减速斜率停机
      kPvMode.traj.acc = get_app_Profile_deceleration() *
                         get_app_Motor_rpm_2_pps() * get_app_Reduction_ratio();
    }
    // 在急停后失能电机模式下，检测到零速后电机失能
    if (kPvMode.emergency_brake_mode <= EMERGENCY_BRAKE_MODE_VOLTAGE_LIMIT) {
      kPvMode.check_status_val = (CheckStatusVal_t)app_get_check_status_val();
      if (kPvMode.emergency_brake_mode == EMERGENCY_BRAKE_MODE_DISABLED ||
          kPvMode.check_status_val.bits.velocity_zero == true) {
        set_app_Controlword(APP_CTRL_DISABLE);
        kPvMode.now_Controlword = APP_CTRL_DISABLE;
      }
    }
  }

  if (kPvMode.now_Controlword != APP_CTRL_DISABLE) {
    SpeedTrajectoryPlanningHandle(&kPvMode.traj);
  }

  kPvMode.pre_Controlword = kPvMode.now_Controlword;

  AppVelocityTargetReachedStateCheck();

  return APP_RET_RUNNING;
}

3.2.2 SpeedTrajectoryPlanningHandle()

在速度轨迹规划函数中直接调用 linear_trajectory_planning 模型：

// 速度轨迹规划器处理函数
void SpeedTrajectoryPlanningHandle(SPEED_TRAJECTORY_DATA *speed_traj_data)
{
    int8_T state_now = 0;
    speed_traj_data->period_s = axis->pmsm_config.tp_s;

    linear_trajectory_planning(&speed_traj_data->speed_tar_p,
                               &speed_traj_data->acc,
                               &speed_traj_data->period_s,
                               &axis->pos_speed_ctl_input.speed_tar_p_s,
                               &state_now, &(kSpeedTrajDw));
}

3.2.3 linear_trajectory_planning 模型

当系统收到一个新的目标指令 traj_cmd 时，如果直接让当前状态 traj_now 跳变过去，可能会导致硬件 (比如电机、执行机构) 受到剧烈冲击。这个函数的任务就是让当前状态以一个恒定的速度(slope，斜率) 和固定的步长 (dt_s，控制周期)，一步一步地、平滑地走向目标值。

输入参数定义：

traj_cmd：目标轨迹指令 (你要去哪里)
traj_now：当前轨迹状态 (你现在在哪里)
slope：允许的最大变化率 (你走多快)
dt_s：控制周期/步长 (每次走多久，通常是系统的采样时间，比如 0.01秒)

输出参数定义：

traj_last：输出的速度
state_now：当前是否完成规划

linear_trajectory_planning 模型外观：

linear_trajectory_planning 内的代码：

function [traj_last, state_now] = fcn(traj_cmd, traj_now, slope, dt_s)
 
% state_now  规划状态  
% 0 初始状态
% 1 规划完成
% 2 规划中

traj_last_temp = single(0);
state_now_temp = int8(0);

if traj_now > traj_cmd
    traj_last_temp = traj_now - slope * dt_s;
    state_now_temp = int8(2);
    if traj_last_temp < traj_cmd
        traj_last_temp = traj_cmd;
        state_now_temp = int8(1);
    end
else
    traj_last_temp = traj_now + slope * dt_s;
    state_now_temp = int8(2);
    if traj_last_temp > traj_cmd
        traj_last_temp = traj_cmd;
        state_now_temp = int8(1);
    end

end

traj_last = traj_last_temp;
state_now = state_now_temp;

3.3 ModeRun() 系列逻辑

3.3.1 ModeRun()

这个函数位于整个电机状态机的运行阶段。它的核心作用是查表并派单。

根据当前系统配置的工作模式 (axis->motor_ctl_sm_config.mode)，通过一个巨大的 switch-case 语句，调用对应的底层执行步 (Step) 函数。

例如：

如果当前是“位置模式”，它就调用 PositionStep()
如果当前是“速度模式”，它就调用 SpeedStep()
如果当前是“转矩摩擦辨识模式”，它就调用 TqFcIdStep()
等等

// 位置环内部模式运行
static void ModeRun(Axis *const axis, AxisDw *const axis_dw)
{
   // TODO：增加模式切换 和 当前模式显示处理
   axis->motor_ctl_sm_output.mode_now = axis->motor_ctl_sm_config.mode;
   switch (axis->motor_ctl_sm_config.mode)
   {
   case MOTOR_CTL_SM_MODE_POSITION: // 位置模式
      PositionStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_SPEED: // 速度模式
      SpeedStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_TORQUE: // 力矩模式
      TorqueStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_TQ_FRICTION_IDENTIFICATION: // 转矩脉动与摩擦辨识
      TqFcIdStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_MECHANICAL_IDENTIFICATION: // 机械特性辨识模式
      MecIdStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_ELECTRIC_ANGLE_IDENTIFICATION: // 电角度辨识模式
      ElecAngleIdStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_DIRECTION_IDENTIFICATION: // 方向辨识模式
      DirectionIdStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_POLE_PAIRS_IDENTIFICATION: // 极对数辨识模式
      PolePairsIdStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_ELECTRIC_IDENTIFICATION: // 电气特性辨识模式
      ElectricIdStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_IDLE: // 空闲模式
      axis->motor_ctl_sm_output.state = 0;
      break;
   case MOTOR_CTL_SM_MODE_VOLTAGE: // 电压直接控制模式
      VoltageStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_RECIPROCAL_MOTION: // 往复运动模式
      ReciprocalMotionStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_REFERENCE_SIGNAL: // 参考信号模式
      ReferenceSignalStep(axis, axis_dw);
      break;
   case MOTOR_CTL_SM_MODE_MIT: // MIT模式
      MitCtlStep(axis, axis_dw);
      break;

   default:
      break;
   }
}

3.3.2 SpeedStep()

当 ModeRun() 发现当前是速度模式时，就会进入这个函数。它的作用是为速度环闭环做数据准备，并调用控制器，最后附加补偿。如果是 debug 模式则会再加一个规划。

// 速度模式
static void SpeedStep(Axis *const axis, AxisDw *const axis_dw)
{
   axis->pos_speed_ctl_config.mode = POS_SPEED_CTL_MODE_SPEED; // 纯速度控制

#ifdef MOTOR_CTL_DEBUG // 控制层调试模式，加入规划器
   PosSpeedCtlInput pos_speed_ctl_input = axis->pos_speed_ctl_input;

   linear_trajectory_planning(&axis->pos_speed_ctl_input.speed_tar_p_s,
                              &axis->pos_speed_ctl_input.acc_tar_p_ss,
                              &axis->pmsm_config.tp_s,
                              &pos_speed_ctl_input.speed_tar_p_s,
                              &kSpeedTrajStateNowTest, &(kSpeedTrajDwTest));

   pos_speed_ctl_pip(
       &pos_speed_ctl_input,
       &axis->pos_speed_ctl_config,
       &axis->pos_speed_ctl_output,
       &(axis_dw->pos_speed_ctl_pip_InstanceData.rtdw));
#else
   axis->pos_speed_ctl_input.pos_tar_p = axis->pos_speed_ctl_input.pos_now_p; // 将位置目标等于当前值，位置误差清零

   pos_speed_ctl_pip(
       &axis->pos_speed_ctl_input,
       &axis->pos_speed_ctl_config,
       &axis->pos_speed_ctl_output,
       &(axis_dw->pos_speed_ctl_pip_InstanceData.rtdw));
#endif

   // 转矩脉动与摩擦补偿
   TqFcComStep(axis, axis_dw);

   // 指令电流滤波
   CommandCurrentFilter(axis, axis_dw);
}

3.3.3 pos_speed_ctl_pip()

由 Simulink 模型生成的地方的速度环函数：

/* Output and update for referenced model: 'pos_speed_ctl_pip' */
void pos_speed_ctl_pip(const PosSpeedCtlInput *rtu_input, const
                       PosSpeedCtlConfig *rtu_config, PosSpeedCtlOutput
                       *rty_output, pos_speed_ctl_pip_DW_f *localDW)
{
    ...
}

input、output 和 config 的结构体如下：

typedef struct
{
    /* 目标位置 */
    int64_T pos_tar_p;

    /* 当前位置 */
    int64_T pos_now_p;

    /* 最大目标q轴电流 */
    real32_T iq_max_A;

    /* 目标速度 */
    real32_T speed_tar_p_s;

    /* 目标加速度 */
    real32_T acc_tar_p_ss;

    /* 当前转速 */
    real32_T speed_now_rad_s;

    /* 目标位置偏置 */
    int64_T position_offset_p;

    /* 目标速度偏置 */
    real32_T speed_offset_p_s;
}

PosSpeedCtlInput;

typedef struct
{
    /* 目标q轴电流 */
    real32_T iq_tar_A;

    /* 当前位置控制误差 */
    int32_T pos_ctl_error_p;

    /* 负载扰动补偿电流 */
    real32_T dob_iq_com_A;
}

PosSpeedCtlOutput;

typedef struct
{
    /* 加速度前馈系数 */
    real32_T aff;

    /* 速度前馈系数 */
    real32_T vff;

    /* 编码器分辨率倒数 */
    real32_T enc_line_inv_p_n;

    /* 惯性系数 */
    real32_T j_kt;

    /* 速度环积分系数 */
    real32_T ki_s;

    /* 位置环比例系数 */
    real32_T kp_p;

    /* 速度环比例系数 */
    real32_T kp_s;

    /* 最大目标速度限制 */
    real32_T speed_max_rad_s;

    /* 位置速度环运行周期 */
    real32_T tp_s;

    /* 运行模式 */
    real32_T mode;

    /* 负载扰动观测器带宽 */
    real32_T dob_wn_Hz;

    /* 负载扰动观测使能开关 */
    uint8_T dob_enable;

    /* 速度环动态积分使能 */
    uint8_T dynamic_integration_enable;
}

PosSpeedCtlConfig;

3.4 CurrentCtlLoopTask() 系列逻辑

3.4.1 CurrentCtlLoopTask()

// 位置环中断调用
void PosSpeedCtlLoopTask(Axis *const axis, AxisDw *const axis_dw)
{
   uint32_T error_code = 0;

   // 运行电机端位置传感器数据处理
   motor_pos_sensor(&axis->motor_pos_sensor_input,
                    &axis->motor_pos_sensor_config,
                    &axis->motor_pos_sensor_output,
                    &(axis_dw->motor_pos_sensor_InstanceData.rtdw));

   axis->pos_speed_ctl_input.pos_now_p = axis->motor_pos_sensor_output.enc_sum_p; // 获取电机端当前绝对位置

   // 运行负载端位置传感器数据处理
   load_pos_sensor(&axis->load_pos_sensor_input,
                   &axis->load_pos_sensor_config,
                   &axis->load_pos_sensor_output,
                   &(axis_dw->load_pos_sensor_InstanceData.rtdw));

   // 运行速度观测器 进行速度估计
   axis->speed_obs_pll_input.pos_now_p = axis->pos_speed_ctl_input.pos_now_p;

   speed_obs_pll(&axis->speed_obs_pll_input, &axis->speed_obs_pll_config,
                 &axis->speed_obs_pll_output,
                 &(axis_dw->speed_obs_pll_InstanceData.rtdw));

   axis->pos_speed_ctl_input.speed_now_rad_s = axis->speed_obs_pll_output.ev_rad_s; // 速度观测器输出

   // 保护检测  更新状态机错误代码
   protect_check(&axis->current_ctl_input.iabc_now_A[0], &axis->current_ctl_input.dc_bus_now_V,
                 &axis->current_ctl_input.driver_temp,
                 &axis->pos_speed_ctl_output.pos_ctl_error_p,
                 &axis->pos_speed_ctl_input.speed_now_rad_s,
                 &axis->motor_ctl_sm_config,
                 &error_code);

   // 状态机切换
   // 错误状态
   if (error_code != MOTOR_CTL_SM_NONE_ERROR) // 有错误 进入错误状态
   {
      axis->motor_ctl_sm_output.state = MOTOR_CTL_SM_STATE_ERROR;
      axis->motor_ctl_sm_output.error = error_code;
   }

   if (axis->motor_ctl_sm_output.state == MOTOR_CTL_SM_STATE_ERROR)
   {
      if (axis->motor_ctl_sm_input.motor_enable == MOTOR_CTL_SM_MOTOR_CLEAR_ERROR &&
          axis->motor_ctl_sm_output.motor_enable_last == MOTOR_CTL_SM_MOTOR_DISABLE) // 清除错误  0->3 清除错误
      {
         axis->pos_speed_ctl_output.pos_ctl_error_p = 0;               // 清除位置跟随误差
         axis->motor_ctl_sm_output.state = MOTOR_CTL_SM_STATE_DISABLE; // 进入空闲状态
         axis->motor_ctl_sm_output.error = MOTOR_CTL_SM_NONE_ERROR;    // 清除错误
      }
      axis->motor_ctl_sm_output.motor_enable_last = axis->motor_ctl_sm_input.motor_enable;
      return; // 没有清除错误指令 直接退出
   }

   // 其他状态下 失能指令会立即进入失能状态
   if (axis->motor_ctl_sm_input.motor_enable == MOTOR_CTL_SM_MOTOR_DISABLE) // 失能电机状态
   {
      axis->motor_ctl_sm_output.state = MOTOR_CTL_SM_STATE_DISABLE; // 进入失能状态
   }

   // 失能状态
   if (axis->motor_ctl_sm_output.state == MOTOR_CTL_SM_STATE_DISABLE)
   {
      // 电流环 进入 电压控制  输出电压为 0
      axis->current_ctl_input.idq_tar_A[0] = 0.0f;
      axis->current_ctl_input.idq_tar_A[1] = 0.0f;
      axis->current_ctl_input.mode = 0; // 电压直接控制模式
      axis->current_ctl_input.uabc_tar_V[0] = 0.0f;
      axis->current_ctl_input.uabc_tar_V[1] = 0.0f;
      axis->current_ctl_input.uabc_tar_V[2] = 0.0f;
      axis->pos_speed_ctl_output.pos_ctl_error_p = 0; // 清除位置跟随误差
   }

   // 空闲状态
   if (axis->motor_ctl_sm_output.state == MOTOR_CTL_SM_STATE_DISABLE) // 空闲状态等待使能指令
   {
      // 使能状态  0->1 电机进入使能状态
      if (axis->motor_ctl_sm_input.motor_enable == MOTOR_CTL_SM_MOTOR_ENABLE &&
          axis->motor_ctl_sm_output.motor_enable_last == MOTOR_CTL_SM_MOTOR_DISABLE)
      {
         ModeCheckInInit(axis, axis_dw);                              // 使能前初始化
         axis->motor_ctl_sm_output.state = MOTOR_CTL_SM_STATE_ENABLE; // 进入使能状态
      }
   }

   // 使能状态 且 硬件准备就绪
   if (axis->motor_ctl_sm_output.state == MOTOR_CTL_SM_STATE_ENABLE)
   {
      if (axis->motor_ctl_sm_input.hw_ready_state)
      {
         ModeRun(axis, axis_dw); // 模式运行
      }
   }

   // 记录上个周期指令和状态
   axis->motor_ctl_sm_output.motor_enable_last = axis->motor_ctl_sm_input.motor_enable;
}