From eadc7ebb1a0753dbb8873853001e4ba3f45efbcc Mon Sep 17 00:00:00 2001
From: raiden00pl <raiden00@railab.me>
Date: Mon, 1 Mar 2021 19:56:36 +0100
Subject: [PATCH] libdsp/lib_observer.c: optimize and add some comments

---
 include/dsp.h              | 23 ++++++++++++-----------
 libs/libdsp/lib_observer.c | 33 ++++++++++++++-------------------
 2 files changed, 26 insertions(+), 30 deletions(-)

diff --git a/include/dsp.h b/include/dsp.h
index 18198764f6..dc24c1693f 100644
--- a/include/dsp.h
+++ b/include/dsp.h
@@ -252,11 +252,11 @@ struct openloop_data_f32_s
 
 struct motor_observer_f32_s
 {
-  float angle;               /* Estimated observer angle */
-  float speed;               /* Estimated observer speed */
-  float per;                 /* Observer execution period */
+  float angle;             /* Estimated observer angle */
+  float speed;             /* Estimated observer speed */
+  float per;               /* Observer execution period */
 
-  float angle_err;           /* Observer angle error.
+  float angle_err;         /* Observer angle error.
                               * This can be used to gradually eliminate
                               * error between openloop angle and observer
                               * angle
@@ -274,13 +274,13 @@ struct motor_observer_f32_s
 
 struct motor_sobserver_div_f32_s
 {
-  float angle_diff;             /* Mechanical angle difference */
-  float angle_acc;              /* Accumulated mechanical angle */
-  float angle_prev;             /* Previous mechanical angle */
-  float one_by_dt;              /* Frequency of observer execution */
-  float cntr;                   /* Sample counter */
-  float samples;                /* Number of samples for observer */
-  float filter;                 /* Low-pass filter for final omega */
+  float angle_diff;           /* Mechanical angle difference */
+  float angle_acc;            /* Accumulated mechanical angle */
+  float angle_prev;           /* Previous mechanical angle */
+  float one_by_dt;            /* Frequency of observer execution */
+  float cntr;                 /* Sample counter */
+  float samples;              /* Number of samples for observer */
+  float filter;               /* Low-pass filter for final omega */
 };
 
 /* Speed observer PLL method data */
@@ -297,6 +297,7 @@ struct motor_observer_smo_f32_s
 {
   float k_slide;        /* Bang-bang controller gain */
   float err_max;        /* Linear mode threshold */
+  float one_by_err_max; /* One by err_max */
   float F;              /* Current observer F gain (1-Ts*R/L) */
   float G;              /* Current observer G gain (Ts/L) */
   float emf_lp_filter1; /* Adaptive first low pass EMF filter */
diff --git a/libs/libdsp/lib_observer.c b/libs/libdsp/lib_observer.c
index 706651c0ff..ce9b7dbca9 100644
--- a/libs/libdsp/lib_observer.c
+++ b/libs/libdsp/lib_observer.c
@@ -107,6 +107,10 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
 
   smo->k_slide = kslide;
   smo->err_max = err_max;
+
+  /* Store inverted err_max to avoid division */
+
+  smo->one_by_err_max = (1.0f/err_max);
 }
 
 /****************************************************************************
@@ -159,7 +163,8 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
  *   i_ab   - (in) inverter alpha-beta current
  *   v_ab   - (in) inverter alpha-beta voltage
  *   phy    - (in) pointer to the motor physical parameters
- *   dir    - (in) rotation direction (1.0 for CW, -1.0 for CCW)
+ *   dir    - (in) rotation direction (1.0 for CCW, -1.0 for CW)
+ *            NOTE: (mechanical dir) = -(electrical dir)
  *
  * Returned Value:
  *   None
@@ -189,7 +194,9 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
   float angle        = 0.0f;
   float filter       = 0.0f;
 
-  /* REVISIT: observer works only when IQ current is high enough */
+  /* REVISIT: observer works only when IQ current is high enough
+   * Lower IQ current -> lower K_SLIDE
+   */
 
   /* Calculate observer gains */
 
@@ -289,7 +296,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
     {
       /* Enter linear region if error is small enough */
 
-      z->a = i_err->a * smo->k_slide / smo->err_max;
+      z->a = i_err->a * smo->k_slide * smo->one_by_err_max;
     }
   else
     {
@@ -302,7 +309,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
     {
       /* Enter linear region if error is small enough */
 
-      z->b = i_err->b * smo->k_slide / smo->err_max;
+      z->b = i_err->b * smo->k_slide * smo->one_by_err_max;
     }
   else
     {
@@ -325,24 +332,12 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
    *   emf_a = -|emf| * sin(th)
    *   emf_b =  |emf| * cos(th)
    *   th = atan2(-emf_a, emf->b)
+   *
+   * NOTE: bottleneck but we can't do much more to optimise this
    */
 
   angle = fast_atan2(-emf->a, emf->b);
 
-#if 1
-  /* Some assertions
-   * TODO: simplify
-   */
-
-  if (angle != angle) angle = 0.0f;
-  if (emf->a != emf->a) emf->a = 0.0f;
-  if (emf->b != emf->b) emf->b = 0.0f;
-  if (z->a != z->a) z->a = 0.0f;
-  if (z->b != z->b) z->b = 0.0f;
-  if (i_est->a != i_est->a) i_est->a = 0.0f;
-  if (i_est->b != i_est->b) i_est->b = 0.0f;
-#endif
-
   /* Angle compensation.
    * Due to low pass filtering we have some delay in estimated phase angle.
    *
@@ -398,7 +393,7 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
 
   so->filter  = filter;
 
-  /*  */
+  /* Store inverted sampling period */
 
   so->one_by_dt = 1.0f / (so->samples * per);
 }