Skip to content

Commit 3f07791

Browse files
author
camilo
committed
some fixes
1 parent fca339c commit 3f07791

File tree

2 files changed

+37
-11
lines changed

2 files changed

+37
-11
lines changed

‎src/include/pid.hpp‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,9 @@ namespace qlibs {
131131
real_t speed{ 0.25_re }; /*final controller speed*/
132132
uint32_t it{ UNDEFINED }; /*enable time*/
133133

134+
135+
static bool resetEstimation( estimationParams &p,
136+
const real_t r );
134137
static bool getTimeConstant( real_t& tau,
135138
const real_t abs_z,
136139
const real_t dt );

‎src/pid.cpp‎

Lines changed: 34 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -371,34 +371,54 @@ void pidAutoTuning::computeParamsN2( systemParams& p,
371371
const real_t rPart = -0.5_re*a1;
372372
const real_t iPart = -0.5_re*ffmath::sqrt( a2_4 - a1a1 );
373373
const real_t r = ffmath::hypot( rPart, iPart );
374+
p.tau2 = 0.0_re;
374375
p.stable = getTimeConstant( p.tau1, r, dt );
375376
}
376377
}
377378
/*============================================================================*/
379+
bool pidAutoTuning::resetEstimation( estimationParams &p,
380+
const real_t r )
381+
{
382+
bool rst = ( r < 0.0_re ) || ( r > 10000.0_re );
383+
if ( rst ) {
384+
constexpr real_t r_alfa = 0.1_re;
385+
p.P[0][0] = r_alfa; p.P[0][1] = 0.0_re; p.P[0][2] = 0.0_re; p.P[0][3] = 0.0_re;
386+
p.P[1][0] = 0.0_re; p.P[1][1] = r_alfa; p.P[1][2] = 0.0_re; p.P[1][3] = 0.0_re;
387+
p.P[2][0] = 0.0_re; p.P[2][1] = 0.0_re; p.P[2][2] = r_alfa; p.P[2][3] = 0.0_re;
388+
p.P[3][0] = 0.0_re; p.P[3][1] = 0.0_re; p.P[3][2] = 0.0_re; p.P[3][3] = r_alfa;
389+
}
390+
return rst;
391+
}
392+
/*============================================================================*/
378393
void pidAutoTuning::estimationStepN1( estimationParams &p,
379394
const real_t y )
380395
{
396+
constexpr real_t reg = 0.0001_re;
381397
const real_t f = 1.0_re/p.lambda;
382398
const real_t phi[2] = { p.u1, p.y2 };
383399
const real_t P_phi[2] = {
384400
( p.P[0][0]*phi[0] ) + ( p.P[0][1]*phi[1] ),
385401
( p.P[1][0]*phi[0] ) + ( p.P[1][1]*phi[1] ),
386402
};
387403
const real_t r = p.lambda + ( ( phi[0]*P_phi[0] ) + ( phi[1]*P_phi[1] ) );
404+
if ( resetEstimation( p, r ) ) {
405+
return;
406+
}
388407
const real_t inv_r = 1.0_re/r;
389408
const real_t L[4] ={ P_phi[0]*inv_r, P_phi[1]*inv_r };
390409
const real_t e = y - ( ( phi[0]*p.theta[0] ) + ( phi[1]*p.theta[1] ) );
391410
p.theta[0] += L[0]*e;
392411
p.theta[1] += L[1]*e;
393-
p.P[0][0] = f*( p.P[0][0] - ( L[0]*P_phi[0] ) );
412+
p.P[0][0] = f*( p.P[0][0] - ( L[0]*P_phi[0] ) ) + reg;
394413
p.P[0][1] = f*( p.P[0][1] - ( L[0]*P_phi[1] ) );
395414
p.P[1][0] = f*( p.P[1][0] - ( L[1]*P_phi[0] ) );
396-
p.P[1][1] = f*( p.P[1][1] - ( L[1]*P_phi[1] ) );
415+
p.P[1][1] = f*( p.P[1][1] - ( L[1]*P_phi[1] ) ) + reg;
397416
}
398417
/*============================================================================*/
399418
void pidAutoTuning::estimationStepN2( estimationParams &p,
400419
const real_t y )
401420
{
421+
constexpr real_t reg = 0.0001_re;
402422
const real_t f = 1.0_re/p.lambda;
403423
const real_t phi[4] = { p.u1, p.u2, p.y1, p.y2 };
404424
const real_t P_phi[4] = {
@@ -408,23 +428,26 @@ void pidAutoTuning::estimationStepN2( estimationParams &p,
408428
( p.P[0][3]*phi[0] ) + ( p.P[1][3]*phi[1] ) + ( p.P[2][3]*phi[2] ) + ( p.P[3][3]*phi[3] ),
409429
};
410430
const real_t r = p.lambda + ( ( phi[0]*P_phi[0] ) + ( phi[1]*P_phi[1] ) + ( phi[2]*P_phi[2] ) + ( phi[3]*P_phi[3]) );
431+
if ( resetEstimation( p, r ) ) {
432+
return;
433+
}
411434
const real_t inv_r = 1.0_re/r;
412435
const real_t L[4] ={ P_phi[0]*inv_r, P_phi[1]*inv_r, P_phi[2]*inv_r, P_phi[3]*inv_r };
413436
const real_t e = y - ( ( phi[0]*p.theta[0] ) + ( phi[1]*p.theta[1] ) + ( phi[2]*p.theta[2] ) + ( phi[3]*p.theta[3] ) );
414437
p.theta[0] += L[0]*e;
415438
p.theta[1] += L[1]*e;
416439
p.theta[2] += L[2]*e;
417440
p.theta[3] += L[3]*e;
418-
p.P[0][0] = f*( p.P[0][0] - ( L[0]*P_phi[0] ) );
441+
p.P[0][0] = f*( p.P[0][0] - ( L[0]*P_phi[0] ) ) + reg;
419442
p.P[0][1] = f*( p.P[0][1] - ( L[0]*P_phi[1] ) );
420443
p.P[0][2] = f*( p.P[0][2] - ( L[0]*P_phi[2] ) );
421444
p.P[0][3] = f*( p.P[0][3] - ( L[0]*P_phi[3] ) );
422-
p.P[1][1] = f*( p.P[1][1] - ( L[1]*P_phi[1] ) );
445+
p.P[1][1] = f*( p.P[1][1] - ( L[1]*P_phi[1] ) ) + reg;
423446
p.P[1][2] = f*( p.P[1][2] - ( L[1]*P_phi[2] ) );
424447
p.P[1][3] = f*( p.P[1][3] - ( L[1]*P_phi[3] ) );
425-
p.P[2][2] = f*( p.P[2][2] - ( L[2]*P_phi[2] ) );
448+
p.P[2][2] = f*( p.P[2][2] - ( L[2]*P_phi[2] ) ) + reg;
426449
p.P[2][3] = f*( p.P[2][3] - ( L[2]*P_phi[3] ) );
427-
p.P[3][3] = f*( p.P[3][3] - ( L[3]*P_phi[3] ) );
450+
p.P[3][3] = f*( p.P[3][3] - ( L[3]*P_phi[3] ) ) + reg;
428451
}
429452
/*============================================================================*/
430453
bool pidAutoTuning::step( const real_t u,
@@ -435,7 +458,7 @@ bool pidAutoTuning::step( const real_t u,
435458

436459
estimationStep( estParams, y );
437460
estParams.y2 = estParams.y1;
438-
estParams.y1 = y;
461+
estParams.y1 = -y;
439462
estParams.u2 = estParams.u1;
440463
estParams.u1 = u;
441464
computeParameters( sysParams, estParams, dt );
@@ -498,10 +521,10 @@ void pidAutoTuning::initialize( const pidGains current,
498521
estParams = {
499522
{ th0, th1, th2, th3 },
500523
{
501-
{ 1000.0_re, 0.0_re, 0.0_re, 0.0_re },
502-
{ 0.0_re, 1000.0_re, 0.0_re, 0.0_re },
503-
{ 0.0_re, 0.0_re, 1000.0_re, 0.0_re },
504-
{ 0.0_re, 0.0_re, 0.0_re, 10000.0_re },
524+
{ 1000.0_re, 0.0_re, 0.0_re, 0.0_re },
525+
{ 0.0_re, 1000.0_re, 0.0_re, 0.0_re },
526+
{ 0.0_re, 0.0_re, 1000.0_re, 0.0_re },
527+
{ 0.0_re, 0.0_re, 0.0_re, 1000.0_re },
505528
},
506529
0.9898_re,
507530
0.0_re, 0.0_re, 0.0_re, 0.0_re

0 commit comments

Comments
 (0)