void CAL_setParams(CAL_Handle handle,const USER_Params *pUserParams)
{
CAL_Obj *obj = (CAL_Obj *)handle;
_iq beta_lp_pu = _IQ(pUserParams->offsetPole_rps/(float_t)pUserParams->ctrlFreq_Hz);
uint_least8_t cnt;
for(cnt=0;cnt<USER_NUM_CURRENT_SENSORS;cnt++)
{
OFFSET_setBeta(obj->offsetHandle_I[cnt],beta_lp_pu);
OFFSET_setInitCond(obj->offsetHandle_I[cnt],_IQ(0.0));
OFFSET_setOffset(obj->offsetHandle_I[cnt],_IQ(0.0));
}
for(cnt=0;cnt<USER_NUM_VOLTAGE_SENSORS;cnt++)
{
OFFSET_setBeta(obj->offsetHandle_V[cnt],beta_lp_pu);
OFFSET_setInitCond(obj->offsetHandle_V[cnt],_IQ(0.0));
OFFSET_setOffset(obj->offsetHandle_V[cnt],_IQ(0.0));
}
// set the wait times for each state
CAL_setWaitTimes(handle,&(pUserParams->calWaitTime[0]));
CAL_setCount_state(handle,0);
CAL_setFlag_enable(handle,false);
CAL_setFlag_enableAdcOffset(handle,true);
CAL_setState(handle,CAL_State_Idle);
return;
} // end of CAL_setParams() function
interrupt void mainISR(void)
{
// toggle status LED
if(gLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz))
{
HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
gLEDcnt = 0;
}
// acknowledge the ADC interrupt
HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
// convert the ADC data
HAL_readAdcData(halHandle,&gAdcData);
// run the controller
CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData);
// write the PWM compare values
HAL_writePwmData(halHandle,&gPwmData);
if(FW_getFlag_enableFw(fwHandle) == true)
{
FW_incCounter(fwHandle);
if(FW_getCounter(fwHandle) > FW_getNumIsrTicksPerFwTick(fwHandle))
{
_iq refValue;
_iq fbackValue;
_iq output;
FW_clearCounter(fwHandle);
refValue = gMotorVars.VsRef;
fbackValue = gMotorVars.Vs;
FW_run(fwHandle, refValue, fbackValue, &output);
CTRL_setId_ref_pu(ctrlHandle, output);
gMotorVars.IdRef_A = _IQmpy(CTRL_getId_ref_pu(ctrlHandle), _IQ(USER_IQ_FULL_SCALE_CURRENT_A));
}
}
else
{
CTRL_setId_ref_pu(ctrlHandle, _IQmpy(gMotorVars.IdRef_A, _IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
}
// setup the controller
CTRL_setup(ctrlHandle);
return;
} // end of mainISR() function
void ST_setupVelPlan(ST_Handle handle) {
_iq accMax, jrkMax;
ST_Obj *stObj = (ST_Obj *)handle;
// Pass the configuration array pointer into SpinTAC Velocity Plan
STVELPLAN_setCfgArray(stObj->velPlanHandle, &stVelPlanCfgArray[0], sizeof(stVelPlanCfgArray), 0, 0, 0, NUM_PLAN_TRANS, NUM_PLAN_STATES);
// Establish the Acceleration, and Jerk Maximums
accMax = _IQ24(10.0);
jrkMax = _IQ20(62.5);
// Configure SpinTAC Velocity Plan: Sample Time, LoopENB
STVELPLAN_setCfg(stObj->velPlanHandle, _IQ(ST_SAMPLE_TIME), false);
// Configure halt state: VelEnd, AccMax, JrkMax, Timer
STVELPLAN_setCfgHaltState(stObj->velPlanHandle, 0, accMax, jrkMax, 1000L);
//Example: STVELPLAN_addCfgState(handle, VelSetpoint[pups], StateTimer[ticks]);
STVELPLAN_addCfgState(stObj->velPlanHandle, 0, 200L); // StateA
STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(0.25 * ST_SPEED_PU_PER_KRPM), 2000L); // StateB
STVELPLAN_addCfgState(stObj->velPlanHandle, _IQ(-0.25 * ST_SPEED_PU_PER_KRPM), 2000L); // StateC
//Example: STVELPLAN_addCfgTran(handle, FromState, ToState, CondOption, CondIdx1, CondiIdx2, AccLim[pups2], JrkLim[pups3]);
STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_A, STATE_B, ST_COND_NC, 0, 0, _IQ(0.1), _IQ20(1)); // From StateA to StateB
STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_B, STATE_C, ST_COND_NC, 0, 0, _IQ(0.1), _IQ20(1)); // From StateB to StateC
STVELPLAN_addCfgTran(stObj->velPlanHandle, STATE_C, STATE_A, ST_COND_NC, 0, 0, _IQ(1), _IQ20(1)); // From StateC to StateA
// Note: set CondIdx1 to 0 if CondOption is ST_COND_NC; set CondIdx2 to 0 if CondOption is ST_COND_NC or ST_COND_FC
}
void ST_runVelMove(ST_Handle handle, CTRL_Handle ctrlHandle)
{
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Run SpinTAC Move
// If we are not in reset, and the SpeedRef_krpm has been modified
if((EST_getState(ctrlObj->estHandle) == EST_State_OnLine) && (_IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)) != STVELMOVE_getVelocityEnd(stObj->velMoveHandle))) {
// Get the configuration for SpinTAC Move
STVELMOVE_setCurveType(stObj->velMoveHandle, gMotorVars.SpinTAC.VelMoveCurveType);
STVELMOVE_setVelocityEnd(stObj->velMoveHandle, _IQmpy(gMotorVars.SpeedRef_krpm, _IQ(ST_SPEED_PU_PER_KRPM)));
STVELMOVE_setAccelerationLimit(stObj->velMoveHandle, _IQmpy(gMotorVars.MaxAccel_krpmps, _IQ(ST_SPEED_PU_PER_KRPM)));
STVELMOVE_setJerkLimit(stObj->velMoveHandle, _IQ20mpy(gMotorVars.MaxJrk_krpmps2, _IQ20(ST_SPEED_PU_PER_KRPM)));
// Enable SpinTAC Move
STVELMOVE_setEnable(stObj->velMoveHandle, true);
// If starting from zero speed, enable ForceAngle, otherwise disable ForceAngle
if(_IQabs(STVELMOVE_getVelocityStart(stObj->velMoveHandle)) < _IQ(ST_MIN_ID_SPEED_PU)) {
EST_setFlag_enableForceAngle(ctrlObj->estHandle, true);
gMotorVars.Flag_enableForceAngle = true;
}
else {
EST_setFlag_enableForceAngle(ctrlObj->estHandle, false);
gMotorVars.Flag_enableForceAngle = false;
}
}
STVELMOVE_run(stObj->velMoveHandle);
}
void recalcKpKi(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
EST_State_e EstState = EST_getState(obj->estHandle);
if((EST_isMotorIdentified(obj->estHandle) == false) && (EstState == EST_State_Rs))
{
float_t Lhf = CTRL_getLhf(handle);
float_t Rhf = CTRL_getRhf(handle);
float_t RhfoverLhf = Rhf/Lhf;
_iq Kp = _IQ(0.25*Lhf*USER_IQ_FULL_SCALE_CURRENT_A/(USER_CTRL_PERIOD_sec*USER_IQ_FULL_SCALE_VOLTAGE_V));
_iq Ki = _IQ(RhfoverLhf*USER_CTRL_PERIOD_sec);
// set Rhf/Lhf
CTRL_setRoverL(handle,RhfoverLhf);
// set the controller proportional gains
CTRL_setKp(handle,CTRL_Type_PID_Id,Kp);
CTRL_setKp(handle,CTRL_Type_PID_Iq,Kp);
// set the Id controller gains
CTRL_setKi(handle,CTRL_Type_PID_Id,Ki);
PID_setKi(obj->pidHandle_Id,Ki);
// set the Iq controller gains
CTRL_setKi(handle,CTRL_Type_PID_Iq,Ki);
PID_setKi(obj->pidHandle_Iq,Ki);
}
return;
} // end of recalcKpKi() function
void runRsOnLine(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
// execute Rs OnLine code
if(gMotorVars.Flag_Run_Identify == true)
{
if(EST_getState(obj->estHandle) == EST_State_OnLine)
{
float_t RsError_Ohm = gMotorVars.RsOnLine_Ohm - gMotorVars.Rs_Ohm;
EST_setFlag_enableRsOnLine(obj->estHandle,true);
EST_setRsOnLineId_mag_pu(obj->estHandle,_IQmpy(gMotorVars.RsOnLineCurrent_A,_IQ(1.0/USER_IQ_FULL_SCALE_CURRENT_A)));
if(abs(RsError_Ohm) < (gMotorVars.Rs_Ohm * 0.05))
{
EST_setFlag_updateRs(obj->estHandle,true);
}
}
else
{
EST_setRsOnLineId_mag_pu(obj->estHandle,_IQ(0.0));
EST_setRsOnLineId_pu(obj->estHandle,_IQ(0.0));
EST_setRsOnLine_pu(obj->estHandle,_IQ(0.0));
EST_setFlag_enableRsOnLine(obj->estHandle,false);
EST_setFlag_updateRs(obj->estHandle,false);
EST_setRsOnLine_qFmt(obj->estHandle,EST_getRs_qFmt(obj->estHandle));
}
}
return;
} // end of runRsOnLine() function
void USER_calcPIgains(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
float_t fullScaleCurrent = USER_IQ_FULL_SCALE_CURRENT_A;
float_t fullScaleVoltage = USER_IQ_FULL_SCALE_VOLTAGE_V;
float_t ctrlPeriod_sec = CTRL_getCtrlPeriod_sec(handle);
float_t Ls_d;
float_t Ls_q;
float_t Rs;
float_t RoverLs_d;
float_t RoverLs_q;
_iq Kp_Id;
_iq Ki_Id;
_iq Kp_Iq;
_iq Ki_Iq;
_iq Kd;
#ifdef __TMS320C28XX_FPU32__
int32_t tmp;
// when calling EST_ functions that return a float, and fpu32 is enabled, an integer is needed as a return
// so that the compiler reads the returned value from the accumulator instead of fpu32 registers
tmp = EST_getLs_d_H(obj->estHandle);
Ls_d = *((float_t *)&tmp);
tmp = EST_getLs_q_H(obj->estHandle);
Ls_q = *((float_t *)&tmp);
tmp = EST_getRs_Ohm(obj->estHandle);
Rs = *((float_t *)&tmp);
#else
Ls_d = EST_getLs_d_H(obj->estHandle);
Ls_q = EST_getLs_q_H(obj->estHandle);
Rs = EST_getRs_Ohm(obj->estHandle);
#endif
RoverLs_d = Rs/Ls_d;
Kp_Id = _IQ((0.25*Ls_d*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
Ki_Id = _IQ(RoverLs_d*ctrlPeriod_sec);
RoverLs_q = Rs/Ls_q;
Kp_Iq = _IQ((0.25*Ls_q*fullScaleCurrent)/(ctrlPeriod_sec*fullScaleVoltage));
Ki_Iq = _IQ(RoverLs_q*ctrlPeriod_sec);
Kd = _IQ(0.0);
// set the Id controller gains
PID_setKi(obj->pidHandle_Id,Ki_Id);
CTRL_setGains(handle,CTRL_Type_PID_Id,Kp_Id,Ki_Id,Kd);
// set the Iq controller gains
PID_setKi(obj->pidHandle_Iq,Ki_Iq);
CTRL_setGains(handle,CTRL_Type_PID_Iq,Kp_Iq,Ki_Iq,Kd);
return;
} // end of calcPIgains() function
/******************************************************************************
* Записывает параметры по умолчанию
*/
void prmInitDef(void){
uint8_t i;
ps.error.bit.noCalibration = 1;
sysset.kKorHighvoltage = 1125;
sysset.kLval = 10000;
sysset.hiTemp = 50;
sysset.lowTemp = 30;
sysset.freqBeep = 4000;
sysset.lcdContrast = 100;
sysset.spsUse = 0; //Используется sps
sysset.stopsFan = 1; //Не будет останавливатся куллер
/*******************************
* U
*/
sysset.pU[0].qu = _IQ(0.039);
sysset.pU[0].adc = 60;
sysset.pU[0].dac = 31;
sysset.pU[1].qu = _IQ(0.999);
sysset.pU[1].adc = 384;
sysset.pU[1].dac = 193;
/*******************************
* I
*/
sysset.pI[0].qi = _IQ(0.002);
sysset.pI[0].adc = 50;
sysset.pI[0].dac = 25;
sysset.pI[1].qi = _IQ(0.061);
sysset.pI[1].adc = 110;
sysset.pI[1].dac = 55;
/**************************************
* BS
*/
for(i = 0; i < NPRESET; i++){
bs.set[i].i = I_BIG_STEP;
bs.set[i].u = U_BIG_STEP;
bs.set[i].Mode = baseImax;
}
/**************************************
* CH
*/
ch.Idac = I_BIG_STEP;
ch.Mode = 0;
ch.Time = 60;
ch.Udac = U_BIG_STEP;
}
/*
* Function : vInitDataForTask6
* Arguments : None
* Returns : None
* Description : Initializes data points that will be used in Task 6 of the CLA
*/
void vInitDataForTask6()
{
sCPUtoCLAMsg.x1.iq24 = _IQ(1.2346); sCPUtoCLAMsg.x2.iq24 = _IQ(0.1233);
sCPUtoCLAMsg.x3.iq24 = _IQ(-3.1244); sCPUtoCLAMsg.x4.iq24 = _IQ(0.0199);
sCPUtoCLAMsg.x5.iq24 = _IQ(3.2162); sCPUtoCLAMsg.x6.iq24 = _IQ(3.2163);
sCPUtoCLAMsg.x7.iq24 = _IQ(0.0000); sCPUtoCLAMsg.x8.iq24 = _IQ(-1.5000);
}
extern void ANGLE_COMP_setParams(ANGLE_COMP_Handle handle,
float_t iqFullScaleFreq_Hz,
float_t pwmPeriod_usec,
uint_least16_t numPwmTicksPerIsrTick)
{
ANGLE_COMP_Obj *obj = (ANGLE_COMP_Obj *)handle;
float_t pwmPeriod_s = 1.0e-6 * pwmPeriod_usec;
obj->angleDeltaFactor = _IQ(iqFullScaleFreq_Hz * pwmPeriod_s);
obj->angleCompFactor = _IQ(1.0 + (float_t)numPwmTicksPerIsrTick * 0.5);
return;
} // end of ANGLE_COMP_setParams() function
void ST_runVelCtl(ST_Handle handle, CTRL_Handle ctrlHandle)
{
_iq speedFeedback, iqReference, gSpdError, gUp, gUi;
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Get the mechanical speed in pu
speedFeedback = EST_getFm_pu(ctrlObj->estHandle);
// Run the SpinTAC Controller
// Note that the library internal ramp generator is used to set the speed reference
STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd));
STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0)); // Internal ramp generator does not provide Acceleration Reference
STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback);
STVELCTL_run(stObj->velCtlHandle);
// select SpinTAC Velocity Controller
iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle);
if(gMotorVars.SpinTAC.VelCtlEnb == true) {
// Set the Iq reference that came out of SpinTAC Velocity Control
CTRL_setIq_ref_pu(ctrlHandle, iqReference);
// Update internal PI integrator, if running SpinTAC
gSpdError = TRAJ_getIntValue(ctrlObj->trajHandle_spd) - speedFeedback;
gUp = _IQmpy(gSpdError, CTRL_getKp(ctrlHandle, CTRL_Type_PID_spd));
gUi = _IQmpy(gSpdError, CTRL_getKi(ctrlHandle, CTRL_Type_PID_spd));
CTRL_setUi(ctrlHandle, CTRL_Type_PID_spd, iqReference - gUp - gUi);
}
}
//! \brief Sets up the throttle module parameters initially
//! \param[in] handle The throttle handle
extern void THROTTLE_setParams(THROTTLE_Handle handle, \
const bool invert,
const _iq max_adc, \
const _iq min_adc, \
const _iq max_out, \
const _iq min_out)
{
THROTTLE_Obj *obj = (THROTTLE_Obj *)handle;
obj->flagSw1 = false;
obj->flagSw2 = false;
obj->max_adc = max_adc;
obj->min_adc = min_adc;
obj->max_out = max_out;
obj->min_out = min_out;
obj->slope = _IQdiv((obj->max_out - obj->min_out),(obj->max_adc - obj->min_adc));
obj->offset = obj->max_out - _IQmpy(obj->slope,obj->max_adc);
obj->state = THROTTLE_Run;
obj->value = _IQ(0.0);
return;
}
//! \brief Computes the scale factor needed to convert from torque created by Ld, Lq, Id and Iq, from per unit to Nm
//!
_iq USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf(void)
{
float_t FullScaleInductance = (USER_IQ_FULL_SCALE_VOLTAGE_V/(USER_IQ_FULL_SCALE_CURRENT_A*USER_VOLTAGE_FILTER_POLE_rps));
float_t FullScaleCurrent = (USER_IQ_FULL_SCALE_CURRENT_A);
float_t lShift = ceil(log(USER_MOTOR_Ls_d/(0.7*FullScaleInductance))/log(2.0));
return(_IQ(FullScaleInductance*FullScaleCurrent*FullScaleCurrent*USER_MOTOR_NUM_POLE_PAIRS*1.5*pow(2.0,lShift)));
} // end of USER_computeTorque_Ls_Id_Iq_pu_to_Nm_sf() function
//! \brief Computes the scale factor needed to convert from per unit to V/Hz
//!
_iq USER_computeFlux_pu_to_VpHz_sf(void)
{
float_t FullScaleFlux = (USER_IQ_FULL_SCALE_VOLTAGE_V/(float_t)USER_EST_FREQ_Hz);
float_t maxFlux = (USER_MOTOR_RATED_FLUX*((USER_MOTOR_TYPE==MOTOR_Type_Induction)?0.05:0.7));
float_t lShift = -ceil(log(FullScaleFlux/maxFlux)/log(2.0));
return(_IQ(FullScaleFlux*pow(2.0,lShift)));
} // end of USER_computeFlux_pu_to_VpHz_sf() function
//! \brief Computes the scale factor needed to convert from torque created by flux and Iq, from per unit to Nm
//!
_iq USER_computeTorque_Flux_Iq_pu_to_Nm_sf(void)
{
float_t FullScaleFlux = (USER_IQ_FULL_SCALE_VOLTAGE_V/(float_t)USER_EST_FREQ_Hz);
float_t FullScaleCurrent = (USER_IQ_FULL_SCALE_CURRENT_A);
float_t maxFlux = (USER_MOTOR_RATED_FLUX*((USER_MOTOR_TYPE==MOTOR_Type_Induction)?0.05:0.7));
float_t lShift = -ceil(log(FullScaleFlux/maxFlux)/log(2.0));
return(_IQ(FullScaleFlux/(2.0*MATH_PI)*FullScaleCurrent*USER_MOTOR_NUM_POLE_PAIRS*1.5*pow(2.0,lShift)));
} // end of USER_computeTorque_Flux_Iq_pu_to_Nm_sf() function
PID_Handle PID_init(void *pMemory,const size_t numBytes)
{
PID_Handle handle;
if(numBytes < sizeof(PID_Obj))
return((PID_Handle)NULL);
// assign the handle
handle = (PID_Handle)pMemory;
// set some defaults
PID_setUi(handle,_IQ(0.0));
PID_setRefValue(handle,_IQ(0.0));
PID_setFbackValue(handle,_IQ(0.0));
return(handle);
} // end of PID_init() function
__interrupt void ecapISR(void)
{
// Clear capture (CAP) interrupt flags
CAP_clearInt(halHandle->capHandle, CAP_Int_Type_All);
// Compute the PWM high period (rising edge to falling edge)
uint32_t PwmDuty = (uint32_t) CAP_getCap2(halHandle->capHandle) - (uint32_t) CAP_getCap1(halHandle->capHandle);
// Assign the appropriate speed command, combine 0-5% and 95-100%
// 0-100% speed is proportional to 1-2ms high period
// 60MHz * 2ms = 120000
// 0-1%
if (PwmDuty <= 61000)
{
gSpeedRef_Ok = 0;
gSpeedRef_duty = _IQ(0);
gMotorVars.Flag_Run_Identify = 0;
}
// 1-99%
if ((PwmDuty > 61000) && (PwmDuty < 119000))
{
gSpeedRef_Ok = 0;
gSpeedRef_duty = _IQdiv(PwmDuty - 60000, 60000);
gMotorVars.Flag_Run_Identify = 1;
}
// 99-100%
else if (PwmDuty >= 119000)
{
gSpeedRef_Ok = 0;
gSpeedRef_duty = _IQ(1.0);
gMotorVars.Flag_Run_Identify = 1;
}
// Catch all
else
{
gSpeedRef_duty = _IQ(0);
gMotorVars.Flag_Run_Identify = 0;
}
// Clears an interrupt defined by group number
PIE_clearInt(halHandle->pieHandle, PIE_GroupNumber_4);
} // end of ecapISR() function
void updateGlobalVariables_motor(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *) handle;
#ifdef __TMS320C28XX_FPU32__
int32_t tmp;
#endif
// get the speed estimate
gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);
// get the real time speed reference coming out of the speed trajectory generator
gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle), EST_get_pu_to_krpm_sf(obj->estHandle));
// get the torque estimate
gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);
// get the magnetizing current
fpu_assign(gMotorVars.MagnCurr_A ,EST_getIdRated( obj->estHandle ));
// get the rotor resistance
fpu_assign(gMotorVars.Rr_Ohm, EST_getRr_Ohm( obj->estHandle ));
// get the stator resistance
fpu_assign(gMotorVars.Rs_Ohm , EST_getRs_Ohm( obj->estHandle ));
// get the stator inductance in the direct coordinate direction
fpu_assign(gMotorVars.Lsd_H, EST_getLs_d_H( obj->estHandle ));
// get the stator inductance in the quadrature coordinate direction
fpu_assign(gMotorVars.Lsq_H , EST_getLs_q_H( obj->estHandle ));
// get the flux in V/Hz in floating point
fpu_assign(gMotorVars.Flux_VpHz , EST_getFlux_VpHz( obj->estHandle ));
// get the flux in Wb in fixed point
gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);
// get the controller state
gMotorVars.CtrlState = CTRL_getState(handle);
// get the estimator state
gMotorVars.EstState = EST_getState(obj->estHandle);
// Get the DC buss voltage
gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus, _IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));
return;
} // end of updateGlobalVariables_motor() function
void updateGlobalVariables_motor(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
int32_t tmp;
// get the speed estimate
gMotorVars.Speed_krpm = EST_getSpeed_krpm(obj->estHandle);
// get the real time speed reference coming out of the speed trajectory generator
gMotorVars.SpeedTraj_krpm = _IQmpy(CTRL_getSpd_int_ref_pu(handle),EST_get_pu_to_krpm_sf(obj->estHandle));
// get the torque estimate
gMotorVars.Torque_Nm = USER_computeTorque_Nm(handle, gTorque_Flux_Iq_pu_to_Nm_sf, gTorque_Ls_Id_Iq_pu_to_Nm_sf);
// when calling EST_ functions that return a float, and fpu32 is enabled, an integer is needed as a return
// so that the compiler reads the returned value from the accumulator instead of fpu32 registers
// get the magnetizing current
tmp = EST_getIdRated(obj->estHandle);
gMotorVars.MagnCurr_A = *((float_t *)&tmp);
// get the rotor resistance
tmp = EST_getRr_Ohm(obj->estHandle);
gMotorVars.Rr_Ohm = *((float_t *)&tmp);
// get the stator resistance
tmp = EST_getRs_Ohm(obj->estHandle);
gMotorVars.Rs_Ohm = *((float_t *)&tmp);
// get the stator inductance in the direct coordinate direction
tmp = EST_getLs_d_H(obj->estHandle);
gMotorVars.Lsd_H = *((float_t *)&tmp);
// get the stator inductance in the quadrature coordinate direction
tmp = EST_getLs_q_H(obj->estHandle);
gMotorVars.Lsq_H = *((float_t *)&tmp);
// get the flux in V/Hz in floating point
tmp = EST_getFlux_VpHz(obj->estHandle);
gMotorVars.Flux_VpHz = *((float_t *)&tmp);
// get the flux in Wb in fixed point
gMotorVars.Flux_Wb = USER_computeFlux(handle, gFlux_pu_to_Wb_sf);
// get the controller state
gMotorVars.CtrlState = CTRL_getState(handle);
// get the estimator state
gMotorVars.EstState = EST_getState(obj->estHandle);
// Get the DC buss voltage
gMotorVars.VdcBus_kV = _IQmpy(gAdcData.dcBus,_IQ(USER_IQ_FULL_SCALE_VOLTAGE_V/1000.0));
return;
} // end of updateGlobalVariables_motor() function
void OFFSET_setBeta(OFFSET_Handle handle,const _iq beta)
{
OFFSET_Obj *obj = (OFFSET_Obj *)handle;
_iq a1 = (beta - _IQ(1.0));
_iq b0 = beta;
_iq b1 = 0;
FILTER_FO_setDenCoeffs(obj->filterHandle,a1);
FILTER_FO_setNumCoeffs(obj->filterHandle,b0,b1);
return;
} // end of OFFSET_setBeta() function
void setupClarke_I(CLARKE_Handle handle,const uint_least8_t numCurrentSensors)
{
_iq alpha_sf,beta_sf;
// initialize the Clarke transform module for current
if(numCurrentSensors == 3)
{
alpha_sf = _IQ(MATH_ONE_OVER_THREE);
beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
}
else if(numCurrentSensors == 2)
{
alpha_sf = _IQ(1.0);
beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
}
else
{
alpha_sf = _IQ(0.0);
beta_sf = _IQ(0.0);
}
// set the parameters
CLARKE_setScaleFactors(handle,alpha_sf,beta_sf);
CLARKE_setNumSensors(handle,numCurrentSensors);
return;
} // end of setupClarke_I() function
void CTRL_setupClarke_I(CTRL_Handle handle,uint_least8_t numCurrentSensors)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
_iq alpha_sf,beta_sf;
// initialize the Clarke transform module for current
if(numCurrentSensors == 3)
{
alpha_sf = _IQ(MATH_ONE_OVER_THREE);
beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
}
else if(numCurrentSensors == 2)
{
alpha_sf = _IQ(1.0);
beta_sf = _IQ(MATH_ONE_OVER_SQRT_THREE);
}
else
{
alpha_sf = _IQ(0.0);
beta_sf = _IQ(0.0);
}
// set the parameters
CLARKE_setScaleFactors(obj->clarkeHandle_I,alpha_sf,beta_sf);
CLARKE_setNumSensors(obj->clarkeHandle_I,numCurrentSensors);
return;
} // end of CTRL_setupClarke_I() function
void recalcKpKiPmsm(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
EST_State_e EstState = EST_getState(obj->estHandle);
if((EST_isMotorIdentified(obj->estHandle) == false) && (EstState == EST_State_Rs))
{
float_t Lhf = CTRL_getLhf(handle);
float_t Rhf = CTRL_getRhf(handle);
float_t RhfoverLhf = Rhf/Lhf;
_iq Kp = _IQ(0.05*Lhf*USER_IQ_FULL_SCALE_CURRENT_A/(USER_CTRL_PERIOD_sec*USER_IQ_FULL_SCALE_VOLTAGE_V));
_iq Ki = _IQ(RhfoverLhf*USER_CTRL_PERIOD_sec);
_iq Kp_spd = _IQ(0.005*USER_IQ_FULL_SCALE_FREQ_Hz*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
// set Rhf/Lhf
CTRL_setRoverL(handle,RhfoverLhf);
// set the controller proportional gains
CTRL_setKp(handle,CTRL_Type_PID_Id,Kp);
CTRL_setKp(handle,CTRL_Type_PID_Iq,Kp);
// set the Id controller gains
CTRL_setKi(handle,CTRL_Type_PID_Id,Ki);
PID_setKi(obj->pidHandle_Id,Ki);
// set the Iq controller gains
CTRL_setKi(handle,CTRL_Type_PID_Iq,Ki);
PID_setKi(obj->pidHandle_Iq,Ki);
// set speed gains
PID_setKp(obj->pidHandle_spd,Kp_spd);
}
else if(EstState == EST_State_RatedFlux)
{
_iq Ki_spd = _IQ(0.5*USER_IQ_FULL_SCALE_FREQ_Hz*USER_CTRL_PERIOD_sec*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
// Set Ki on closing the feedback loop
PID_setKi(obj->pidHandle_spd,Ki_spd);
}
else if(EstState == EST_State_RampDown)
{
_iq Kp_spd = _IQ(0.02*USER_IQ_FULL_SCALE_FREQ_Hz*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
_iq Ki_spd = _IQ(2.0*USER_IQ_FULL_SCALE_FREQ_Hz*USER_CTRL_PERIOD_sec*USER_MOTOR_MAX_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A);
// Set Kp and Ki of the speed controller
PID_setKp(obj->pidHandle_spd,Kp_spd);
PID_setKi(obj->pidHandle_spd,Ki_spd);
TRAJ_setTargetValue(obj->trajHandle_spdMax,_IQ(USER_MOTOR_RES_EST_CURRENT/USER_IQ_FULL_SCALE_CURRENT_A));
}
return;
} // end of recalcKpKiPmsm() function
//! \brief Computes Torque in Nm
//!
_iq USER_computeTorque_lbin(CTRL_Handle handle, const _iq torque_Flux_sf, const _iq torque_Ls_sf)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
_iq Flux_pu = EST_getFlux_pu(obj->estHandle);
_iq Id_pu = PID_getFbackValue(obj->pidHandle_Id);
_iq Iq_pu = PID_getFbackValue(obj->pidHandle_Iq);
_iq Ld_minus_Lq_pu = _IQ30toIQ(EST_getLs_d_pu(obj->estHandle)-EST_getLs_q_pu(obj->estHandle));
_iq Torque_Flux_Iq_Nm = _IQmpy(_IQmpy(Flux_pu,Iq_pu),torque_Flux_sf);
_iq Torque_Ls_Id_Iq_Nm = _IQmpy(_IQmpy(_IQmpy(Ld_minus_Lq_pu,Id_pu),Iq_pu),torque_Ls_sf);
_iq Torque_Nm = Torque_Flux_Iq_Nm + Torque_Ls_Id_Iq_Nm;
return(_IQmpy(Torque_Nm, _IQ(MATH_Nm_TO_lbin_SF)));
} // end of USER_computeTorque_lbin() function
extern void VS_FREQ_setParams(VS_FREQ_Handle handle,
float_t iqFullScaleFreq_Hz,
float_t iqFullScaleVoltage_V,
float_t maxVsMag_pu)
{
VS_FREQ_Obj *obj = (VS_FREQ_Obj *)handle;
obj->iqFullScaleFreq_Hz = iqFullScaleFreq_Hz;
obj->iqFullScaleVoltage_V = iqFullScaleVoltage_V;
obj->maxVsMag_pu = maxVsMag_pu;
obj->MaxFreq = _IQ(USER_MOTOR_FREQ_MAX/iqFullScaleFreq_Hz);
return;
} // end of VS_FREQ_setParams() function
extern void THROTTLE_runState(THROTTLE_Handle handle)
{
THROTTLE_Obj *obj = (THROTTLE_Obj *)handle;
if(obj->flagSw1)
{
obj->result = _IQ(0.0);
obj->state = THROTTLE_CalMaxMin;
obj->max_adc = obj->min_out;
obj->min_adc = obj->max_out;
}
else
{
switch (obj->state)
{
case THROTTLE_CalMaxMin:
if (obj->value > obj->max_adc)
obj->max_adc = obj->value;
else if (obj->value < obj->min_adc)
obj->min_adc = obj->value;
else if (obj->flagSw2)
obj->state = THROTTLE_CalCalc;
break;
case THROTTLE_CalCalc:
{
obj->slope = _IQdiv((obj->max_out - obj->min_out),(obj->max_adc - obj->min_adc));
obj->offset = obj->max_out - _IQmpy(obj->slope,obj->max_adc);
obj->state = THROTTLE_Run;
}
break;
case THROTTLE_Run:
{
_iq result = _IQmpy(obj->value,obj->slope) + obj->offset;
obj->result = _IQsat(result,obj->max_out,obj->min_out);
}
break;
}
}
return;
}
interrupt void mainISR(void)
{
// toggle status LED
if(gLEDcnt++ > (uint_least32_t)(USER_ISR_FREQ_Hz / LED_BLINK_FREQ_Hz))
{
HAL_toggleLed(halHandle,(GPIO_Number_e)HAL_Gpio_LED2);
gLEDcnt = 0;
}
// Check if speed reference signal is active
// If more than 2000 service routine cycles pass without signal, disable motor
if (gSpeedRef_Ok++ > 2000)
{
gSpeedRef_duty = _IQ(0);
gMotorVars.Flag_Run_Identify = 0;
gSpeedRef_Ok = 0;
}
// acknowledge the ADC interrupt
HAL_acqAdcInt(halHandle,ADC_IntNumber_1);
// convert the ADC data
HAL_readAdcData(halHandle,&gAdcData);
// run the controller
CTRL_run(ctrlHandle,halHandle,&gAdcData,&gPwmData);
// write the PWM compare values
HAL_writePwmData(halHandle,&gPwmData);
// setup the controller
CTRL_setup(ctrlHandle);
return;
} // end of mainISR() function
void ST_runVelCtl(ST_Handle handle, CTRL_Handle ctrlHandle)
{
_iq speedFeedback, iqReference;
ST_Obj *stObj = (ST_Obj *)handle;
CTRL_Obj *ctrlObj = (CTRL_Obj *)ctrlHandle;
// Get the mechanical speed in pu
speedFeedback = EST_getFm_pu(ctrlObj->estHandle);
// Run the SpinTAC Controller
// Note that the library internal ramp generator is used to set the speed reference
STVELCTL_setVelocityReference(stObj->velCtlHandle, TRAJ_getIntValue(ctrlObj->trajHandle_spd));
STVELCTL_setAccelerationReference(stObj->velCtlHandle, _IQ(0.0)); // Internal ramp generator does not provide Acceleration Reference
STVELCTL_setVelocityFeedback(stObj->velCtlHandle, speedFeedback);
STVELCTL_run(stObj->velCtlHandle);
// select SpinTAC Velocity Controller
iqReference = STVELCTL_getTorqueReference(stObj->velCtlHandle);
// Set the Iq reference that came out of SpinTAC Velocity Control
CTRL_setIq_ref_pu(ctrlHandle, iqReference);
}
void smopos_calc(SMOPOS *v)
{
_iq E0;
E0 = _IQ(0.5);
// Sliding mode current observer
v->EstIalpha = _IQmpy(v->Fsmopos,v->EstIalpha) + _IQmpy(v->Gsmopos,(v->Valpha-v->Ealpha-v->Zalpha));
v->EstIbeta = _IQmpy(v->Fsmopos,v->EstIbeta) + _IQmpy(v->Gsmopos,(v->Vbeta-v->Ebeta-v->Zbeta));
// Current errors
v->IalphaError = v->EstIalpha - v->Ialpha;
v->IbetaError = v->EstIbeta - v->Ibeta;
// Sliding control calculator
if (_IQabs(v->IalphaError) < E0)
v->Zalpha = _IQmpy(v->Kslide,_IQdiv(v->IalphaError,E0));
else if (v->IalphaError >= E0)
v->Zalpha = v->Kslide;
else if (v->IalphaError <= -E0)
v->Zalpha = -v->Kslide;
if (_IQabs(v->IbetaError) < E0)
v->Zbeta = _IQmpy(v->Kslide,_IQdiv(v->IbetaError,E0));
else if (v->IbetaError >= E0)
v->Zbeta = v->Kslide;
else if (v->IbetaError <= -E0)
v->Zbeta = -v->Kslide;
// Sliding control filter -> back EMF calculator
v->Ealpha = v->Ealpha + _IQmpy(v->Kslf,(v->Zalpha-v->Ealpha));
v->Ebeta = v->Ebeta + _IQmpy(v->Kslf,(v->Zbeta-v->Ebeta));
// Rotor angle calculator -> Theta = atan(-Ealpha,Ebeta)
v->Theta = _IQatan2PU(-v->Ealpha,v->Ebeta);
}
void speed_frq_calc(SPEED_MEAS_QEP *v)
{
_iq tmp1;
/* Differentiator */
/*
if (v->dir_QEP==0) // T2 is counting down
{
if (v->theta_elec>v->theta_old)
tmp1 = _IQ(1) - v->theta_elec + v->theta_old;
else
tmp1 = v->theta_elec - v->theta_old;
}
else if (v->dir_QEP==1) // T2 is counting up
{
if (v->theta_elec<v->theta_old)
tmp1 = _IQ(1) + v->theta_elec - v->theta_old;
else
tmp1 = v->theta_elec - v->theta_old;
}
tmp1 = _IQmpy(v->K1,tmp1); // Q21 = Q21*GLOBAL_Q
*/
// Differentiator
// Synchronous speed computation
if ((v->theta_elec < _IQ(0.9))&(v->theta_elec > _IQ(0.1)))
// Q21 = Q21*(GLOBAL_Q-GLOBAL_Q)
tmp1 = _IQmpy(v->K1,(v->theta_elec - v->theta_old));
else tmp1 = _IQtoIQ21(v->speed_frq);
// Low-pass filter
// Q21 = GLOBAL_Q*Q21 + GLOBAL_Q*Q21
tmp1 = _IQmpy(v->K2,_IQtoIQ21(v->speed_frq))+_IQmpy(v->K3,tmp1);
if (tmp1>_IQ21(1))
v->speed_frq = _IQ(1);
else if (tmp1<_IQ21(-1))
v->speed_frq = _IQ(-1);
else
v->speed_frq = _IQ21toIQ(tmp1);
/* Update the electrical angle */
v->theta_old = v->theta_elec;
/* Change motor speed from pu value to rpm value (GLOBAL_Q -> Q0) */
/* Q0 = Q0*GLOBAL_Q => _IQXmpy(), X = GLOBAL_Q */
v->speed_rpm = _IQmpy(v->rpm_max,v->speed_frq);
}