fix to numa & pid

kmilo17pet · Feb 13, 2024 · e2116fb · e2116fb
1 parent 76d51e6
commit e2116fb
Show file tree

Hide file tree

Showing 6 changed files with 29 additions and 18 deletions.
diff --git a/library.json b/library.json
@@ -16,7 +16,7 @@
       "maintainer": true
     }
   ],
-  "version": "1.2.0",
+  "version": "1.2.1",
   "license": "MIT",
   "frameworks": "arduino",
   "platforms": "*"

diff --git a/library.properties b/library.properties
@@ -1,5 +1,5 @@
 name=qlibs
-version=1.2.0
+version=1.2.1
 license=MIT
 author=J. Camilo Gomez C. <kmilo17pet@gmail.com>
 maintainer=J. Camilo Gomez C. <kmilo17pet@gmail.com>

diff --git a/src/include/numa.hpp b/src/include/numa.hpp
@@ -89,19 +89,23 @@ namespace qlibs {
             * @brief Perform a numerical integration step.
             * @param[in] s The input signal
             * @param[in] dt The time-step given in seconds.
+            * @param[in] bUpdate Flag to update the states ( @c true by default).
             * @return The current value of the integration step.
             */
             real_t integrate( const real_t s,
-                              const real_t dt ) noexcept;
+                              const real_t dt,
+                              const bool bUpdate = true ) noexcept;
 
              /**
             * @brief Perform a numerical derivation step by using the delta rule.
             * @param[in] s The input signal
             * @param[in] dt The time-step given in seconds.
+            * @param[in] bUpdate Flag to update the states ( @c true by default).
             * @return The current value of the derivation step.
             */
             real_t derive( const real_t s,
-                           const real_t dt ) noexcept;
+                           const real_t dt,
+                           const bool bUpdate = true ) noexcept;
 
             /**
             * @brief Set integration method .

diff --git a/src/numa.cpp b/src/numa.cpp
@@ -4,16 +4,17 @@ using namespace qlibs;
 
 /*===========================================================================*/
 void nState::init( const real_t x0,
-                  const real_t sn_1,
-                  const real_t sn_2 ) noexcept
+                   const real_t sn_1,
+                   const real_t sn_2 ) noexcept
 {
     x[ 0 ] = x0;
     x[ 1 ] = sn_1;
     x[ 2 ] = sn_2;
 }
 /*===========================================================================*/
 real_t nState::integrate( const real_t s,
-                         const real_t dt ) noexcept
+                          const real_t dt,
+                          const bool bUpdate ) noexcept
 {
     switch( iMethod ) {
         case INTEGRATION_RECTANGULAR:
@@ -31,30 +32,36 @@ real_t nState::integrate( const real_t s,
         default:
             break;
     }
-    update( s );
+    if ( bUpdate ) {
+        update( s );
+    }
 
     return x[ 0 ];
 }
 /*===========================================================================*/
 real_t nState::derive( const real_t s,
-                       const real_t dt ) noexcept
+                       const real_t dt,
+                       const bool bUpdate ) noexcept
 {
+    float ds = 0.0F;
+
     switch( dMethod ) {
         case DERIVATION_2POINTS:
-            x[ 0 ] = ( s - x[ 1 ] )/dt;
+            ds = ( s - x[ 1 ] )/dt;
             break;
         case DERIVATION_BACKWARD:
-            x[ 0 ] = ( ( 3.0_re*s ) - ( 4.0_re*x[ 1 ] ) + x[ 2 ] )/( 2.0_re*dt );
+            ds = ( ( 3.0_re*s ) - ( 4.0_re*x[ 1 ] ) + x[ 2 ] )/( 2.0_re*dt );
             break;
         case DERIVATION_FORWARD:
-            x[ 0 ] = ( ( 4.0_re*x[ 1 ] ) - ( 3.0_re*x[ 2 ] ) - s )/( 2.0_re*dt );
+            ds = ( ( 4.0_re*x[ 1 ] ) - ( 3.0_re*x[ 2 ] ) - s )/( 2.0_re*dt );
             break;
         default:
             break;
     }
+    if ( bUpdate ) {
+        update( s );
+    }
 
-    update( s );
-
-    return x[ 0 ];
+    return ds;
 }
 /*===========================================================================*/
diff --git a/src/pid.cpp b/src/pid.cpp
@@ -265,7 +265,7 @@ real_t pidController::control( const real_t w,
         if ( ffmath::absf( e ) <= epsilon ) {
             e = 0.0_re;
         }
-        ie = c_state.integrate( e + u1, dt );
+        ie = c_state.integrate( e + u1, dt, false );
         de = c_state.derive( ( c*w ) - y , dt );
         D = de + beta*( D - de ); /*derivative filtering*/
         v  = ( kc*( ( b*w ) - y ) ) + ( ki*ie ) + ( kd*D ); /*compute PID action*/

diff --git a/src/qlibs.h b/src/qlibs.h
@@ -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.0"
-#define QLIBS_CPP_VERNUM          ( 120U )
+#define QLIBS_CPP_VERSION         "1.2.1"
+#define QLIBS_CPP_VERNUM          ( 121U )
 #define QLIBS_CPP_CAPTION         "qLibs++" QLIBS_CPP_VERSION
 
 #include <include/qlibs_types.hpp>