TEST(IQmath_ExtractInteger, IQ30int)
{
LONGS_EQUAL(1, _IQ30int(_IQ30(1.999999999))) ;
LONGS_EQUAL(-2, _IQ30int(_IQ30(-2.0))) ;
LONGS_EQUAL(-1, _IQ30int(_IQ30(-1.23456789))) ;
}
void setFeLimitZero(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
EST_State_e EstState = EST_getState(obj->estHandle);
_iq fe_neg_max_pu;
_iq fe_pos_min_pu;
if((EST_isMotorIdentified(obj->estHandle) == false) && (CTRL_getMotorType(handle) == MOTOR_Type_Induction))
{
fe_neg_max_pu = _IQ30(0.0);
fe_pos_min_pu = _IQ30(0.0);
}
else
{
fe_neg_max_pu = _IQ30(-USER_ZEROSPEEDLIMIT);
fe_pos_min_pu = _IQ30(USER_ZEROSPEEDLIMIT);
}
EST_setFe_neg_max_pu(obj->estHandle, fe_neg_max_pu);
EST_setFe_pos_min_pu(obj->estHandle, fe_pos_min_pu);
return;
} // end of setFeLimitZero() function
void USER_softwareUpdate1p6(CTRL_Handle handle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
float_t fullScaleInductance = USER_IQ_FULL_SCALE_VOLTAGE_V/(USER_IQ_FULL_SCALE_CURRENT_A*USER_VOLTAGE_FILTER_POLE_rps);
float_t Ls_coarse_max = _IQ30toF(EST_getLs_coarse_max_pu(obj->estHandle));
int_least8_t lShift = ceil(log(obj->motorParams.Ls_d_H/(Ls_coarse_max*fullScaleInductance))/log(2.0));
uint_least8_t Ls_qFmt = 30 - lShift;
float_t L_max = fullScaleInductance * pow(2.0,lShift);
_iq Ls_d_pu = _IQ30(obj->motorParams.Ls_d_H / L_max);
_iq Ls_q_pu = _IQ30(obj->motorParams.Ls_q_H / L_max);
// store the results
EST_setLs_d_pu(obj->estHandle,Ls_d_pu);
EST_setLs_q_pu(obj->estHandle,Ls_q_pu);
EST_setLs_qFmt(obj->estHandle,Ls_qFmt);
return;
} // end of softwareUpdate1p6() function
void CTRL_resetLs_qFmt(CTRL_Handle handle, const uint_least8_t Ls_qFmt)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
static bool LsResetLatch = false;
// reset Ls Q format to a higher value when Ls identification starts
if(EST_getState(obj->estHandle) == EST_State_Ls)
{
if((EST_getLs_d_pu(obj->estHandle) != _IQ30(0.0)) && (LsResetLatch == false))
{
EST_setLs_qFmt(obj->estHandle, Ls_qFmt);
LsResetLatch = true;
}
}
else
{
LsResetLatch = false;
}
return;
} // end of CTRL_resetLs_qFmt() function
TEST(IQmath_Multiplication, IQ30mpyNegative)
{
LONGS_EQUAL(_IQ30(0.75), _IQ30mpy(_IQ30(-1.5), _IQ30(-0.5))) ;
LONGS_EQUAL(_IQ30(-0.999999998), _IQ30mpy(_IQ30(-0.666666666), _IQ30(1.5))) ;
}
bool CTRL_updateState(CTRL_Handle handle)
{
CTRL_State_e ctrlState = CTRL_getState(handle);
bool flag_enableCtrl = CTRL_getFlag_enableCtrl(handle);
bool stateChanged = false;
if(flag_enableCtrl)
{
uint_least32_t waitTime = CTRL_getWaitTime(handle,ctrlState);
uint_least32_t counter_ctrlState = CTRL_getCount_state(handle);
// check for errors
CTRL_checkForErrors(handle);
if(counter_ctrlState >= waitTime)
{
// reset the counter
CTRL_resetCounter_state(handle);
if(ctrlState == CTRL_State_OnLine)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
_iq Id_target = TRAJ_getTargetValue(obj->trajHandle_Id);
// update the estimator state
bool flag_estStateChanged = EST_updateState(obj->estHandle,Id_target);
if(flag_estStateChanged)
{
// setup the controller
CTRL_setupCtrl(handle);
// setup the trajectory
CTRL_setupTraj(handle);
}
if(EST_isOnLine(obj->estHandle))
{
// setup the estimator for online state
CTRL_setupEstOnLineState(handle);
}
if(EST_isLockRotor(obj->estHandle) ||
(EST_isIdle(obj->estHandle) && EST_isMotorIdentified(obj->estHandle)))
{
// set the enable controller flag to false
CTRL_setFlag_enableCtrl(handle,false);
// set the next controller state
CTRL_setState(handle,CTRL_State_Idle);
}
}
else if(ctrlState == CTRL_State_OffLine)
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OnLine);
}
else if(ctrlState == CTRL_State_Idle)
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
bool flag_enableUserMotorParams = CTRL_getFlag_enableUserMotorParams(handle);
if(flag_enableUserMotorParams)
{
// initialize the motor parameters using values from the user.h file
CTRL_setUserMotorParams(handle);
}
if(EST_isIdle(obj->estHandle))
{
// setup the estimator for idle state
CTRL_setupEstIdleState(handle);
if(EST_isMotorIdentified(obj->estHandle))
{
if(CTRL_getFlag_enableOffset(handle))
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OffLine);
}
else
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OnLine);
}
}
else
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OffLine);
}
}
else if(EST_isLockRotor(obj->estHandle))
{
// set the next controller state
CTRL_setState(handle,CTRL_State_OnLine);
}
}
} // if(counter_ctrlState >= waitTime) loop
}
else
{
CTRL_Obj *obj = (CTRL_Obj *)handle;
// set the next controller state
CTRL_setState(handle,CTRL_State_Idle);
// set the estimator to idle
if(!EST_isLockRotor(obj->estHandle))
{
if(EST_isMotorIdentified(obj->estHandle))
{
EST_setIdle(obj->estHandle);
}
else
{
EST_setIdle_all(obj->estHandle);
EST_setRs_pu(obj->estHandle,_IQ30(0.0));
}
}
}
// check to see if the state changed
if(ctrlState != CTRL_getState(handle))
{
stateChanged = true;
}
return(stateChanged);
} // end of CTRL_updateState() function
CTRL_Obj ctrl; //v1p7 format #endif ST_Obj st_obj; ST_Handle stHandle; uint16_t gLEDcnt = 0; volatile MOTOR_Vars_t gMotorVars = MOTOR_Vars_INIT; #ifdef FLASH // Used for running BackGround in flash, and ISR in RAM extern uint16_t *RamfuncsLoadStart, *RamfuncsLoadEnd, *RamfuncsRunStart; #endif _iq gLs_pu = _IQ30(0.0); uint_least8_t gLs_qFmt = 0; uint_least8_t gMax_Ls_qFmt = 0; #ifdef DRV8301_SPI // Watch window interface to the 8301 SPI DRV_SPI_8301_Vars_t gDrvSpi8301Vars; #endif _iq gFlux_pu_to_Wb_sf; _iq gFlux_pu_to_VpHz_sf; _iq gTorque_Ls_Id_Iq_pu_to_Nm_sf; _iq gTorque_Flux_Iq_pu_to_Nm_sf;
