Added low-pass and high-pass filter coefficients calculation to IIRFi…

…lters.c

.github/releaseBody.md

@@ -1,6 +1,9 @@
-## Minor improvement to quaternion.c
+## Added low-pass and high-pass filter coefficients calculation to IIRFilters.c
+
+**New features:**
+- Added low-pass and high-pass filter coefficients calculation to `IIRFilters.c` to simplify initialization
 
 **Improvements:**
-- Changed `quaternion.c` to give the user the possiblity to choose whether to use fast or standard math functions through the addition of `USE_FAST_MATH` to compile definitions
+- Moved faster math functions macros to `basicMath.h`
 
 See [Changelog](Changelog.md)

Changelog.md

@@ -1,5 +1,13 @@
 # Changelog
 
+## v1.11.0
+
+**New features:**
+- Added low-pass and high-pass filter coefficients calculation to `IIRFilters.c` to simplify initialization
+
+**Improvements:**
+- Moved faster math functions macros to `basicMath.h`
+
 ## v1.10.1
 
 **Improvements:**

Readme.md

@@ -14,7 +14,7 @@
 - ***button:*** interrupt-based button object with debounce and multiple press types detection
 - ***LPHashTable:*** linear-probing hash-table object with auto-resize capability
 - ***LKHashTable:*** dynamic linked hash-table object (based on _list_)
-- ***IIRFilters:*** simple discrete-time IIR filters, with on-the-fly conversion continuous -> discrete of derivative and integrator
+- ***IIRFilters:*** simple discrete-time IIR filters, with on-the-fly conversion continuous -> discrete of derivative, integrator, 2nd order low-pass and 2nd order high-pass filters
 - ***commonTypes:*** collection of common type definitions
 
 ## Configuration:
@@ -32,7 +32,7 @@
     - `ADVUTILS_MALLOC`
     - `ADVUTILS_CALLOC`
     - `ADVUTILS_FREE`
-- User can select to use fast math functions from `basicMath` by adding `USE_FAST_MATH` to compile definitions 
+- User can select to use faster math functions from `basicMath` by adding `USE_FAST_MATH` to compile definitions 
 
 
 

inc/IIRFilters.h

@@ -42,8 +42,6 @@ extern "C" {
 
 #include <stdint.h>
 
-/* Macros --------------------------------------------------------------------*/
-
 /* Typedefs ------------------------------------------------------------------*/
 
 /**
@@ -84,10 +82,27 @@ typedef struct {
  * \param[in]       filter: pointer to IIR filter structure
  * \param[in]       n0...n3: IIR numerator coefficients
  * \param[in]       d1...d3: IIR denominator coefficients
- *
  */
 void IIRFilterInit(IIRFilterGeneric_t* filter, float n0, float n1, float n2, float n3, float d1, float d2, float d3);
 
+/**
+ * \brief           Initialize second-order Butterworth low-pass IIR filter
+ *
+ * \param[in]       filter: pointer to IIR filter structure
+ * \param[in]       lpFreq: low-pass cutoff frequency in Hz
+ * \param[in]       dT_ms: sampling time in ms
+ */
+void IIRFilterInitLP(IIRFilterGeneric_t* filter, float lpFreq, float dT_ms);
+
+/**
+ * \brief           Initialize second-order Butterworth high-pass IIR filter
+ *
+ * \param[in]       filter: pointer to IIR filter structure
+ * \param[in]       hpFreq: low-pass cutoff frequency in Hz
+ * \param[in]       dT_ms: sampling time in ms
+ */
+void IIRFilterInitHP(IIRFilterGeneric_t* filter, float hpFreq, float dT_ms);
+
 /**
  * \brief           Apply generic IIR filter to provided sample
  *
@@ -96,7 +111,6 @@ void IIRFilterInit(IIRFilterGeneric_t* filter, float n0, float n1, float n2, flo
  * \param[in]       input: input sample to be filtered
  *
  * \return		    filtered value
- *
  */
 float IIRFilterProcess(IIRFilterGeneric_t* filter, float input);
 
@@ -105,11 +119,10 @@ float IIRFilterProcess(IIRFilterGeneric_t* filter, float input);
  *
  *
  * \param[in]       filter: pointer to IIR filter structure
- *
  */
 static inline void IIRFilterReset(IIRFilterGeneric_t* filter) {
     /* Initialize state variables */
-    filter->i1 = filter->i2 = filter->i3 = filter->o1 = filter->o2 = filter->o3 = 0.0;
+    filter->i1 = filter->i2 = filter->i3 = filter->o1 = filter->o2 = filter->o3 = 0.f;
 }
 
 /**
@@ -119,7 +132,6 @@ static inline void IIRFilterReset(IIRFilterGeneric_t* filter) {
  * \param[in]       filter: pointer to IIR derivative filter structure
  * \param[in]       ndVal: derivative filter constant N - derivative in Laplace=s/(1+s/N)
  * \param[in]       dT_ms: loop time in ms
- *
  */
 static inline void IIRFilterDerivativeInit(IIRFilterDerivative_t* filter, float ndVal, float dT_ms) {
     /* Store filter coefficients */
@@ -138,7 +150,6 @@ static inline void IIRFilterDerivativeInit(IIRFilterDerivative_t* filter, float
  * \param[in]       input: input sample to be filtered
  *
  * \return		    filtered value
- *
  */
 static inline float IIRFilterDerivativeProcess(IIRFilterDerivative_t* filter, float input) {
     filter->output = filter->n0 * (input - filter->i1) + filter->d1 * filter->output;
@@ -151,7 +162,6 @@ static inline float IIRFilterDerivativeProcess(IIRFilterDerivative_t* filter, fl
  *
  *
  * \param[in]       filter: pointer to IIR filter structure
- *
  */
 static inline void IIRFilterDerivativeReset(IIRFilterDerivative_t* filter) {
     /* Initialize state variables */
@@ -164,7 +174,6 @@ static inline void IIRFilterDerivativeReset(IIRFilterDerivative_t* filter) {
  *
  * \param[in]       filter: pointer to IIR integrator filter structure
  * \param[in]       dT_ms: loop time in ms
- *
  */
 static inline void IIRFilterIntegratorInit(IIRFilterIntegrator_t* filter, float dT_ms) {
     /* Store filter coefficients */
@@ -182,7 +191,6 @@ static inline void IIRFilterIntegratorInit(IIRFilterIntegrator_t* filter, float
  * \param[in]       input: input sample to be filtered
  *
  * \return		    filtered value
- *
  */
 static inline float IIRFilterIntegratorProcess(IIRFilterIntegrator_t* filter, float input) {
     filter->output += filter->n0 * (input + filter->i1);
@@ -195,7 +203,6 @@ static inline float IIRFilterIntegratorProcess(IIRFilterIntegrator_t* filter, fl
  *
  *
  * \param[in]       filter: pointer to IIR filter structure
- *
  */
 static inline void IIRFilterIntegratorReset(IIRFilterIntegrator_t* filter) {
     /* Initialize state variables */

inc/basicMath.h

@@ -41,6 +41,7 @@ extern "C" {
 /* Includes ------------------------------------------------------------------*/
 
 #include <stdint.h>
+#include "math.h"
 
 /* Macros --------------------------------------------------------------------*/
 
@@ -93,7 +94,7 @@ extern "C" {
 #define C2K(x)                         ((x) + 273.15f)
 
 /* Conversion between K and C */
-#define K2C(x)                         ((x)-273.15f)
+#define K2C(x)                         ((x) - 273.15f)
 
 /* Conversion between milliG and m/s^2 */
 #define MG2MS2(x)                      ((x) * 0.00980665f)
@@ -140,16 +141,31 @@ extern "C" {
 /* Toggle bits in place */
 #define BIT_TOGGLE_IN_PLACE(val, mask) val ^= (bit_mask)
 
+/* Faster math operations */
+#ifdef USE_FAST_MATH
+#define SIN(x)     fastSin(x)
+#define COS(x)     fastCos(x)
+#define SQRT(x)    fastSqrt(x)
+#define INVSQRT(x) fastInvSqrt(x)
+#define TAN(x)     (SIN(x) / COS(x))
+#else
+#define SIN(x)     sinf(x)
+#define COS(x)     cosf(x)
+#define SQRT(x)    sqrtf(x)
+#define INVSQRT(x) 1.0f / sqrtf(x)
+#define TAN(x)     tanf(x)
+#endif /* USE_FAST_MATH */
+
 /* Constants -----------------------------------------------------------------*/
 
 /* Pi value */
-#define constPI                        3.141592654f
+#define constPI 3.141592654f
 
 /* G value in m/s^2 */
-#define constG                         9.80665f
+#define constG  9.80665f
 
 /* e value */
-#define constE                         2.71828182845904523536028747135266249f
+#define constE  2.71828182845904523536028747135266249f
 
 /* Functions -----------------------------------------------------------------*/
 

src/IIRFilters.c

@@ -34,6 +34,7 @@
 /* Includes ------------------------------------------------------------------*/
 
 #include "IIRFilters.h"
+#include "basicMath.h"
 
 /* Functions -----------------------------------------------------------------*/
 
@@ -52,6 +53,27 @@ void IIRFilterInit(IIRFilterGeneric_t* filter, float n0, float n1, float n2, flo
     filter->i1 = filter->i2 = filter->i3 = filter->o1 = filter->o2 = filter->o3 = 0.0;
 }
 
+void IIRFilterInitLP(IIRFilterGeneric_t* filter, float lpFreq, float dT_ms) {
+    const float lambda = 1.f / TAN(constPI * (lpFreq * dT_ms * 1e-3f));
+    const float q = SQRT(2.f);
+    float n0 = 1.f / (1.f + q * lambda + lambda * lambda);
+    float n1 = 2.f * n0;
+    float d1 = 2.f * (1.f - lambda * lambda) * n0;
+    float d2 = (1.f - q * lambda + lambda * lambda) * n0;
+    IIRFilterInit(filter, n0, n1, n0, 0, d1, d2, 0);
+}
+
+void IIRFilterInitHP(IIRFilterGeneric_t* filter, float hpFreq, float dT_ms) {
+    const float lambda = 1.f / TAN(3.141592653f * (hpFreq * dT_ms * 1e-3f));
+    const float q = SQRT(2.f);
+    float n0 = 1.f / (1.f + q * lambda + lambda * lambda);
+    float n1 = -2.f * n0 * lambda * lambda;
+    float d1 = 2.f * (1.f - lambda * lambda) * n0;
+    float d2 = (1.f - q * lambda + lambda * lambda) * n0;
+    n0 *= lambda * lambda;
+    IIRFilterInit(filter, n0, n1, n0, 0, d1, d2, 0);
+}
+
 float IIRFilterProcess(IIRFilterGeneric_t* filter, float input) {
     /* Apply the IIR filter equation */
     float output;

src/quaternion.c

@@ -34,19 +34,12 @@
 /* Includes ------------------------------------------------------------------*/
 
 #include "quaternion.h"
-#include <math.h>
 #include "basicMath.h"
 
 /* Macros --------------------------------------------------------------------*/
 
 #define PI_2 constPI * 0.5f
 
-#ifdef USE_FAST_MATH
-#define INVSQRT(x) fastInvSqrt(x);
-#else
-#define INVSQRT(x) 1.0f / sqrtf(x);
-#endif /* USE_FAST_MATH */
-
 /* Functions -----------------------------------------------------------------*/
 
 void quaternionNorm(quaternion_t* q) {