libdsp/lib_observer.c: separate angle observer from speed observer

They can be used completely independently, so they should'n be coupled.
For example, a sensored motor controller in speed control mode doesn't need an angle estimator.

include/dsp.h

@@ -257,25 +257,31 @@ struct openloop_data_f32_s
   float per;           /* Open-loop control execution period */
 };
 
-/* Common motor observer structure */
+/* Common motor speed observer structure */
 
-struct motor_observer_f32_s
+struct motor_sobserver_f32_s
 {
-  float angle;             /* Estimated observer angle */
   float speed;             /* Estimated observer speed */
   float per;               /* Observer execution period */
 
-  float angle_err;         /* Observer angle error.
-                              * This can be used to gradually eliminate
-                              * error between openloop angle and observer
-                              * angle
-                              */
-
   /* There are different types of motor observers which different
    * sets of private data.
    */
 
   void *so;                  /* Speed estimation observer data */
+};
+
+/* Common motor angle observer structure */
+
+struct motor_aobserver_f32_s
+{
+  float angle;             /* Estimated observer angle */
+  float per;               /* Observer execution period */
+
+  /* There are different types of motor observers which different
+   * sets of private data.
+   */
+
   void *ao;                  /* Angle estimation observer data */
 };
 
@@ -303,7 +309,7 @@ struct motor_sobserver_pll_f32_s
 
 /* Motor Sliding Mode Observer private data */
 
-struct motor_observer_smo_f32_s
+struct motor_aobserver_smo_f32_s
 {
   float k_slide;        /* Bang-bang controller gain */
   float err_max;        /* Linear mode threshold */
@@ -323,7 +329,7 @@ struct motor_observer_smo_f32_s
 
 /* Motor Nonlinear FluxLink Observer private data */
 
-struct motor_observer_nfo_f32_s
+struct motor_aobserver_nfo_f32_s
 {
   float x1;
   float x2;
@@ -529,30 +535,33 @@ void foc_vdq_mag_max_get(FAR struct foc_data_f32_s *foc, FAR float *max);
 
 /* BLDC/PMSM motor observers */
 
-void motor_observer_init(FAR struct motor_observer_f32_s *observer,
+void motor_sobserver_init(FAR struct motor_sobserver_f32_s *observer,
-                         FAR void *ao, FAR void *so, float per);
+                         FAR void *so, float per);
-float motor_observer_speed_get(FAR struct motor_observer_f32_s *o);
+void motor_aobserver_init(FAR struct motor_aobserver_f32_s *observer,
-float motor_observer_angle_get(FAR struct motor_observer_f32_s *o);
+                           FAR void *ao, float per);
+float motor_sobserver_speed_get(FAR struct motor_sobserver_f32_s *o);
+float motor_aobserver_angle_get(FAR struct motor_aobserver_f32_s *o);
 
-void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
+void motor_aobserver_smo_init(FAR struct motor_aobserver_smo_f32_s *smo,
                               float kslide, float err_max);
-void motor_observer_smo(FAR struct motor_observer_f32_s *o,
+void motor_aobserver_smo(FAR struct motor_aobserver_f32_s *o,
                          FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
-                        FAR struct motor_phy_params_f32_s *phy, float dir);
+                         FAR struct motor_phy_params_f32_s *phy, float dir,
+                         float speed);
 
 void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
                               uint8_t samples, float filer, float per);
-void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
+void motor_sobserver_div(FAR struct motor_sobserver_f32_s *o,
                          float angle, float dir);
 
-void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo);
+void motor_aobserver_nfo_init(FAR struct motor_aobserver_nfo_f32_s *nfo);
-void motor_observer_nfo(FAR struct motor_observer_f32_s *o,
+void motor_aobserver_nfo(FAR struct motor_aobserver_f32_s *o,
                         FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
                         FAR struct motor_phy_params_f32_s *phy, float gain);
 
 void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so,
                               float pll_kp, float pll_ki);
-void motor_sobserver_pll(FAR struct motor_observer_f32_s *o,
+void motor_sobserver_pll(FAR struct motor_sobserver_f32_s *o,
                          float angle, float dir);
 
 /* Motor openloop control */

libs/libdsp/lib_observer.c

@@ -45,14 +45,13 @@
  ****************************************************************************/
 
 /****************************************************************************
- * Name: motor_observer_init
+ * Name: motor_sobserver_init
  *
  * Description:
- *   Initialize motor observer
+ *   Initialize motor speed observer
  *
  * Input Parameters:
- *   observer - pointer to the common observer data
+ *   observer - pointer to the speed observer data
- *   ao       - pointer to the angle specific observer data
  *   so       - pointer to the speed specific observer data
  *   per      - observer execution period
  *
@@ -61,17 +60,52 @@
  *
  ****************************************************************************/
 
-void motor_observer_init(FAR struct motor_observer_f32_s *observer,
+void motor_sobserver_init(FAR struct motor_sobserver_f32_s *observer,
-                         FAR void *ao, FAR void *so, float per)
+                         FAR void *so, float per)
 {
   LIBDSP_DEBUGASSERT(observer != NULL);
-  LIBDSP_DEBUGASSERT(ao != NULL);
   LIBDSP_DEBUGASSERT(so != NULL);
   LIBDSP_DEBUGASSERT(per > 0.0f);
 
   /* Reset observer data */
 
-  memset(observer, 0, sizeof(struct motor_observer_f32_s));
+  memset(observer, 0, sizeof(struct motor_sobserver_f32_s));
+
+  /* Set observer period */
+
+  observer->per = per;
+
+  /* Connect speed estimation observer data */
+
+  observer->so = so;
+}
+
+/****************************************************************************
+ * Name: motor_aobserver_init
+ *
+ * Description:
+ *   Initialize motor angle observer
+ *
+ * Input Parameters:
+ *   observer - pointer to the angle observer data
+ *   ao       - pointer to the angle specific observer data
+ *   per      - observer execution period
+ *
+ * Returned Value:
+ *   None
+ *
+ ****************************************************************************/
+
+void motor_aobserver_init(FAR struct motor_aobserver_f32_s *observer,
+                          FAR void *ao, float per)
+{
+  LIBDSP_DEBUGASSERT(observer != NULL);
+  LIBDSP_DEBUGASSERT(ao != NULL);
+  LIBDSP_DEBUGASSERT(per > 0.0f);
+
+  /* Reset observer data */
+
+  memset(observer, 0, sizeof(struct motor_aobserver_f32_s));
 
   /* Set observer period */
 
@@ -80,14 +114,10 @@ void motor_observer_init(FAR struct motor_observer_f32_s *observer,
   /* Connect angle estimation observer data */
 
   observer->ao = ao;
-
-  /* Connect speed estimation observer data */
-
-  observer->so = so;
 }
 
 /****************************************************************************
- * Name: motor_observer_smo_init
+ * Name: motor_aobserver_smo_init
  *
  * Description:
  *   Initialize motor sliding mode observer.
@@ -102,7 +132,7 @@ void motor_observer_init(FAR struct motor_observer_f32_s *observer,
  *
  ****************************************************************************/
 
-void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
+void motor_aobserver_smo_init(FAR struct motor_aobserver_smo_f32_s *smo,
                               float kslide, float err_max)
 {
   LIBDSP_DEBUGASSERT(smo != NULL);
@@ -111,7 +141,7 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
 
   /* Reset structure */
 
-  memset(smo, 0, sizeof(struct motor_observer_smo_f32_s));
+  memset(smo, 0, sizeof(struct motor_aobserver_smo_f32_s));
 
   /* Initialize structure */
 
@@ -124,7 +154,7 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
 }
 
 /****************************************************************************
- * Name: motor_observer_smo
+ * Name: motor_aobserver_smo
  *
  * Description:
  *  One step of the SMO observer.
@@ -169,29 +199,32 @@ void motor_observer_smo_init(FAR struct motor_observer_smo_f32_s *smo,
  *    z    - output correction factor voltage
  *
  * Input Parameters:
- *   o      - (in/out) pointer to the common observer data
+ *   o      - (in/out) pointer to the angle observer data
  *   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 CCW, -1.0 for CW)
  *            NOTE: (mechanical dir) = -(electrical dir)
+ *   speed  - (in) electrical speed
+ *            TODO: pass rotation direction with speed sign
  *
  * Returned Value:
  *   None
  *
  ****************************************************************************/
 
-void motor_observer_smo(FAR struct motor_observer_f32_s *o,
+void motor_aobserver_smo(FAR struct motor_aobserver_f32_s *o,
                          FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
-                        FAR struct motor_phy_params_f32_s *phy, float dir)
+                         FAR struct motor_phy_params_f32_s *phy, float dir,
+                         float speed)
 {
   LIBDSP_DEBUGASSERT(o != NULL);
   LIBDSP_DEBUGASSERT(i_ab != NULL);
   LIBDSP_DEBUGASSERT(v_ab != NULL);
   LIBDSP_DEBUGASSERT(phy != NULL);
 
-  FAR struct motor_observer_smo_f32_s *smo =
+  FAR struct motor_aobserver_smo_f32_s *smo =
-    (FAR struct motor_observer_smo_f32_s *)o->ao;
+    (FAR struct motor_aobserver_smo_f32_s *)o->ao;
   FAR ab_frame_f32_t *emf    = &smo->emf;
   FAR ab_frame_f32_t *emf_f  = &smo->emf_f;
   FAR ab_frame_f32_t *z      = &smo->z;
@@ -204,6 +237,8 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
   float angle        = 0.0f;
   float filter       = 0.0f;
 
+  LIBDSP_DEBUGASSERT(smo != NULL);
+
   /* REVISIT: observer works only when IQ current is high enough
    * Lower IQ current -> lower K_SLIDE
    */
@@ -255,7 +290,7 @@ void motor_observer_smo(FAR struct motor_observer_f32_s *o,
    *
    */
 
-  filter = o->per * o->speed * phy->p;
+  filter = o->per * speed * phy->p;
 
   /* Limit SMO filters
    * REVISIT: lowest filter limit should depend on minimum speed:
@@ -416,14 +451,14 @@ void motor_sobserver_div_init(FAR struct motor_sobserver_div_f32_s *so,
  *   difference.
  *
  * Input Parameters:
- *   o      - (in/out) pointer to the common observer data
+ *   o      - (in/out) pointer to the speed observer data
  *   angle  - (in) mechanical angle normalized to <0.0, 2PI>
  *   dir    - (in) mechanical rotation direction. Valid values:
  *                 DIR_CW (1.0f) or DIR_CCW(-1.0f)
  *
  ****************************************************************************/
 
-void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
+void motor_sobserver_div(FAR struct motor_sobserver_f32_s *o,
                          float angle, float dir)
 {
   LIBDSP_DEBUGASSERT(o != NULL);
@@ -434,6 +469,8 @@ void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
     (FAR struct motor_sobserver_div_f32_s *)o->so;
   volatile float omega = 0.0f;
 
+  LIBDSP_DEBUGASSERT(so != NULL);
+
   /* Get angle diff */
 
   so->angle_diff = angle - so->angle_prev;
@@ -505,7 +542,7 @@ void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
 }
 
 /****************************************************************************
- * Name: motor_observer_nfo_init
+ * Name: motor_aobserver_nfo_init
  *
  * Description:
  *   Initialize motor nolinear fluxlink observer.
@@ -518,17 +555,17 @@ void motor_sobserver_div(FAR struct motor_observer_f32_s *o,
  *
  ****************************************************************************/
 
-void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo)
+void motor_aobserver_nfo_init(FAR struct motor_aobserver_nfo_f32_s *nfo)
 {
-  LIBDSP_DEBUGASSERT(smo != NULL);
+  LIBDSP_DEBUGASSERT(nfo != NULL);
 
   /* Reset structure */
 
-  memset(nfo, 0, sizeof(struct motor_observer_nfo_f32_s));
+  memset(nfo, 0, sizeof(struct motor_aobserver_nfo_f32_s));
 }
 
 /****************************************************************************
- * Name: motor_observer_nfo
+ * Name: motor_aobserver_nfo
  *
  * Description:
  *  nolinear fluxlink observer.
@@ -536,7 +573,7 @@ void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo)
  *  2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
  *
  * Input Parameters:
- *   o      - (in/out) pointer to the common observer data
+ *   o      - (in/out) pointer to the angle observer data
  *   i_ab   - (in) inverter alpha-beta current
  *   v_ab   - (in) inverter alpha-beta voltage
  *   phy    - (in) pointer to the motor physical parameters
@@ -547,12 +584,12 @@ void motor_observer_nfo_init(FAR struct motor_observer_nfo_f32_s *nfo)
  *
  ****************************************************************************/
 
-void motor_observer_nfo(FAR struct motor_observer_f32_s *o,
+void motor_aobserver_nfo(FAR struct motor_aobserver_f32_s *o,
                         FAR ab_frame_f32_t *i_ab, FAR ab_frame_f32_t *v_ab,
                         FAR struct motor_phy_params_f32_s *phy, float gain)
 {
-  FAR struct motor_observer_nfo_f32_s *nfo =
+  FAR struct motor_aobserver_nfo_f32_s *nfo =
-                               (FAR struct motor_observer_nfo_f32_s *)o->ao;
+                               (FAR struct motor_aobserver_nfo_f32_s *)o->ao;
   float angle;
   float err;
   float x1_dot;
@@ -563,6 +600,8 @@ void motor_observer_nfo(FAR struct motor_observer_f32_s *o,
   float r_ia = (3.0 / 2.0) * phy->res * i_ab->a;
   float r_ib = (3.0 / 2.0) * phy->res * i_ab->b;
 
+  LIBDSP_DEBUGASSERT(nfo != NULL);
+
   err = SQ(phy->flux_link) - (SQ(nfo->x1 - l_ia) + SQ(nfo->x2 - l_ib));
 
   /* Forcing this term to stay negative helps convergence according to
@@ -649,20 +688,22 @@ void motor_sobserver_pll_init(FAR struct motor_sobserver_pll_f32_s *so,
  *   difference.
  *
  * Input Parameters:
- *   o      - (in/out) pointer to the common observer data
+ *   o      - (in/out) pointer to the speed observer data
  *   angle  - (in) electrical angle normalized to <0.0, 2PI>
  *   dir    - (in) electrical rotation direction. Valid values:
  *                 DIR_CW (1.0f) or DIR_CCW(-1.0f)
  *
  ****************************************************************************/
 
-void motor_sobserver_pll(FAR struct motor_observer_f32_s *o,
+void motor_sobserver_pll(FAR struct motor_sobserver_f32_s *o,
                          float angle, float dir)
 {
   FAR struct motor_sobserver_pll_f32_s *so =
       (FAR struct motor_sobserver_pll_f32_s *)o->so;
   float delta_theta = 0.0;
 
+  LIBDSP_DEBUGASSERT(so != NULL);
+
   NAN_ZERO(so->pll_phase);
 
   /* Normalize angle to range <-PI, PI> */
@@ -687,20 +728,20 @@ void motor_sobserver_pll(FAR struct motor_observer_f32_s *o,
 }
 
 /****************************************************************************
- * Name: motor_observer_speed_get
+ * Name: motor_sobserver_speed_get
  *
  * Description:
  *   Get the estmiated motor mechanical speed from the observer
  *
  * Input Parameters:
- *   o      - (in/out) pointer to the common observer data
+ *   o      - (in/out) pointer to the speed observer data
  *
  * Returned Value:
  *   Return estimated motor mechanical speed from observer
  *
  ****************************************************************************/
 
-float motor_observer_speed_get(FAR struct motor_observer_f32_s *o)
+float motor_sobserver_speed_get(FAR struct motor_sobserver_f32_s *o)
 {
   LIBDSP_DEBUGASSERT(o != NULL);
 
@@ -708,20 +749,20 @@ float motor_observer_speed_get(FAR struct motor_observer_f32_s *o)
 }
 
 /****************************************************************************
- * Name: motor_observer_angle_get
+ * Name: motor_aobserver_angle_get
  *
  * Description:
  *   Get the estmiated motor electrical angle from the observer
  *
  * Input Parameters:
- *   o      - (in/out) pointer to the common observer data
+ *   o      - (in/out) pointer to the angle observer data
  *
  * Returned Value:
  *   Return estimated motor mechanical angle from observer
  *
  ****************************************************************************/
 
-float motor_observer_angle_get(FAR struct motor_observer_f32_s *o)
+float motor_aobserver_angle_get(FAR struct motor_aobserver_f32_s *o)
 {
   LIBDSP_DEBUGASSERT(o != NULL);