18 #include "sw/modules/cal/src/float/cal.h"
19 #include "sw/modules/ipark/src/float/ipark.h"
20 #include "sw/modules/park/src/float/park.h"
22 #include "sw/modules/svgen/src/float/svgen.h"
23 #include "sw/drivers/qep/src/32b/f28x/f2806x/qep.h"
24 #include "dmc_enc/sw/modules/enc/src/float/enc.h"
25 #include "dmc_enc/sw/modules/enc_cal/src/float/enc_cal.h"
207 #define GRAPH_SIZE 100
223 uint_least8_t ctrlNumber = 0;
224 uint_least8_t estNumber = 0;
225 uint_least8_t graphCounter = 0;
233 USER_setParams_priv(&gUserParams);
237 halHandle =
HAL_init(&hal,
sizeof(hal));
245 angleHandle =
ANGLE_init(&angle,
sizeof(angle));
253 calHandle =
CAL_init(&cal,
sizeof(cal));
261 clarkeHandle_I =
CLARKE_init(&clarke_I,
sizeof(clarke_I));
262 clarkeHandle_V =
CLARKE_init(&clarke_V,
sizeof(clarke_V));
271 ctrlHandle =
CTRL_init(&(ctrl[ctrlNumber]),
sizeof(ctrl[ctrlNumber]));
287 encHandle =
ENC_init(&enc,
sizeof(enc));
288 ENC_setParams(encHandle,\
300 USER_MOTOR_MAX_CURRENT_A,\
324 if(gUserParams.
motor_type == MOTOR_Type_Induction)
332 iparkHandle =
IPARK_init(&ipark,
sizeof(ipark));
336 parkHandle =
PARK_init(&park,
sizeof(park));
340 queueHandle =
QUEUE_init(&queue,
sizeof(queue));
344 svgenHandle =
SVGEN_init(&svgen,
sizeof(svgen));
383 for(graphCounter=0; graphCounter<
GRAPH_SIZE; graphCounter++)
414 if(gUserParams.
motor_type == MOTOR_Type_Induction)
428 if(gUserParams.
motor_type == MOTOR_Type_Induction)
532 if(flag_calStateChanged)
536 if(calState == CAL_State_Done)
561 bool flag_EncCalStateChanged = ENC_CAL_updateState(
enc_calHandle);
563 if(flag_EncCalStateChanged)
565 ENC_CAL_State_e enc_calState = ENC_CAL_getState(
enc_calHandle);
567 if(enc_calState == ENC_CAL_State_Done)
589 if(flag_trajStateChanged)
594 (state == EST_Traj_State_OnLine))
601 EST_setMinValue_spd(
estHandle,spd_min_Hz);
602 EST_setMaxValue_spd(
estHandle,spd_max_Hz);
603 EST_setMaxDelta_spd(
estHandle,spd_maxDelta_Hz);
624 if(flag_estStateChanged)
652 if(gUserParams.
motor_type == MOTOR_Type_Induction)
707 float_t RoverLd_rps = Rs_Ohm / Ls_d_H;
708 float_t RoverLq_rps = Rs_Ohm / Ls_q_H;
712 float_t Kp_Id = Ls_d_H * BWc_rps;
713 float_t Ki_Id = RoverLd_rps * currentCtrlPeriod_sec;
715 float_t Kp_Iq = Ls_q_H * BWc_rps;
716 float_t Ki_Iq = RoverLq_rps * currentCtrlPeriod_sec;
787 if(count_traj >= numIsrTicksPerTrajTick)
812 if(count_est >= numIsrTicksPerEstTick)
834 argList.
arg[3] = NULL;
842 &gEstOutputData[gBufferIndex_est]);
889 if(count_ctrl >= numIsrTicksPerCtrlTick)
892 bool flag_doCurrentCtrl =
true;
893 bool flag_useZeroIq_ref =
false;
894 float_t angleElec_rad = ENC_getAngleElec(encHandle);
895 float_t angleMech_rad = ENC_getAngleMech(encHandle);
896 float_t oneOverDcBus_invV = 1/24.0;
909 uint16_t flagQepInt = QEP_read_interrupt_flag(hal.qepHandle[0],QEINT_Uto);
910 uint32_t posCntReg = QEP_read_posn_count(hal.qepHandle[0]);
913 if(flagQepInt == QEINT_Uto)
915 uint16_t qepsts = QEP_read_status(hal.qepHandle[0]);
916 bool qepDirection = ((qepsts & QEP_QEPSTS_QDF) == QEP_QEPSTS_QDF);
917 bool qepCapOverflow = ((qepsts & QEP_QEPSTS_COEF) == QEP_QEPSTS_COEF);
918 bool qepsts_upevent = ((qepsts & QEP_QEPSTS_UPEVNT) == QEP_QEPSTS_UPEVNT);
919 uint32_t qepPosFreq = QEP_read_posn_latch(hal.qepHandle[0]);
920 uint16_t qepCapPrd = QEP_read_capture_period_latch(hal.qepHandle[0]);
922 QEP_clear_interrupt_flag(hal.qepHandle[0],QEINT_Uto);
926 QEP_reset_status(hal.qepHandle[0],UPEVNT);
928 QEP_reset_status(hal.qepHandle[0],COEF);
931 ENC_setupSpeed(encHandle, \
937 ENC_runSpeed(encHandle);
948 ENC_setupAngle(encHandle,posCntReg);
950 ENC_runAngle(encHandle);
956 QEP_clear_posn_counter(hal.qepHandle[0]);
959 Idq_offset_A.
value[1] = 0.0;
961 flag_doSpeedCtrl =
false;
974 angleElec_rad = ENC_getAngleElec(encHandle);
975 angleMech_rad = ENC_getAngleMech(encHandle);
979 Vdq_offset_V.
value[0] = 0.0;
980 Vdq_offset_V.
value[1] = 0.0;
992 PARK_run(parkHandle,&Iab_in_A,&Idq_A);
1007 flag_useZeroIq_ref);
1019 angleWithDelay_rad = angleElec_rad;
1031 IPARK_run(iparkHandle,&Vdq_out_V,&Vab_out_V);
1135 if(numCurrentSensors == 3)
1140 else if(numCurrentSensors == 2)
1165 if(numVoltageSensors == 3)
float_t gCpuUsagePercentageAvg
SVGEN_Obj svgen
the space vector generator object
#define USER_SYSTEM_FREQ_MHz
CLOCKS & TIMERS.
bool EST_isError(EST_Handle handle)
float_t EST_getRsOnLine_Ohm(EST_Handle handle)
void EST_enableTraj(EST_Handle handle)
void ANGLE_setup(ANGLE_Handle handle, const int_least32_t timeStamp, const float_t angle_rad, const float_t speed_rps, const float_t accel_rps2)
EST_State_e gEstState
Global variable for the estimator state.
float_t angleDelayed_sf_sec
HAL_Obj hal
the hardware abstraction layer object
void EST_setFlag_enableForceAngle(EST_Handle handle, const bool state)
static void CAL_enable(CAL_Handle handle)
IPARK_Handle IPARK_init(void *pMemory, const size_t numBytes)
void EST_resetTrajCounter_isr(EST_Handle handle)
EST_Traj_State_e gTrajState
EST_Handle estHandle
the handle for the estimator
void CTRL_incrCounter_isr(CTRL_Handle handle)
volatile float_t gRsOnLineId_mag_A
void HAL_enableGlobalInts(HAL_Handle handle)
void HAL_enableAdcInts(HAL_Handle handle)
bool EST_getFlag_estComplete(EST_Handle handle)
float_t EST_getFlux_Wb(EST_Handle handle)
static float_t CTRL_getCurrentCtrlPeriod_sec(CTRL_Handle handle)
void EST_incrTrajCounter_isr(EST_Handle handle)
void EST_setRsOnLineId_mag_A(EST_Handle handle, const float_t Id_mag_A)
void CTRL_run(CTRL_Handle handle, HAL_Handle halHandle, const HAL_AdcData_t *pAdcData, HAL_PwmData_t *pPwmData)
EST_OutputData_t gEstOutputData[NUM_EST_DATA_BUFFERS]
uint16_t Graph_Tick_Counter
void calcGains(CTRL_Handle handle, EST_Handle estHandle)
Calculates controller gains.
float_t EST_getRr_q_Ohm(EST_Handle handle)
static void CAL_run(CAL_Handle handle, const HAL_AdcData_t *pAdcData)
float_t gLs_d_H
Global variable for the stator inductance in the direct coordinate direction, Henry.
PARK_Handle parkHandle
the handle for the Park object
static void PARK_setPhasor(PARK_Handle handle, const MATH_vec2 *pPhasor)
QUEUE_Handle QUEUE_init(void *pMemory, const size_t numBytes)
volatile bool gFlag_updateRs
float_t gRs_b_Ohm
Global variable for the stator resistance in the beta coordinate direction, Ohm.
float_t EST_getRoverL_rps(EST_Handle handle)
float_t gIdRated_A
Global variable for the rated Id current current, A.
void ANGLE_run(ANGLE_Handle handle, const int_least32_t timeStamp)
CAL_Obj cal
the calibrator object
float_t gRoverL_rps
Global variable for the R/L value used for the current controller.
#define USER_EST_FREQ_Hz
Defines the estimator frequency, Hz.
int_least16_t EST_getCount_isr(EST_Handle handle)
static void HAL_writePwmData(HAL_Handle handle, HAL_PwmData_t *pPwmData)
bool EST_isOnLine(EST_Handle handle)
static float_t CTRL_getBWc_rps(CTRL_Handle handle)
struct _EST_Obj_ * EST_Handle
void HAL_writeDrvData(HAL_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars)
bool CTRL_isError(CTRL_Handle handle)
CTRL_Obj ctrl[CTRL_NUM_CONTROLLERS]
the controller object
uint_least16_t CTRL_getNumIsrTicksPerCtrlTick(CTRL_Handle handle)
void EST_incrCounter_isr(EST_Handle handle)
float_t Graph_Data3[GRAPH_SIZE]
volatile bool gFlag_enableRsRecalc
float_t gRs_d_Ohm
Global variable for the stator resistance in the direct coordinate direction, Ohm.
static void HAL_acqAdcInt(HAL_Handle handle, const ADC_IntNumber_e intNumber)
float_t EST_getRr_d_Ohm(EST_Handle handle)
static void CTRL_setSpeed_outMax_A(CTRL_Handle handle, const float_t speed_outMax_A)
void * arg[EVENT_MAX_NUM_ARGS]
static void CAL_setFlag_enableAdcOffset(CAL_Handle handle, const bool value)
float_t gCpuUsagePercentageMin
uint_least8_t numCurrentSensors
IPARK_Handle iparkHandle
the handle for the inverse Park transform
#define USER_MOTOR_NUM_POLE_PAIRS
bool EST_isEnabled(EST_Handle handle)
volatile bool gflag_configCtrl_once
void EST_setupTraj(EST_Handle handle, CTRL_Handle ctrlHandle, const float_t targetValue_spd_Hz, const float_t targetValue_Id_A)
void CAL_setParams(CAL_Handle handle, const USER_Params *pUserParams)
ENC_CAL_Obj enc_cal
the encoder calibration object
void EST_setParams(EST_Handle handle, USER_Params *pUserParams)
static void SVGEN_run(SVGEN_Handle handle, const MATH_vec2 *pVab, MATH_vec3 *pT)
static uint32_t CPU_USAGE_getAvgDeltaCntObserved(CPU_USAGE_Handle handle)
static MATH_vec2 * CTRL_getIdq_ref_A_addr(CTRL_Handle handle)
static status QUEUE_postEventLast(QUEUE_Handle handle, const EVENT_Fxn eventFxn, const EVENT_ArgList *pArgList, const uint_least8_t numArgs)
void EST_configureTraj(EST_Handle handle)
volatile float_t gSpeed_Kp
#define MATH_ONE_OVER_THREE
CAL_Handle calHandle
the handle for the calibrator
void USER_setParams(USER_Params *pUserParams)
Sets the user parameter values.
uint_least16_t CTRL_getCount_isr(CTRL_Handle handle)
void setupClarke_V(CLARKE_Handle handle, const uint_least8_t numVoltageSensors)
Sets the number of voltage sensors.
void EST_setFlag_updateRs(EST_Handle handle, const bool state)
float_t EST_computeTorque_Nm(EST_Handle handle)
bool EST_isTrajEnabled(EST_Handle handle)
void CTRL_setGains(CTRL_Handle handle, const CTRL_Type_e ctrlType, const _iq Kp, const _iq Ki, const _iq Kd)
CLARKE_Obj clarke_V
the voltage Clarke transform object
static void CLARKE_setScaleFactors(CLARKE_Handle handle, const _iq alpha_sf, const _iq beta_sf)
void EST_enable(EST_Handle handle)
CTRL_Handle CTRL_init(void *pMemory, const size_t numBytes)
static void incrCount_isr(void)
Increments the ISR counter.
void HAL_setupFaults(HAL_Handle handle)
IPARK_Obj ipark
the inverse Park transform object
float_t EST_getLs_d_H(EST_Handle handle)
ENC_CAL_State_e gEncCalState
void EST_setFlag_enableRsRecalc(EST_Handle handle, const bool state)
USER_Params gUserParams
The user parameters.
void updateCPUusage(CTRL_Handle handle)
Updates CPU usage.
static void incrBufferIndex_est(void)
Increments the estimation buffer index.
static void HAL_enablePwm(HAL_Handle handle)
void EST_configureCtrl(EST_Handle handle, CTRL_Handle ctrlHandle)
CTRL_Version gVersion
Global variable for the controller version.
void CTRL_setParams(CTRL_Handle handle, USER_Params *pUserParams)
void EST_setFlag_enablePowerWarp(EST_Handle handle, const bool state)
QUEUE_Obj queue
the queue object
int_least32_t gCounter_isr
A counter that is incremented by each call to the mainISR() function.
CLARKE_Handle clarkeHandle_V
the handle for the voltage Clarke transform
void HAL_enableDrv(HAL_Handle handle)
void CTRL_resetCounter_isr(CTRL_Handle handle)
EST_Handle EST_initEst(const uint_least8_t estNumber)
ANGLE_Handle ANGLE_init(void *pMemory, const size_t numBytes)
volatile bool gFlag_enableRsOnLine
uint_least32_t trajFreq_Hz
float_t EST_getIdRated_A(EST_Handle handle)
void HAL_setupDrvSpi(HAL_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars)
volatile bool gFlag_enablePowerWarp
uint_least8_t numVoltageSensors
float_t EST_getLs_q_H(EST_Handle handle)
void HAL_enableDebugInt(HAL_Handle handle)
volatile float_t gRsOnLineFreq_Hz
CLARKE_Handle CLARKE_init(void *pMemory, const size_t numBytes)
int_least32_t gCounter_main
A counter that is incremented by each iteration of the main() function.
void EST_disableTraj(EST_Handle handle)
CTRL_State_e CTRL_getState(CTRL_Handle handle)
volatile bool gFlag_runEncCal
uint_least16_t gBufferIndex_setup
A counter that to denote the buffer index for the setup.
static void IPARK_run(IPARK_Handle handle, const MATH_vec2 *pInVec, MATH_vec2 *pOutVec)
ENC_Handle ENC_init(void *pMemory, const size_t numBytes)
float_t EST_getRs_b_Ohm(EST_Handle handle)
EST_State_e EST_getState(EST_Handle handle)
static void CAL_disable(CAL_Handle handle)
void setupClarke_I(CLARKE_Handle handle, const uint_least8_t numCurrentSensors)
Sets the number of current sensors.
void CTRL_setup(CTRL_Handle handle)
void EST_setFlag_estComplete(EST_Handle handle, const bool state)
volatile bool gFlag_runInSpeedMode
HAL_DacData_t gDacData
Defines the DAC data.
float_t gLs_q_H
Global variable for the stator inductance in the quadrature coordinate direction, Henry...
volatile uint16_t Graph_Tick_Counter_Value
float_t gRs_a_Ohm
Global variable for the stator resistance in the alpha coordinate direction, Ohm. ...
float_t EST_getRs_a_Ohm(EST_Handle handle)
static bool CTRL_isEnabled(CTRL_Handle handle)
void EST_setFlag_enableRsOnLine(EST_Handle handle, const bool state)
interrupt void mainISR(void)
The main interrupt service (ISR) routine.
float_t gCpuUsagePercentageMax
int_least16_t EST_getNumIsrTicksPerEstTick(EST_Handle handle)
static OFFSET_Handle * CAL_getOffsetHandleAddr_I(CAL_Handle handle)
static uint32_t HAL_readTimerCnt(HAL_Handle handle, const uint_least8_t timerNumber)
void EST_setIdq_ref_A(EST_Handle handle, const MATH_vec2 *pIdq_ref_A)
CPU_USAGE_Handle cpu_usageHandle
static void CLARKE_setNumSensors(CLARKE_Handle handle, const uint_least8_t numSensors)
volatile bool gFlag_enableQueue
float_t EST_getRs_Ohm(EST_Handle handle)
float_t EST_computeLmag_H(EST_Handle handle, const float_t current_A)
float_t gRr_d_Ohm
Global variable for the rotor resistance in the direct coordinate direction, Ohm. ...
float_t EST_getRs_q_Ohm(EST_Handle handle)
void updateGlobalVariables_motor(CTRL_Handle ctrlHandle, EST_Handle estHandle)
Updates the global motor variables.
void ANGLE_setParams(ANGLE_Handle handle, const float_t timeStampPeriod_sec, const float_t angleDelayed_sf_sec)
void CPU_USAGE_setParams(CPU_USAGE_Handle handle, const uint32_t timerPeriod_cnts, const uint32_t numDeltaCntsAvg)
void EST_runTraj(EST_Handle handle)
static bool CAL_isError(CAL_Handle handle)
#define USER_ISR_FREQ_Hz
Defines the Interrupt Service Routine (ISR) frequency, Hz.
static void IPARK_setPhasor(IPARK_Handle handle, const MATH_vec2 *pPhasor)
ENC_Obj enc
the encoder object
CPU_USAGE_Handle CPU_USAGE_init(void *pMemory, const size_t numBytes)
volatile float_t gRsOnLinePole_Hz
float_t gRs_q_Ohm
Global variable for the stator resistance in the quadrature coordinate direction, Ohm...
static void HAL_disablePwm(HAL_Handle handle)
void EST_setRsOnLineAngleDelta_rad(EST_Handle handle, const float_t angleDelta_rad)
bool EST_isMotorIdentified(EST_Handle handle)
ENC_Handle encHandle
the handle for the encoder
float_t Graph_Data2[GRAPH_SIZE]
float_t EST_getRs_d_Ohm(EST_Handle handle)
HAL_Handle HAL_init(void *pMemory, const size_t numBytes)
bool EST_isLockRotor(EST_Handle handle)
static void CLARKE_run(CLARKE_Handle handle, const MATH_vec3 *pInVec, MATH_vec2 *pOutVec)
static uint32_t CPU_USAGE_getMaxDeltaCntObserved(CPU_USAGE_Handle handle)
HAL_Handle halHandle
the handle for the hardware abstraction layer
void EST_setRsOnLine_beta_rad(EST_Handle handle, const float_t beta_rad)
static void PARK_run(PARK_Handle handle, const MATH_vec2 *pInVec, MATH_vec2 *pOutVec)
volatile float_t gSpeed_Ki
static CAL_State_e CAL_getState(CAL_Handle handle)
static void CTRL_setSpeed_outMin_A(CTRL_Handle handle, const float_t speed_outMin_A)
float_t Graph_Data1[GRAPH_SIZE]
float_t gFlux_Wb
Global variable for the rotor flux estimate, Wb.
#define NUM_EST_DATA_BUFFERS
Defines the number of estimator data buffers NOTE: Must be a multiple of two.
static void HAL_readAdcData(HAL_Handle handle, HAL_AdcData_t *pAdcData)
CAL_Handle CAL_init(void *pMemory, const size_t numBytes)
#define USER_TRAJ_FREQ_Hz
Defines the trajectory frequency, Hz.
void SVGEN_setup(SVGEN_Handle svgenHandle)
QUEUE_Handle queueHandle
the handle for the queue
uint_least16_t gBufferIndex_est
A counter that to denote the buffer index for the estimation.
ENC_CAL_Handle enc_calHandle
the handle for the encoder calibration
CTRL_Handle ctrlHandle
the handle for the controller
PARK_Obj park
the Park transform object
bool EST_isIdle(EST_Handle handle)
volatile bool gFlag_bypassLockRotor
static void CPU_USAGE_updateCnts(CPU_USAGE_Handle handle, const uint32_t cnt)
volatile bool gFlag_enableSys
Global flag to enable/disable the system.
static uint32_t CPU_USAGE_getMinDeltaCntObserved(CPU_USAGE_Handle handle)
#define MATH_ONE_OVER_SQRT_THREE
bool EST_isTrajError(EST_Handle handle)
EST_Traj_State_e EST_getTrajState(EST_Handle handle)
float_t gLmag_H
Global variable for the magnetizing inductance, Henry.
static void HAL_initIntVectorTable(HAL_Handle handle)
static void QUEUE_executeEvent(QUEUE_Handle handle)
static void HAL_updateAdcBias(HAL_Handle handle)
PARK_Handle PARK_init(void *pMemory, const size_t numBytes)
float_t EST_getIdRated_indEst_A(EST_Handle handle)
void CTRL_getVersion(CTRL_Handle handle, CTRL_Version *pVersion)
static uint16_t HAL_readPwmPeriod(HAL_Handle handle, const PWM_Number_e pwmNumber)
HAL_PwmData_t gPwmData
Defines the PWM data.
static bool QUEUE_isEvent(QUEUE_Handle handle)
static void CTRL_enable(CTRL_Handle handle)
bool EST_updateState(EST_Handle handle, const _iq Id_target_pu)
static void CTRL_disable(CTRL_Handle handle)
void CTRL_computePhasor(const _iq angle_pu, MATH_vec2 *pPhasor)
#define CTRL_NUM_CONTROLLERS
void EST_disable(EST_Handle handle)
void HAL_setParams(HAL_Handle handle, const USER_Params *pUserParams)
static void CPU_USAGE_run(CPU_USAGE_Handle handle)
CLARKE_Handle clarkeHandle_I
the handle for the current Clarke transform
volatile bool gFlag_runCal
bool CAL_updateState(CAL_Handle handle)
HAL_AdcData_t gAdcData
Defines the ADC data.
CTRL_State_e gCtrlState
Global variable for the controller state.
float_t EST_getFe_Hz(EST_Handle handle)
void HAL_readDrvData(HAL_Handle handle, DRV_SPI_8301_Vars_t *Spi_8301_Vars)
static bool CAL_isEnabled(CAL_Handle handle)
static float_t ANGLE_getAngle_rad(ANGLE_Handle handle)
ANGLE_Handle angleHandle
the handle for the angle generator
CLARKE_Obj clarke_I
the current Clarke transform object
float_t gTorque_Nm
Global variable for the estimated torque, N*m.
static void incrBufferIndex_setup(void)
Increments the setup buffer index.
EST_InputData_t gEstInputData[NUM_EST_DATA_BUFFERS]
static void HAL_writeDacData(HAL_Handle handle, HAL_DacData_t *pDacData)
volatile bool gFlag_enableForceAngle
void EST_resetCounter_isr(EST_Handle handle)
volatile bool Graph_Flag_Enable_update
SVGEN_Handle SVGEN_init(void *pMemory, const size_t numBytes)
bool EST_updateTrajState(EST_Handle handle)
int_least32_t gCounter_enableSys
A counter that is incremented while waiting for the enable system flag to be set. ...
void EST_setFlag_bypassLockRotor(EST_Handle handle, const bool state)
SVGEN_Handle svgenHandle
the handle for the space vector generator
ANGLE_Obj angle
the angle object
static OFFSET_Handle * CAL_getOffsetHandleAddr_V(CAL_Handle handle)
int_least16_t EST_getNumIsrTicksPerTrajTick(EST_Handle handle)
void EST_run(EST_Handle handle, const MATH_vec2 *pIab_pu, const MATH_vec2 *pVab_pu, const _iq dcBus_pu, const _iq speed_ref_pu)
#define USER_NUM_PWM_TICKS_PER_ISR_TICK
DECIMATION.
int_least16_t EST_getTrajCount_isr(EST_Handle handle)
volatile bool gFlag_runIdentAndOnLine