![Logo](ti_logo.svg) |
AM263x Motor Control SDK
09.02.00
|
|
Go to the documentation of this file.
48 #include "../dcl_common.h"
55 typedef struct dcl_pid64_sps {
68 #define PIDF64_SPS_DEFAULTS { 1.0L, 0.0L, 0.0L, 1.0L, 1.0L, 0.0L, 1.0L, -1.0L }
96 #define PIDF64_DEFAULTS { 1.0L, 0.0L, 0.0L, 1.0L, 1.0L, 0.0L, \
97 1.0L, -1.0L, 0.0L, 0.0L, 0.0L, 1.0L, \
98 &(DCL_PIDF64_SPS)PIDF64_SPS_DEFAULTS, &(DCL_CSSF64)DCL_CSSF64_DEFAULTS }
108 #define PIDF64_INT_DEFAULTS .d2=0.0L, .d3=0.0L, .i10=0.0L, .i14=1.0L, \
109 .sps=&(DCL_PIDF64_SPS)PI_SPS_DEFAULTS, .css=&(DCL_CSSF64)DCL_CSS_DEFAULTS
116 #define DCL_initPIDF64() &(DCL_PIDF64)PIDF64_DEFAULTS
124 #define DCL_initPIDF64asParam(kp,ki,kd,kr,_c1,_c2,umax,umin) &(DCL_PIDF64){ .Kp=kp, .Ki=ki, .Kd=kd, .Kr=kr, \
125 .c1=_c1, .c2=_c2, .Umax=umax, .Umin=umin, PIDF64_INT_DEFAULTS }
137 #define DCL_initPIDF64asSPS(pid_ptr,sps_ptr) \
139 DCL_PIDF64* new_pid = (pid_ptr) ? pid_ptr : DCL_initPID(); \
140 DCL_PIDF64_SPS* new_sps = (sps_ptr) ? sps_ptr : &(DCL_PIDF64_SPS)PID_SPS_DEFAULTS; \
143 *new_pi = (DCL_PID){ (new_sps)->Kp, (new_sps)->Ki, (new_sps)->Kd,(new_sps)->Kr,\
144 (new_sps)->c1, (new_sps)->c2, (new_sps)->Umax, (new_sps)->Umin, 0.0L, 0.0L, \
145 0.0L, 1.0L,(DCL_PIDF64_SPS*)new_sps, &(DCL_CSS)DCL_CSS_DEFAULTS }; \
159 pid->d2 = pid->d3 = pid->i10 = 0.0L;
172 #ifdef DCL_ERROR_HANDLING_ENABLED
173 float64_t tau = (2.0L - pid->sps->c1 * pid->css->T) / (2.0L * pid->sps->c1);
174 float64_t ec2 = pid->sps->c1 * (pid->css->T - 2.0L * tau) / 2.0L;
179 err_code |= ((pid->sps->Kp > 0.0L) && (pid->sps->Ki > 0.0L) && (pid->sps->Kd > 0.0L) && (pid->sps->Kr > 0.0L)) ?
dcl_none :
dcl_param_range_err ;
188 pid->Kp = pid->sps->Kp;
189 pid->Ki = pid->sps->Ki;
190 pid->Kd = pid->sps->Kd;
191 pid->Kr = pid->sps->Kr;
192 pid->c1 = pid->sps->c1;
193 pid->c2 = pid->sps->c2;
194 pid->Umax = pid->sps->Umax;
195 pid->Umin = pid->sps->Umin;
207 #ifdef DCL_ERROR_HANDLING_ENABLED
208 float64_t tau = (2.0L - pid->sps->c1 * pid->css->T) / (2.0L * pid->sps->c1);
209 float64_t ec2 = pid->sps->c1 * (pid->css->T - 2.0L * tau) / 2.0L;
214 err_code |= ((pid->sps->Kp > 0.0L) && (pid->sps->Ki > 0.0L) && (pid->sps->Kd > 0.0L) && (pid->sps->Kr > 0.0L)) ?
dcl_none :
dcl_param_range_err ;
225 pid->Kp = pid->sps->Kp;
226 pid->Ki = pid->sps->Ki;
227 pid->Kd = pid->sps->Kd;
228 pid->Kr = pid->sps->Kr;
229 pid->c1 = pid->sps->c1;
230 pid->c2 = pid->sps->c2;
231 pid->Umax = pid->sps->Umax;
232 pid->Umin = pid->sps->Umin;
267 #ifdef DCL_ERROR_HANDLING_ENABLED
280 pid->sps->c1 = 2.0L / (T + (2.0L * tau));
281 pid->sps->c2 = (T - (2.0L * tau)) / (T + (2.0L * tau));
296 #ifdef DCL_ERROR_HANDLING_ENABLED
308 pid->c1 = 2.0L / (T + (2.0L * tau));
309 pid->c2 = (T - (2.0L * tau)) / (T + (2.0L * tau));
321 float64_t tau = ((2.0L - pid->c1 * pid->css->T) / (2.0L * pid->c1));
337 #ifdef DCL_ERROR_HANDLING_ENABLED
356 float64_t a0p = 4.0L + (alpha1 * 2.0L * T) + (alpha0 * T * T);
357 float64_t b0 = zpk->
K * (4.0L + (beta1 * 2.0L * T) + (beta0 * T *T)) / a0p;
358 float64_t b1 = zpk->
K * (-8.0L + (2.0f * beta0 * T * T)) / a0p;
359 float64_t b2 = zpk->
K * (4.0L - (beta1 * 2.0L * T) + (beta0 * T * T)) / a0p;
360 float64_t a2 = (4.0L - (alpha1 * 2.0L * T) + (alpha0 * T * T)) / a0p;
362 float64_t tau = (T / 2.0L) * (1.0L - c2) / (1.0L + c2);
363 pid->sps->c1 = 2.0L / (T + 2.0L * tau);
368 #ifdef DCL_ERROR_HANDLING_ENABLED
379 float64_t k1 = ((c2 * b0) - b1 - ((2.0L + c2) * b2)) / det;
380 float64_t k2 = (c2 + 1.0L) * (b0 + b1 + b2) / det;
381 float64_t k3 = ((c2 * c2 * b0) - (c2 * b1) + b2) / det;
383 pid->sps->Ki = k2 / k1;
384 pid->sps->Kd = k3 / (k1 * pid->sps->c1);
386 #ifdef DCL_TESTPOINTS_ENABLED
403 #ifdef DCL_ERROR_HANDLING_ENABLED
422 float64_t a0p = 4.0L + (alpha1 * 2.0L * T) + (alpha0 * T * T);
423 float64_t b0 = zpk->
K * (4.0L + (beta1 * 2.0L * T) + (beta0 * T * T)) / a0p;
424 float64_t b1 = zpk->
K * (-8.0L + (2.0L * beta0 * T * T)) / a0p;
425 float64_t b2 = zpk->
K * (4.0L - (beta1 * 2.0L * T) + (beta0 * T * T)) / a0p;
426 float64_t a2 = (4.0L - (alpha1 * 2.0L * T) + (alpha0 * T * T)) / a0p;
428 float64_t tau = (T / 2.0L) * (1.0L - c2) / (1.0L + c2);
429 pid->sps->c1 = 2.0L / (T + 2.0L * tau);
434 #ifdef DCL_ERROR_HANDLING_ENABLED
444 pid->sps->Kp = ((c2 * b0) - b1 - ((2.0L + c2) * b2)) / det;
445 pid->sps->Ki = (c2 + 1.0L) * (b0 + b1 + b2) / det;
446 pid->sps->Kd = ((c2 * c2 * b0) - (c2 * b1) + b2) / (det * pid->sps->c1);
448 #ifdef DCL_TESTPOINTS_ENABLED
467 v5 = (pid->Kr * rk) - yk;
468 v8 = ((rk - yk) * pid->Ki * pid->Kp * pid->i14) + pid->i10;
470 v1 = yk * pid->Kd * pid->c1;
471 v4 = v1 - pid->d2 - pid->d3;
473 pid->d3 = v4 * pid->c2;
474 v9 = ((v5 - v4) * pid->Kp) + v8;
476 v12 = (v10 == v9) ? 1.0L : 0.0L;
479 #ifdef DCL_TESTPOINTS_ENABLED
497 float64_t v1, v4, v5, v6, v8, v9, v10, v12;
501 v8 = v5 * pid->Ki * pid->i14 + pid->i10;
503 v1 = v5 * pid->Kd * pid->c1;
504 v4 = v1 - pid->d2 - pid->d3;
506 pid->d3 = v4 * pid->c2;
509 v12 = (v10 == v9) ? 1.0L : 0.0L;
512 #ifdef DCL_TESTPOINTS_ENABLED
525 #endif // _DCL_PIDF64_H_
_DCL_CODE_ACCESS void DCL_updatePIDF64NoCheck(DCL_PIDF64 *pid)
Updates PID parameter from its SPS parameter with interrupt protection.
Definition: dcl_pidf64.h:204
float64_t c1
D path low-pass filter coefficient 1, default is 1.
Definition: dcl_pidf64.h:78
_DCL_CODE_ACCESS void DCL_forceUpdatePIDF64(DCL_PIDF64 *pid)
Loads PIDF64 tuning parameter from its SPS parameter without interrupt protection.
Definition: dcl_pidf64.h:169
#define DCL_isValue(x, y)
Determines numerical proximity to specified value.
Definition: dcl_macro.h:72
float64_t Umax
Upper saturation limit.
Definition: dcl_pidf64.h:62
float64_t c1
D path low-pass filter coefficient 1, default is 1.
Definition: dcl_pidf64.h:60
_DCL_CODE_ACCESS void DCL_loadSeriesPIDF64asZPK(DCL_PIDF64 *pid, DCL_ZPK3F64 *zpk)
Configures a series PID controller parameter in ZPK form.
Definition: dcl_pidf64.h:334
float32_t K
Real gain.
Definition: dcl_zpk3.h:69
_DCL_CODE_ACCESS void DCL_setPIDF64filterBW(DCL_PIDF64 *pid, float64_t fc)
Loads the derivative path filter shadow coefficients.
Definition: dcl_pidf64.h:264
float64_t Kp
Proportional gain.
Definition: dcl_pidf64.h:74
_DCL_CODE_ACCESS void DCL_resetPIDF64(DCL_PIDF64 *pid)
Resets PID64 internal storage data with interrupt protection.
Definition: dcl_pidf64.h:155
float complex z2
Complex zeros 2.
Definition: dcl_zpk3.h:64
float complex p2
Complex poles 2.
Definition: dcl_zpk3.h:67
float64_t d3
D path low-pass filter storage (c2)
Definition: dcl_pidf64.h:85
@ dcl_none
No error.
Definition: dcl_error.h:57
_DCL_CODE_ACCESS float64_t DCL_getPIDF64filterBW(DCL_PIDF64 *pid)
Returns the active derivative path filter bandwidth in Hz.
Definition: dcl_pidf64.h:319
_DCL_CODE_ACCESS bool DCL_updatePIDF64(DCL_PIDF64 *pid)
A conditional update based on the update flag. If the update status is set, the function will update ...
Definition: dcl_pidf64.h:245
float64_t Umin
Lower saturation limit.
Definition: dcl_pidf64.h:81
float64_t Kd
Derivative gain.
Definition: dcl_pidf64.h:76
#define DCL_runErrorHandler(ptr)
Prototype for basic error handler.
Definition: dcl_error.h:108
float64_t Umax
Upper saturation limit.
Definition: dcl_pidf64.h:80
_DCL_VOLATILE struct dcl_pidf64 DCL_PIDF64
float64_t Kd
Derivative gain.
Definition: dcl_pidf64.h:58
_DCL_CRIT_ACCESS float64_t DCL_runPIDF64Series(DCL_PIDF64 *pid, float64_t rk, float64_t yk, float64_t lk)
Executes an ideal form PID64 controller.
Definition: dcl_pidf64.h:463
#define DCL_getUpdateStatus(p)
Determine whether a parameter update-in-progress flag is set.
Definition: dcl_css.h:122
float64_t Kp
Proportional gain.
Definition: dcl_pidf64.h:56
#define DCL_clearUpdateStatus(p)
Definition: dcl_css.h:116
float64_t Umin
Lower saturation limit.
Definition: dcl_pidf64.h:63
#define DCL_c2LimitF64
Definition: dcl_macro.h:94
double complex p1
Complex poles 1.
Definition: dcl_zpk3.h:85
#define _DCL_CODE_ACCESS
Defines the scope of dcl functions.
Definition: dcl_common.h:63
float complex p1
Complex poles 1.
Definition: dcl_zpk3.h:66
float64_t c2
D path low-pass filter coefficient 2, default is 0.
Definition: dcl_pidf64.h:79
float64_t Kr
Set point weight, default is 1.
Definition: dcl_pidf64.h:77
_DCL_CODE_ACCESS void DCL_setActivePIDF64filterBW(DCL_PIDF64 *pid, float64_t fc, float64_t T)
Loads the PID64 derivative path filter active coefficients.
Definition: dcl_pidf64.h:293
float64_t d2
D path low-pass filter storage (Kd * c1)
Definition: dcl_pidf64.h:84
_DCL_CODE_ACCESS void DCL_loadParallelPIDF64asZPK(DCL_PIDF64 *pid, DCL_ZPK3 *zpk)
Configures a parallel PID controller in ZPK form.
Definition: dcl_pidf64.h:401
uint32_t dcl_interrupt_t
Definition: dcl_common.h:107
float64_t Ki
Integral gain.
Definition: dcl_pidf64.h:57
Defines DCL_PIDF64 shadow PID64 controller structure used for updating controller parameter.
Definition: dcl_pidf64.h:55
float64_t i10
I path feedback storage.
Definition: dcl_pidf64.h:86
#define DCL_isZero(x)
Determines floating point numerical proximity to zero.
Definition: dcl_macro.h:77
@ dcl_param_range_err
Parameter range exceeded.
Definition: dcl_error.h:58
DCL_PIDF64_SPS * sps
updates controller parameter
Definition: dcl_pidf64.h:90
#define CONST_PI_F64
Definition: dcl_macro.h:57
Defines the 64bit CSS structure.
Definition: dcl_css.h:72
double complex p2
Complex poles 2.
Definition: dcl_zpk3.h:86
#define DCL_runSat(data, Umax, Umin)
Macro to saturate a control variable but does not change the data itself unlike runClamp()
Definition: dcl_clamp.h:89
float64_t K
Real gain.
Definition: dcl_zpk3.h:88
DCL_PIDF64 object for storing 64bit PID specific parameters.
Definition: dcl_pidf64.h:72
float64_t i14
Saturation multiplier, ranges between 1*lk ~ 0, where 0 means fully saturated.
Definition: dcl_pidf64.h:87
#define DCL_restoreInts(v)
Definition: dcl_common.h:106
@ dcl_param_invalid_err
Parameter not valid.
Definition: dcl_error.h:59
_DCL_CRIT_ACCESS float64_t DCL_runPIDF64Parallel(DCL_PIDF64 *pid, float64_t rk, float64_t yk, float64_t lk)
Executes an parallel form PID64 controller.
Definition: dcl_pidf64.h:495
Defines the DCL_ZPK3 structure.
Definition: dcl_zpk3.h:62
#define _DCL_VOLATILE
Defines volatile for DCL strctures.
Definition: dcl_common.h:79
double float64_t
Definition: dcl_common.h:59
Defines the 64bit ZPK3 structure.
Definition: dcl_zpk3.h:81
float64_t Kr
Set point weight, default is 1.
Definition: dcl_pidf64.h:59
DCL_CSSF64 * css
configuration & debugging
Definition: dcl_pidf64.h:91
#define DCL_getErrorInfo(ptr)
Macro to store error info in CSS.
Definition: dcl_error.h:98
#define DCL_setError(ptr, code)
Macro to set error code in CSS.
Definition: dcl_error.h:80
#define _DCL_CRIT_ACCESS
Defines the scope of critical dcl functions.
Definition: dcl_common.h:70
float64_t Ki
Integral gain.
Definition: dcl_pidf64.h:75
float64_t c2
D path low-pass filter coefficient 2, default is 0.
Definition: dcl_pidf64.h:61
double complex z2
Complex zeros 2.
Definition: dcl_zpk3.h:83
double complex z1
Complex zeros 1.
Definition: dcl_zpk3.h:82
float complex z1
Complex zeros 1.
Definition: dcl_zpk3.h:63
#define DCL_disableInts()
Define enable and disable interrupt operations.
Definition: dcl_common.h:105