整体理解
理解为启动时为开环模式加速启动,到一定阶段转为FOC闭环处理。在运行过程中会根据外部输入命令进行处理。
流程简介:
STC_SPEED_MODE和STC_TORQUE_MODE在运行过程中的转换
开始启动的时候即revup 加速过程实际上是STC_TORQUE_MODE模式,运行到RUN的状态即FOC过程的时候转换为速度模式。
在SWITCH_OVER阶段使用 MCI_ExecBufferedCommands(&Mci[M1]);调用进行转换
调用关系为
系统定时器->MC_RunMotorControlTasks-> MC_Scheduler()
或 Systick is not executed due low priority so is necessary to TIMx_BRK_M1_IRQHandler call .系统定时器没有调用的情况下TIMx_BRK_M1_IRQHandler需要调用。
TIMx_BRK_M1_IRQHandler->MC_Scheduler()
MC_Scheduler(hMFTaskCounterM1递减计数为0)->MediumFrequencyTask
/**
* @brief Executes medium frequency periodic Motor Control tasks
*
* This function performs some of the control duties on Motor 1 according to the
* present state of its state machine. In particular, duties requiring a periodic
* execution at a medium frequency rate (such as the speed controller for instance)
* are executed here.
*/
__weak void TSK_MediumFrequencyTaskM1(void)
{
int16_t wAux = 0;
bool IsSpeedReliable = STO_PLL_CalcAvrgMecSpeedUnit(&STO_PLL_M1, &wAux);
PQD_CalcElMotorPower(pMPM[M1]);//检测速度是不是可靠,如果不可靠后续运行时需要错误处理
if (MCI_GetCurrentFaults(&Mci[M1]) == MC_NO_FAULTS)//检测是电流获取是否正常
{
if (MCI_GetOccurredFaults(&Mci[M1]) == MC_NO_FAULTS)//检测是否有错误
{
switch (Mci[M1].State)
{
case IDLE://空闲状态
{
if ((MCI_START == Mci[M1].DirectCommand) || (MCI_MEASURE_OFFSETS == Mci[M1].DirectCommand))
{//收到运行命令
RUC_Clear(&RevUpControlM1, MCI_GetImposedMotorDirection(&Mci[M1]));//初始化内部转速控制器状态
if (pwmcHandle[M1]->offsetCalibStatus == false)//没有校准转入校准
{
PWMC_CurrentReadingCalibr(pwmcHandle[M1], CRC_START);//通过读取没有电流时的电流值电压值作为校准,每次启动都需要调用。
Mci[M1].State = OFFSET_CALIB;
}
else//校准了就使能定时器相关通道,转入CHARGE_BOOT_CAP ,启动模式
{
/* calibration already done. Enables only TIM channels */
pwmcHandle[M1]->OffCalibrWaitTimeCounter = 1u;
PWMC_CurrentReadingCalibr(pwmcHandle[M1], CRC_EXEC);//通过读取没有电流时的电流值电压值作为校准,每次启动都需要调用。
R3_2_TurnOnLowSides(pwmcHandle[M1]);//开启PWM定时器相关设置,有可能电容开始充电
TSK_SetChargeBootCapDelayM1(CHARGE_BOOT_CAP_TICKS);//记录电容充电开始时间
Mci[M1].State = CHARGE_BOOT_CAP;//转入BOOT 启动模式
}
}
else
{
/* nothing to be done, FW stays in IDLE state */
}
break;
}
case OFFSET_CALIB:
{
if (MCI_STOP == Mci[M1].DirectCommand)//收到停止命令
{
TSK_MF_StopProcessing(&Mci[M1], M1);
}
else
{
if (PWMC_CurrentReadingCalibr(pwmcHandle[M1], CRC_EXEC))//通过读取没有电流时的电流值电压值作为校准,每次启动都需要调用。
{
if (MCI_MEASURE_OFFSETS == Mci[M1].DirectCommand)//根据以前的状态返回对应的应有状态。
{
FOC_Clear(M1);
MPM_Clear((MotorPowMeas_Handle_t*) pMPM[M1]);
Mci[M1].DirectCommand = MCI_NO_COMMAND;
Mci[M1].State = IDLE;
}
else//从IDLE转启动启动跳过来的话转入启动
{
R3_2_TurnOnLowSides(pwmcHandle[M1]);//前有介绍
TSK_SetChargeBootCapDelayM1(CHARGE_BOOT_CAP_TICKS);//前面有介绍
Mci[M1].State = CHARGE_BOOT_CAP;
}
}
else
{
/* nothing to be done, FW waits for offset calibration to finish */
}
}
break;
}
case CHARGE_BOOT_CAP:
{
if (MCI_STOP == Mci[M1].DirectCommand)//收到停止命令转入停止
{
TSK_MF_StopProcessing(&Mci[M1], M1);
}
else
{
if (TSK_ChargeBootCapDelayHasElapsedM1())//如果有开机电容需要判断到充电完成时间够再运行
{
R3_2_SwitchOffPWM(pwmcHandle[M1]);//关闭PWM
FOCVars[M1].bDriveInput = EXTERNAL;//设置影响FOC_CalcCurrRef,使用INTERNAL是内部根据速度等计算获得,使用EXTERNAL
STC_SetSpeedSensor( pSTC[M1], &VirtualSpeedSensorM1._Super );//设置速度传感器为虚拟传感器
STO_PLL_Clear(&STO_PLL_M1);//STO滑模观测的状态清0,后续到一定速度后会启动检测
FOC_Clear( M1 );//清除FOC状态
Mci[M1].State = START;//转到开始状态
PWMC_SwitchOnPWM(pwmcHandle[M1]);//开启PWM
}
else
{
/* nothing to be done, FW waits for bootstrap capacitor to charge */
}
}
break;
}
case START:
{
if (MCI_STOP == Mci[M1].DirectCommand)//如果是停止命令则停止
{
TSK_MF_StopProcessing(&Mci[M1], M1);
}
else
{
/* Mechanical speed as imposed by the Virtual Speed Sensor during the Rev Up phase. */
int16_t hForcedMecSpeedUnit;
qd_t IqdRef;
bool ObserverConverged = false;
/* Execute the Rev Up procedure */
if(! RUC_Exec(&RevUpControlM1))//进行加速过程,加速可以由多个步骤的加速设置组合而成
{
/* The time allowed for the startup sequence has expired 时间超过了允许的启动时间*/
MCI_FaultProcessing(&Mci[M1], MC_START_UP, 0);
}
else
{
/* Execute the torque open loop current start-up ramp:进行扭矩开环启动上坡
* Compute the Iq reference current as configured in the Rev Up sequence计算Iq电流参考加速的过程序列 */
IqdRef.q = STC_CalcTorqueReference( pSTC[M1] );//计算Iq值
IqdRef.d = FOCVars[M1].UserIdref;//Id使用用户设定值
/* Iqd reference current used by the High Frequency Loop to generate the PWM output */
FOCVars[M1].Iqdref = IqdRef;
}
//计算虚拟速度传感器的速度值
(void) VSS_CalcAvrgMecSpeedUnit(&VirtualSpeedSensorM1, &hForcedMecSpeedUnit);
/* check that startup stage where the observer has to be used has been reached 启动到了可以使用观测器的状态,可以使用滑膜观察器就可以使用闭环控制*/
if (true == RUC_FirstAccelerationStageReached(&RevUpControlM1))
{
ObserverConverged = STO_PLL_IsObserverConverged(&STO_PLL_M1, &hForcedMecSpeedUnit);
STO_SetDirection(&STO_PLL_M1, (int8_t)MCI_GetImposedMotorDirection(&Mci[M1]));
//将命令设置为从虚拟速度传感器启动转换阶段到其他速度传感器。
(void)VSS_SetStartTransition(&VirtualSpeedSensorM1, ObserverConverged);
}
if (ObserverConverged)//观测器是否收敛
{
qd_t StatorCurrent = MCM_Park(FOCVars[M1].Ialphabeta, SPD_GetElAngle(&STO_PLL_M1._Super));
/* Start switch over ramp. This ramp will transition from the revup to the closed loop FOC. 结束斜坡,加速转换成闭环的FOC*/
REMNG_Init(pREMNG[M1]);
(void)REMNG_ExecRamp(pREMNG[M1], FOCVars[M1].Iqdref.q, 0);
(void)REMNG_ExecRamp(pREMNG[M1], StatorCurrent.q, TRANSITION_DURATION);
Mci[M1].State = SWITCH_OVER;
}
}
break;
}
case SWITCH_OVER:
{
if (MCI_STOP == Mci[M1].DirectCommand)//是否停止
{
TSK_MF_StopProcessing(&Mci[M1], M1);
}
else
{
bool LoopClosed;
int16_t hForcedMecSpeedUnit;
if(! RUC_Exec(&RevUpControlM1))//进行加速过程
{
/* The time allowed for the startup sequence has expired */
MCI_FaultProcessing(&Mci[M1], MC_START_UP, 0);
}
else
{
/* Compute the virtual speed and positions of the rotor.
The function returns true if the virtual speed is in the reliability range 计算转子的虚拟速度和位置。 如果虚拟速度在可靠性范围内,则该函数返回 true*/
LoopClosed = VSS_CalcAvrgMecSpeedUnit(&VirtualSpeedSensorM1, &hForcedMecSpeedUnit);
/* Check if the transition ramp has completed. */
bool tempBool;
//从虚拟速度传感器启动转换阶段到其他速度传感器是否完成判断。
tempBool = VSS_TransitionEnded(&VirtualSpeedSensorM1);
LoopClosed = LoopClosed || tempBool;
/* If any of the above conditions is true, the loop is considered closed.
The state machine transitions to the START_RUN state. 如果上述任一条件为真,则认为循环是闭合的。状态机将转换为START_RUN状态。*/
if (true == LoopClosed)
{
#if ( PID_SPEED_INTEGRAL_INIT_DIV == 0 )
PID_SetIntegralTerm(&PIDSpeedHandle_M1, 0);
#else
PID_SetIntegralTerm(&PIDSpeedHandle_M1,
(((int32_t)FOCVars[M1].Iqdref.q * (int16_t)PID_GetKIDivisor(&PIDSpeedHandle_M1))
/ PID_SPEED_INTEGRAL_INIT_DIV));
#endif
/* USER CODE BEGIN MediumFrequencyTask M1 1 */
/* USER CODE END MediumFrequencyTask M1 1 */
STC_SetSpeedSensor(pSTC[M1], &STO_PLL_M1._Super); /*Observer has converged*/
FOC_InitAdditionalMethods(M1);
FOC_CalcCurrRef( M1 );
STC_ForceSpeedReferenceToCurrentSpeed(pSTC[M1]); /* Init the reference speed to current speed */
MCI_ExecBufferedCommands(&Mci[M1]); /* Exec the speed ramp after changing of the speed sensor 斜坡起步后执行相关的命令,加减速等*/
Mci[M1].State = RUN;
}
}
}
break;
}
case RUN://正常FOC运行
{
if (MCI_STOP == Mci[M1].DirectCommand)//停止命令
{
TSK_MF_StopProcessing(&Mci[M1], M1);
}
else
{
/* USER CODE BEGIN MediumFrequencyTask M1 2 */
/* USER CODE END MediumFrequencyTask M1 2 */
MCI_ExecBufferedCommands(&Mci[M1]);//前面有介绍
FOC_CalcCurrRef(M1);//计算Iq电流值
if(!IsSpeedReliable)
{
MCI_FaultProcessing(&Mci[M1], MC_SPEED_FDBK, 0);
}
}
break;
}
case STOP:
{
if (TSK_StopPermanencyTimeHasElapsedM1())
{
STC_SetSpeedSensor(pSTC[M1], &VirtualSpeedSensorM1._Super); /* sensor-less */
VSS_Clear(&VirtualSpeedSensorM1); /* Reset measured speed in IDLE */
/* USER CODE BEGIN MediumFrequencyTask M1 5 */
/* USER CODE END MediumFrequencyTask M1 5 */
Mci[M1].DirectCommand = MCI_NO_COMMAND;
Mci[M1].State = IDLE;
}
else
{
/* nothing to do, FW waits for to stop */
}
break;
}
case FAULT_OVER:
{
if (MCI_ACK_FAULTS == Mci[M1].DirectCommand)
{
Mci[M1].DirectCommand = MCI_NO_COMMAND;
Mci[M1].State = IDLE;
}
else
{
/* nothing to do, FW stays in FAULT_OVER state until acknowledgement */
}
}
break;
case FAULT_NOW:
{
Mci[M1].State = FAULT_OVER;
}
default:
break;
}
}
else
{
Mci[M1].State = FAULT_OVER;
}
}
else
{
Mci[M1].State = FAULT_NOW;
}
/* USER CODE BEGIN MediumFrequencyTask M1 6 */
/* USER CODE END MediumFrequencyTask M1 6 */
}
RUC 爬坡控制
根据设置控制速度和电流值,其中STC_ExecRamp是获取曲线的参数,
VSS_SetMecAcceleration将曲线对应参设置给电机控制。
/**
* @brief Main revup controller procedure executing overall programmed phases.
* @param pHandle: Pointer on Handle structure of RevUp controller.
* @retval Boolean set to false when entire revup phases have been completed.
*/
__weak bool RUC_Exec(RevUpCtrl_Handle_t *pHandle)
{
bool retVal = true;
#ifdef NULL_PTR_REV_UP_CTL
if (MC_NULL == pHandle)
{
retVal = false;
}
else
{
#endif
if (pHandle->hPhaseRemainingTicks > 0U)//加速时间没到0 减1
{
/* Decrease the hPhaseRemainingTicks.*/
pHandle->hPhaseRemainingTicks--;
} /* hPhaseRemainingTicks > 0 */
if (0U == pHandle->hPhaseRemainingTicks)//一个加速过程是否结束
{
if (pHandle->pCurrentPhaseParams != MC_NULL)//后续没有加速过程,是否还有有的话使用新的加速过程
{
/* If it becomes zero the current phase has been completed.*/
/* Gives the next command to STC and VSS.*/
(void)STC_ExecRamp(pHandle->pSTC, pHandle->pCurrentPhaseParams->hFinalTorque * pHandle->hDirection,(uint32_t)(pHandle->pCurrentPhaseParams->hDurationms));
VSS_SetMecAcceleration(pHandle->pVSS,pHandle->pCurrentPhaseParams->hFinalMecSpeedUnit * pHandle->hDirection,pHandle->pCurrentPhaseParams->hDurationms);//设置加速曲线参数
/* Compute hPhaseRemainingTicks.*/
pHandle->hPhaseRemainingTicks = (uint16_t)((((uint32_t)pHandle->pCurrentPhaseParams->hDurationms) * ((uint32_t)pHandle->hRUCFrequencyHz)) / 1000U );//设置对应曲线的运行时间
pHandle->hPhaseRemainingTicks++;//这个我认为可以可无,估计是想保证不为0
/*Set the next phases parameter pointer.*/
pHandle->pCurrentPhaseParams = pHandle->pCurrentPhaseParams->pNext; //指向下个电流设置曲线阶段//cstat !MISRAC2012-Rule-11.5
/*Increases the rev up stages counter.*/
pHandle->bStageCnt++;//过程计数,用于知道第几步了。
}
else//所有加速过程结束,后续没有新的加速设置
{
retVal = false;
}
}
#ifdef NULL_PTR_REV_UP_CTL
}
#endif
return (retVal);
}
VSS_CalcAvrgMecSpeedUnit
/**
* @brief This method must be called with the same periodicity
* on which speed control is executed.
* This method computes and stores rotor instantaneous el speed (express
* in dpp considering the measurement frequency) in order to provide it
* to SPD_CalcElAngle function and SPD_GetElAngle.
* Then compute store and return - through parameter
* hMecSpeedUnit - the rotor average mech speed, expressed in the unit
* defined by #SPEED_UNIT. Then return the reliability state of the
* sensor (always true).
* @param pHandle: handler of the current instance of the VirtualSpeedSensor component
* @param hMecSpeedUnit pointer to int16_t, used to return the rotor average
* mechanical speed (SPED_UNIT)
* @retval true = sensor information is reliable
* false = sensor information is not reliable
*/
__weak bool VSS_CalcAvrgMecSpeedUnit(VirtualSpeedSensor_Handle_t *pHandle, int16_t *hMecSpeedUnit)
{
bool SpeedSensorReliability;
#ifdef NULL_PTR_VIR_SPD_SEN
if ((MC_NULL == pHandle) || (MC_NULL == hMecSpeedUnit))
{
SpeedSensorReliability = false;
}
else
{
#endif
if (pHandle->hRemainingStep > 1u)//一个步数的计数判断,等于加速被切换成了多个阶段加速
{
pHandle->wElSpeedDpp32 += pHandle->wElAccDppP32;//一个32位的计数累加器一次加一个小的速度 32位精度较高,可以实现缓慢加速
//实际用的是是速度的高16位
#ifndef FULL_MISRA_C_COMPLIANCY_VIRT_SPD_SENS
//cstat !MISRAC2012-Rule-1.3_n !ATH-shift-neg !MISRAC2012-Rule-10.1_R6
pHandle->_Super.hElSpeedDpp = (int16_t)(pHandle->wElSpeedDpp32 >> 16);
#else
pHandle->_Super.hElSpeedDpp = (int16_t)(pHandle->wElSpeedDpp32 / 65536);
#endif
/* Convert dpp into MecUnit */
//从每步的步速加的值计算电机的速度
*hMecSpeedUnit = (int16_t)((((int32_t)pHandle->_Super.hElSpeedDpp)
* ((int32_t )pHandle->_Super.hMeasurementFrequency) * SPEED_UNIT)
/ (((int32_t)pHandle->_Super.DPPConvFactor) * ((int32_t)pHandle->_Super.bElToMecRatio)));
//保存电机的速度值
pHandle->_Super.hAvrMecSpeedUnit = *hMecSpeedUnit;
pHandle->hRemainingStep--;//步数减少
}
else if (1U == pHandle->hRemainingStep)//到最后一部前
{
*hMecSpeedUnit = pHandle->hFinalMecSpeedUnit;//电机速度应该等于设定速度了
pHandle->_Super.hAvrMecSpeedUnit = *hMecSpeedUnit;//保存设定速度
pHandle->_Super.hElSpeedDpp = (int16_t)((((int32_t)*hMecSpeedUnit) * ((int32_t)pHandle->_Super.DPPConvFactor)) / (((int32_t)SPEED_UNIT) * ((int32_t)pHandle->_Super.hMeasurementFrequency)));//从电机的设定速度计算每部的速度
pHandle->_Super.hElSpeedDpp *= ((int16_t)pHandle->_Super.bElToMecRatio);//这个可以和上面合并
pHandle->hRemainingStep = 0U;//加速步骤完成
}
else
{
*hMecSpeedUnit = pHandle->_Super.hAvrMecSpeedUnit;
}
/* If the transition is not done yet, we already know that speed is not reliable */
//在TSK_HighFrequencyTask->VSS_CalcElAngle中会调用一无状态观测器到有状态观测转换的过程。转换步数完成后会设置转换结束pHandle->bTransitionEnded = true;
if (false == pHandle->bTransitionEnded)
{
pHandle->_Super.bSpeedErrorNumber = pHandle->_Super.bMaximumSpeedErrorsNumber;
SpeedSensorReliability = false;
}
else
{
SpeedSensorReliability = SPD_IsMecSpeedReliable(&pHandle->_Super, hMecSpeedUnit);
}
#ifdef NULL_PTR_VIR_SPD_SEN
}
#endif
return (SpeedSensorReliability);
}