fix to pid. bump to 1.2.2

Author: camilo

.vscode/settings.json

@@ -50,7 +50,18 @@
         "iomanip": "cpp",
         "xloctime": "cpp",
         "string": "cpp",
-        "numbers": "cpp"
+        "numbers": "cpp",
+        "thread": "cpp",
+        "charconv": "cpp",
+        "format": "cpp",
+        "forward_list": "cpp",
+        "locale": "cpp",
+        "mutex": "cpp",
+        "sstream": "cpp",
+        "stop_token": "cpp",
+        "vector": "cpp",
+        "xlocbuf": "cpp",
+        "xlocmes": "cpp"
     },
     "C_Cpp.errorSquiggles": "disabled"
 }

library.json

@@ -16,7 +16,7 @@
       "maintainer": true
     }
   ],
-  "version": "1.2.1",
+  "version": "1.2.2",
   "license": "MIT",
   "frameworks": "arduino",
   "platforms": "*"

library.properties

@@ -1,5 +1,5 @@
 name=qlibs
-version=1.2.1
+version=1.2.2
 license=MIT
 author=J. Camilo Gomez C. <[email protected]>
 maintainer=J. Camilo Gomez C. <[email protected]>

src/include/ltisys.hpp

@@ -24,6 +24,13 @@ namespace qlibs {
     * @{
     */
 
+    /** @cond **/
+    /*only for continuousSystem*/
+    #if !defined( LTISYS_EVAL_MODEL_CONTROLLABLE ) || !defined( LTISYS_EVAL_MODEL_OBSERVABLE )
+        #define LTISYS_EVAL_MODEL_CONTROLLABLE
+    #endif
+    /** @endcond **/
+
     /**
     * @brief All the possible natures of a LTI system.
     */
@@ -55,6 +62,7 @@ namespace qlibs {
         real_t den[ order+1 ];
         continuousStates<order> states = {};
         continuousTF( const real_t ( &numerator )[ order + 1 ], const real_t ( &denominator )[ order + 1 ] ) {
+            static_assert( order >= 1 , "Order should be greater than 0" );
             for ( size_t i = 0; i <= order; ++i ) {
                 num[ i ] = numerator[ i ];
                 den[ i ] = denominator[ i ];

src/ltisys.cpp

@@ -168,11 +168,11 @@ bool continuousSystem::setup( real_t *num,
 {
     bool retValue = false;
 
-    if ( ( nullptr != num ) && ( nullptr != den ) && ( nullptr != x ) ) {
+    if ( ( nullptr != num ) && ( nullptr != den ) && ( nullptr != x ) && ( nD >= 1U ) ) {
         b = &num[ 1 ];
         n = nD;
         nb = n;
-        na = nD + 1;
+        na = nD + 1U;
         xc = x;
         dt = dT;
         a = &den[ 1 ];
@@ -206,6 +206,7 @@ bool continuousSystem::setInitStates( const real_t *xi )
     return retValue;
 }
 /*============================================================================*/
+#if defined( LTISYS_EVAL_MODEL_CONTROLLABLE )
 real_t continuousSystem::update( const real_t u )
 {
     real_t y = 0.0_re;
@@ -214,7 +215,6 @@ real_t continuousSystem::update( const real_t u )
     if ( 1U == n ) {
         dx0 = ( u - ( xc[ 0 ]*a[ 0 ] ) );
         (void)xc[ 0 ].integrate( dx0, dt );
-        y = ( b[ 0 ] - ( a[ 0 ]*b0 ) )*xc[ 0 ];
     }
     else {
         /*compute states of the system by using the controllable canonical form*/
@@ -229,11 +229,42 @@ real_t continuousSystem::update( const real_t u )
         dx0 = u - ( dx0 + ( a[ 0 ]*xc[ 0 ] ) );
         (void)xc[ 0 ].integrate( dx0, dt ); /*integrate to get the first state*/
         /*compute the remaining part of the output*/
-        y += ( b[ 0 ] - ( a[ 0 ]*b0 ) )*xc[ 0 ];
     }
+    /*get the output of the system*/
+    y += ( b[ 0 ] - ( a[ 0 ]*b0 ) )*xc[ 0 ];
 
     return y;
 }
+#elif defined( LTISYS_EVAL_MODEL_OBSERVABLE )
+/*============================================================================*/
+real_t continuousSystem::update( const real_t u )
+{
+    real_t y = 0.0_re;
+    const real_t x0 = xc[ 0 ](); /*save first state for computation*/
+    const size_t N = n - 1U;
+    /*compute the last derivative dx_{n}*/
+    const real_t dxn = ( -a[ N ]*x0 ) + ( ( b[ N ] - a[ N ]*b0 )*u );
+
+    if ( 1U == n ) {
+        xc[ 0 ].integrate( dxn, dt );
+    }
+    else {
+        /*compute states of the system by using the observable canonical form*/
+        for ( size_t i = 0U; i < N ; ++i ) {
+            const real_t dxi = ( -a[ i ]*x0 ) + xc[ i + 1U ] + ( ( b[ i ] - a[ i ]*b0 )*u );
+            (void)xc[ i ].integrate( dxi , dt );
+        }
+        /*integrate the remaining last state*/
+        (void)xc[ N ].integrate( dxn, dt );
+    }
+    /*get the output of the system*/
+    y = xc[ 0 ] + ( b0*u );
+
+    return y;
+}
+#else
+    #error "LTISYS evaluation mode not defined"
+#endif
 /*============================================================================*/
 bool continuousSystem::setIntegrationMethod( integrationMethod m )
 {

src/pid.cpp

@@ -1,6 +1,6 @@
 #include <include/pid.hpp>
 #include <include/ffmath.hpp>
-
+#include <iostream>
 using namespace qlibs;
 
 
@@ -265,8 +265,8 @@ real_t pidController::control( const real_t w,
         if ( ffmath::absf( e ) <= epsilon ) {
             e = 0.0_re;
         }
-        ie = c_state.integrate( e + u1, dt, false );
-        de = c_state.derive( ( c*w ) - y , dt );
+        de = c_state.derive( ( c*w ) - y , dt, false );
+        ie = c_state.integrate( e + u1 , dt );
         D = de + beta*( D - de ); /*derivative filtering*/
         v  = ( kc*( ( b*w ) - y ) ) + ( ki*ie ) + ( kd*D ); /*compute PID action*/
         if ( nullptr != yr ) {
@@ -285,8 +285,7 @@ real_t pidController::control( const real_t w,
         sw = ( pidMode::PID_AUTOMATIC == mode ) ? v : m;
         uSat = saturate( sw, sat_Min, sat_Max );
         u = uSat; /*output saturated*/
-        /*anti-windup feedback*/
-        u1 = kw*( u - v );
+        u1 = kw*( u - v ); /*anti-windup feedback*/
         if ( nullptr != adapt ) {
             adaptGains( u, y );
         }

src/qlibs.h

@@ -1,7 +1,7 @@
 /*!
  * @file qlibs.h
  * @author J. Camilo Gomez C.
- * @version 1.1.9
+ * @version 1.2.2
  * @note This file is part of the qlibs++ distribution.
  * @brief Global inclusion header
  **/
@@ -41,8 +41,8 @@ This file is part of the QuarkTS++ OS distribution.
 #ifndef QLIBS_CPP_H
 #define QLIBS_CPP_H
 
-#define QLIBS_CPP_VERSION         "1.2.1"
-#define QLIBS_CPP_VERNUM          ( 121U )
+#define QLIBS_CPP_VERSION         "1.2.2"
+#define QLIBS_CPP_VERNUM          ( 122U )
 #define QLIBS_CPP_CAPTION         "qLibs++" QLIBS_CPP_VERSION
 
 #include <include/qlibs_types.hpp>