pid refactor, added transportDelay to ltisys

check/qlibs_cpp_test.cpp

@@ -182,6 +182,7 @@ void test_fis( void )
     };
     real_t rulesStrength[ 3 ];
 
+
     tipper.setup( fis::Mamdani, tipper_inputs, tipper_outputs, MFin, MFout, rules, rulesStrength );
     tipper.setupInput( service, 0.0f, 1.0f );
     tipper.setupInput( food, 0.0f, 10.0f );
@@ -259,15 +260,22 @@ void test_crc( void )
 
 void test_ltisys( void )
 {
+    constexpr real_t dt = 0.05f;
     cout << "LTISYS TEST"<< endl;
     cout << "continuousSystem"<< endl;
     continuousTF<3> ctf= {
         { 0.0f, 2.0f, 3.0f, 6.0f },
         { 1.0f, 6.0f, 11.0f, 16.0f },
     };
-    continuousSystem gc( ctf, 0.01f );
-    for ( int i = 0; i < 1000; i++ ) {
-        cout << gc.excite( 1.0f ) << endl;
+    transportDelay<delayFromTime(1.3f, dt )> delay;
+
+
+    continuousSystem gc( ctf, dt );
+
+    real_t t = 0.0;
+    for ( int i = 0; i < 100; i++ ) {
+        cout << t << "    "<< delay( gc.excite( 1.0f ) ) << endl;
+        t += dt;
     }
 
     cout << "discreteSystem"<< endl;
@@ -279,8 +287,10 @@ void test_ltisys( void )
     real_t den[] = { 1.0f, -0.85f, 0.02f };
     discreteStates<3> xd= { 0.0f, 0.0f, 0.0f };
     discreteSystem gd( num, den, xd );
+
     for ( int i = 0; i < 20; i++ ) {
-        cout << gd.excite( 1.0f ) << endl;
+        cout <<  gd.excite( 1.0f ) << endl;
+
     }
 
 }
@@ -526,8 +536,6 @@ bool operator<(const thing& lhs, const thing& rhs) {
 
 int main()
 {
-
-
     thing things[] = {
         {1,0},
         {-2,5},
@@ -557,11 +565,26 @@ int main()
     for ( auto i : things ) {
         std::cout <<  i.a << " , "<< i.b << std::endl;
     }
+    std::cout << "fill "<< std::endl;
+    algorithm::fill( things, {0 ,0}, 1,2 );
+    for ( auto i : things ) {
+        std::cout <<  i.a << " , "<< i.b << std::endl;
+    }
 
-    return 0;
+    std::cout << "foreach "<< std::endl;
+    algorithm::for_each( things, +[]( thing& t) { t.a++; } );
+
+    for ( auto i : things ) {
+        std::cout <<  i.a << " , "<< i.b << std::endl;
+    }
+
+    algorithm::any_of( things, +[](const thing x ) -> bool {
+        return x.a < 3;
+    } );
+
+    //return 0;
     test_crc();
     test_fp16();
-    test_ltisys();
     test_tdl();
 
     test_mat();
@@ -571,5 +594,7 @@ int main()
     test_fis3();
     test_ffmath();
 
+test_ltisys();
+
     return 0;
 }

check/settings/sa_check.dnx

@@ -20,6 +20,15 @@
         <LogFile>_ "$PROJ_DIR$\LogFile1.log"</LogFile>
         <Category2>_ 2</Category2>
     </LogFile>
+    <TerminalIO>
+        <InputSource>1</InputSource>
+        <InputMode2>10</InputMode2>
+        <Filename>$PROJ_DIR$\TermIOInput.txt</Filename>
+        <InputEcho>1</InputEcho>
+        <ShowReset>0</ShowReset>
+        <InputEncodingICU>0</InputEncodingICU>
+        <OutputEncodingICU>0</OutputEncodingICU>
+    </TerminalIO>
     <InterruptLog>
         <GraphEnabled>0</GraphEnabled>
         <LogEnabled>0</LogEnabled>
@@ -35,15 +44,6 @@
         <SumEnabled>0</SumEnabled>
         <ShowTimeSum>1</ShowTimeSum>
     </DataLog>
-    <TerminalIO>
-        <InputSource>1</InputSource>
-        <InputMode2>10</InputMode2>
-        <Filename>$PROJ_DIR$\TermIOInput.txt</Filename>
-        <InputEcho>1</InputEcho>
-        <ShowReset>0</ShowReset>
-        <InputEncodingICU>0</InputEncodingICU>
-        <OutputEncodingICU>0</OutputEncodingICU>
-    </TerminalIO>
     <TermIOLog>
         <LoggingEnabled>_ 0</LoggingEnabled>
         <LogFile>_ ""</LogFile>

doc/qpid.dox

@@ -57,7 +57,7 @@
 * As shown above, the derivative term \f$f_{d}(t)\f$ is the output of a
 * low-pass filter that takes the raw derivative as input.
 *
-* <center> \f$ f_{d}(t) = \text{LPF}[ \frac{de_{c}(t)}{dt} ]\f$  </center>
+* <center> \f$ f_{d}(t) = \text{LPF}[ \frac{de_{c}(t)}{dt} ] = de_c(t) + e^{-dt/T_f} \left [ f_d(t-1) - de_c(t)\right  ]\f$  </center>
 *
 * and \f$ c(t)\f$, the saturation feedback for the anti-windup, with \f$K_{w}\f$
 * as the adjustment parameter.
@@ -140,7 +140,7 @@
 *      real_t processMeasurement;
 *      real_t controlOutput;
 *      for ( ;; ) {
-*          processMeasurement = BSP_ScaletoSpeed ( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) );
+*          processMeasurement = BSP_ScaletoSpeed( BSP_AnalogRead( BSP_AI_SPEED_CHANNEL ) );
 *          controlOutput = controller->control( SetPoint, processMeasurement );
 *          BSP_PWMSet( BSP_AO_SPEED_PWM_CHANNEL, BSP_ScaletoPWM( controlOutput ) );
 *          vTaskDelay( dt / portTICK_RATE_MS) ;

keywords.txt

@@ -37,6 +37,8 @@ continuousStates	KEYWORD1
 discreteStates	KEYWORD1
 continuousTF	KEYWORD1
 discreteTF	KEYWORD1
+transportDelay	KEYWORD1
+discreteDelay	KEYWORD1
 ltisys	KEYWORD1
 discreteSystem	KEYWORD1
 continuousSystem	KEYWORD1

library.json

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

library.properties

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

src/CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required( VERSION 3.2 )
 project( qlibs-cpp
-         VERSION 1.2.7
+         VERSION 1.2.8
          DESCRIPTION "A collection of useful C++ libraries for embedded systems"
          LANGUAGES CXX )
 

src/include/ltisys.hpp

@@ -55,13 +55,22 @@ namespace qlibs {
     /**
     * @brief Continuous transfer function for easy LTI system definition
     * @note Initial conditions are zero by default.
+    * @tparam order The continuous system order
     */
     template<size_t order>
     struct continuousTF {
         real_t num[ order+1 ];
         real_t den[ order+1 ];
         continuousStates<order> states = {};
-        continuousTF( const real_t ( &numerator )[ order + 1 ], const real_t ( &denominator )[ order + 1 ] ) {
+        /**
+        * @brief Constructor for the continuousTF class
+        * @param[in] numerator An array of size <order> with the coefficients
+        * of the numerator in descending powers of s.
+        * @param[in] denominator An array of size <order> with the coefficients
+        * of the denominator in descending powers of s.
+        */
+        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 ];
@@ -70,15 +79,78 @@ namespace qlibs {
         }
     };
 
+
+    constexpr size_t delayFromTime( const real_t Time, const real_t dt )
+    {
+        return static_cast<size_t>( ( Time/dt ) + 0.5_re );
+    }
+
+    /**
+    * @brief Delays the input by a specified amount of time. You can use this
+    * class to simulate a time delay.
+    * @tparam numberOfDelay The number of discrete delays to be used. Use the
+    * delayFromTime() function to determine the number of discrete delays required
+    * for a specified amount of time.
+    * Example :
+    * @code{.cpp}
+    * transportDelay<delayFromTime( 2.5f, dt )> myDelay1;
+    * @endcode
+    */
+    template<size_t numberOfDelays>
+    class transportDelay {
+        private:
+            real_t buf[ numberOfDelays + 1 ];
+            tdl delay;
+        public:
+            /**
+            * @brief Constructor for the transportDelay class
+            * @param[in] initValue The output generated by the block between the
+            * start of the simulation and the Time delay.
+            */
+            transportDelay( const real_t initValue = 0.0_re )
+            {
+                static_assert( numberOfDelays >= 1 , "Delay taps should be greater than 0" );
+                delay.setup( buf, initValue);
+            }
+            /**
+            * @brief Delays the input by a specified amount of time.
+            * @param[in] xInput The signal to be delayed.
+            * @return The delayed input signal
+            */
+            real_t operator()( const real_t xInput ) noexcept
+            {
+                delay.insertSample( xInput );
+                return delay.getOldest();
+            }
+    };
+
+    /**
+    * @brief Delays the input by a specified amount of samples. You can use this
+    * class to simulate a discrete time delay.
+    * @tparam Delay The number of samples to delay the signal.
+    */
+    template<size_t delay>
+    using discreteDelay = transportDelay<delay>;
+
+
     /**
     * @brief Discrete transfer function for easy LTI system definition
     * @note Initial conditions are zero by default.
+    * @tparam NB Order of the numerator of the transfer function
+    * @tparam NA Order of the denominator of the transfer function
     */
     template<size_t NB, size_t NA>
     struct discreteTF {
         real_t num[ NB ];
         real_t den[ NA ];
         discreteStates<(NA>NB)? NA:NB> states = {};
+        /**
+        * @brief Constructor for the discreteTF class
+        * @param[in] numerator An array of size <NB> with the coefficients
+        * of the numerator in descending powers of z.
+        * @param[in] denominator An array of size <NA> with the coefficients
+        * of the denominator in descending powers of z.
+        */
         discreteTF( const real_t ( &numerator )[ NB ], const real_t ( &denominator)[ NA ] ) {
             for ( size_t i = 0; i < NB; ++i ) {
                 num[ i ] = numerator[ i ];

src/include/pid.hpp

@@ -75,29 +75,58 @@ namespace qlibs {
             static const uint32_t UNDEFINED;
         protected:
             /*! @cond  */
-            real_t p00{ 1.0_re };
-            real_t p01{ 0.0_re };
-            real_t p10{ 0.0_re };
-            real_t p11{ 1.0_re };   /*covariance values*/
-            real_t b1{ 0.1_re };
-            real_t a1{ 0.9_re };   /*estimation  values*/
-            real_t uk{ 0.0_re };
-            real_t yk{ 0.0_re };      /*process I/O measurements*/
-            real_t l{ 0.9898_re }; /*memory factor [ 0.9 < l < 1 ]*/
-            real_t il{ 1.0_re };
-            real_t k{ 1.0_re };
-            real_t tao{ 1.0_re };      /*process metrics*/
-            real_t mu{ 0.95_re };
-            real_t speed{ 0.25_re };   /*fine adjustments  [ 0 < mu < speed ] [ 0 < speed < 1 ]*/
-            uint32_t it{ UNDEFINED };/*enable time*/
+            real_t p00{ 1.0_re };       /*covariance value*/
+            real_t p01{ 0.0_re };       /*covariance value*/
+            real_t p10{ 0.0_re };       /*covariance value*/
+            real_t p11{ 1.0_re };       /*covariance value*/
+            real_t b1{ 0.1_re };        /*estimation value*/
+            real_t a1{ 0.9_re };        /*estimation value*/
+            real_t uk{ 0.0_re };        /*process input*/
+            real_t yk{ 0.0_re };        /*process output*/
+            real_t l{ 0.9898_re };      /*memory factor [ 0.9 < l < 1 ]*/
+            real_t k{ 1.0_re };         /*process static gain*/
+            real_t tao{ 1.0_re };       /*process time constant*/
+            real_t mu{ 0.95_re };       /*variation attenuation*/
+            real_t speed{ 0.25_re };    /*final controller speed*/
+            uint32_t it{ UNDEFINED };   /*enable time*/
             static bool isValidValue( const real_t x ) noexcept;
             pidType type{ pidType::PID_TYPE_PI };
-            /*! @endcond  */
-        public:
-            pidAutoTuning() = default;
+            void initialize( const pidGains current,
+                             const real_t dt ) noexcept;
+            inline void enable( const uint32_t tEnable ) noexcept
+            {
+                it = ( 0UL == tEnable ) ? pidAutoTuning::UNDEFINED : tEnable;
+            }
+            inline bool isComplete( void ) const noexcept
+            {
+                return ( ( 0UL == it ) && ( it != pidAutoTuning::UNDEFINED ) );
+            }
+            inline void setMemoryFactor( const real_t lambda ) noexcept
+            {
+                l = lambda;
+            }
+            inline void setMomentum( const real_t Mu ) noexcept
+            {
+                mu = Mu;
+            }
+            inline void setEstimatedControllerSpeed( const real_t alpha ) noexcept
+            {
+                speed = alpha;
+            }
+            inline void setEstimatedControllerType( const pidType t ) noexcept
+            {
+                type = t;
+            }
+            static inline bool isValidParam( const real_t p ) noexcept
+            {
+                return ( p > 0.0_re ) && ( p <= 1.0_re );
+            }
             bool step( const real_t u,
                        const real_t y,
                        const real_t dt ) noexcept;
+            /*! @endcond  */
+        public:
+            pidAutoTuning() = default;
             pidGains getEstimates( void ) const noexcept;
     };
 
@@ -107,7 +136,6 @@ namespace qlibs {
     */
     class pidController : public pidGains, public nState, private nonCopyable {
         private:
-            //real_t Kc, Ki, Kd;
             real_t b, c, sat_Min, sat_Max, epsilon, kw, kt, D, u1, beta, uSat;
             real_t dt{ 1.0_re };
             real_t m, mInput;
@@ -226,11 +254,19 @@ namespace qlibs {
 
             /**
             * @brief Set the tuning parameter for the derivative filter.
-            * @param[in] Beta The tuning parameter. [ 0 < Beta < 1 ]
+            * @param[in] Beta The tuning parameter. [ 0 <= Beta < 1 ]
             * @return @c true on success, otherwise return @c false.
             */
             bool setDerivativeFilter( const real_t Beta ) noexcept;
 
+            /**
+            * @brief Set the time constant for the derivative filter.
+            * @note
+            * @param[in] Tf Derivative filter time constant [ Tf >= 0 ]
+            * @return @c true on success, otherwise return @c false.
+            */
+            bool setDerivativeFilterTimeConstant( const real_t Tf ) noexcept;
+
             /**
             * @brief Change the controller operational mode.
             * In pidMode::PID_AUTOMATIC, the computed output of the PID controller