fix pid epsilon

Author: camilo

library.json

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

library.properties

@@ -1,5 +1,5 @@
 name=qlibs
-version=1.2.6
+version=1.2.7
 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.6
+         VERSION 1.2.7
          DESCRIPTION "A collection of useful C++ libraries for embedded systems"
          LANGUAGES CXX )
 

src/include/algorithm.hpp

@@ -87,12 +87,20 @@ namespace qlibs {
         * @param[in,out] array The array to be sorted.
         * @param[in] first Initial position of the portion to be sorted
         * @param[in] last Final position of the portion to be sorted
+        * @param[in] comp Comparison function which returns ​@c true if the first
+        * argument is less than (i.e. is ordered before) the second.
+        * The signature of the comparison function should be equivalent to the
+        * following: bool cmp(const Type1& a, const Type2& b);
+        * @code{.c}
+        * bool cmp( const T& a, const T& b );
+        * @endcode
         * @return none.
         */
         template<typename T, size_t n>
         void sort( T ( &array )[ n ],
                        size_t first = 0U,
-                       size_t last = n - 1U ) noexcept
+                       size_t last = n - 1U,
+                       bool (*comp)( const T&, const T&) = nullptr ) noexcept
         {
             if ( n > 1U ) {
                 algorithm::impl::sort_stack<impl::sort_pair, n> stack;
@@ -109,7 +117,13 @@ namespace qlibs {
                     T pivotValue = array[ end ];
 
                     for ( int i = start; i < end; ++i ) {
-                        if ( array[ i ] < pivotValue ) {
+                        if ( nullptr != comp ) {
+                            if ( comp( array[ i ], array[ pivotIndex ] ) ) {
+                                algorithm::swap( array[ i ], array[ pivotIndex ] );
+                                ++pivotIndex;
+                            }
+                        }
+                        else if ( array[ i ] < pivotValue ) {
                             algorithm::swap( array[ i ], array[ pivotIndex ] );
                             ++pivotIndex;
                         }
@@ -132,7 +146,6 @@ namespace qlibs {
         * @param[in] last Final position of the portion to reverse
         * @return none.
         */
-
         template<typename T, size_t n>
         inline void reverse( T ( &array )[ n ],
                              const size_t first = 0U,
@@ -157,7 +170,6 @@ namespace qlibs {
         * @param[in] k Positions to rotate. Sign determines the rotate direction.
         * @return none.
         */
-
         template<typename T, size_t n>
         void rotate( T ( &array )[ n ],
                          const int k = 1 ) noexcept
@@ -190,6 +202,7 @@ namespace qlibs {
          * @param[in] value The value to set all elements to.
          * @param[in] first Initial position of the portion to fill
          * @param[in] last Final position of the portion to fill
+         * @return none.
          */
         template<typename T, size_t n>
         inline void fill( T ( &array )[ n ],
@@ -293,6 +306,59 @@ namespace qlibs {
             return ret;
         }
 
+        /**
+        * @brief Replaces all elements satisfying specific criteria with
+        * @a new_value in the range [first, last). Replaces all elements that
+        * are equal to old_value (using operator==)
+        * @param[in] array The array where the check is performed
+        * @param[in] old_value The value of elements to replace
+        * @param[in] new_value The value to use as replacement
+        * @param[in] first Initial position of the portion to check
+        * @param[in] last Final position of the portion to check
+        * @return @c none.
+        */
+        template<typename T, size_t n>
+        inline void replace( T ( &array )[ n ],
+                             const T& old_value,
+                             const T& new_value,
+                             const size_t first = 0U,
+                             const size_t last = n - 1U ) noexcept
+        {
+            for ( size_t i = first; i <= last; ++i ) {
+                if ( old_value == array[ i ] ) {
+                    array[ i ] = new_value;
+                    break;
+                }
+            }
+        }
+
+        /**
+        * @brief Replaces all elements satisfying specific criteria with
+        * @a new_value in the range [first, last). Replaces all elements for
+        * which predicate @a pred returns @c true
+        * @param[in] array The array where the check is performed
+        * @param[in] pred Unary predicate which returns @c true if the element
+        * value should be replaced.
+        * @param[in] new_value The value to use as replacement
+        * @param[in] first Initial position of the portion to check
+        * @param[in] last Final position of the portion to check
+        * @return @c none.
+        */
+        template<typename T, size_t n>
+        inline bool replace_if( T ( &array )[ n ],
+                                bool (*pred)( const T& ),
+                                const T& new_value,
+                                const size_t first = 0U,
+                                const size_t last = n - 1U ) noexcept
+        {
+            for ( size_t i = first; i <= last; ++i ) {
+                if ( pred( array[ i ] ) ) {
+                    array[ i ] = new_value;
+                    break;
+                }
+            }
+        }
+
         /**
         * @brief Returns the number of elements in the range [first,last) for
         * which @c pred is @c true.

src/include/pid.hpp

@@ -119,6 +119,7 @@ namespace qlibs {
             pidMode mode{ pidMode::PID_AUTOMATIC };
             pidDirection dir{ pidDirection::PID_FORWARD };
             bool isInitialized{ false };
+            real_t error( real_t w, real_t y, real_t k = 1.0_re ) noexcept;
             static real_t saturate( real_t x,
                                     const real_t vMin,
                                     const real_t vMax ) noexcept;

src/pid.cpp

@@ -246,6 +246,17 @@ bool pidController::removeModelReferenceControl( void ) noexcept
     return retValue;
 }
 /*============================================================================*/
+real_t pidController::error( real_t w, real_t y, real_t k ) noexcept
+{
+    real_t e = ( k*w ) - y;
+
+    if ( ffmath::absf( e ) <= epsilon ) {
+        e = 0.0_re;
+    }
+
+    return e;
+}
+/*============================================================================*/
 real_t pidController::control( const real_t w,
                                const real_t y ) noexcept
 {
@@ -261,14 +272,11 @@ real_t pidController::control( const real_t w,
             ki = ( ki > 0.0_re ) ? -ki : ki;
             kd = ( kd > 0.0_re ) ? -kd : kd;
         }
-        e = w - y;
-        if ( ffmath::absf( e ) <= epsilon ) {
-            e = 0.0_re;
-        }
-        de = derive( ( c*w ) - y , dt, false );
+        e = error( w, y );
+        de = derive( error( w, y, c ) , dt, false );
         ie = integrate( e + u1 , dt );
         D = de + beta*( D - de ); /*derivative filtering*/
-        v  = ( kc*( ( b*w ) - y ) ) + ( ki*ie ) + ( kd*D ); /*compute PID action*/
+        v  = ( kc*error( w, y, b ) ) + ( ki*ie ) + ( kd*D ); /*compute PID action*/
         if ( nullptr != yr ) {
             /*MRAC additive controller using the modified MIT rule*/
             real_t theta = 0.0_re;

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.6"
-    #define QLIBS_CPP_VERNUM          ( 126U )
+    #define QLIBS_CPP_VERSION         "1.2.7"
+    #define QLIBS_CPP_VERNUM          ( 127U )
     #define QLIBS_CPP_CAPTION         "qLibs++" QLIBS_CPP_VERSION
 
     #include <include/qlibs_types.hpp>