examples/foc: add velocity observers

examples/foc/Kconfig

@@ -189,6 +189,60 @@ config EXAMPLES_FOC_HAVE_POS
 	bool "FOC example position controller support"
 	default n
 
+config EXAMPLES_FOC_VELOBS
+	bool "FOC example velocity observer support"
+	default n
+
+if EXAMPLES_FOC_HAVE_VEL
+
+config EXAMPLES_FOC_VELNOW_FILTER
+	int "FOC example velocity controller (x1000)"
+	default 990
+
+endif # EXAMPLES_FOC_HAVE_VEL
+
+if EXAMPLES_FOC_VELOBS
+
+choice
+	prompt "FOC example velocity observer selection"
+	default EXAMPLES_FOC_VELOBS_DIV
+
+config EXAMPLES_FOC_VELOBS_DIV
+	bool "FOC velocity DIV observer"
+	select INDUSTRY_FOC_VELOCITY_ODIV
+
+config EXAMPLES_FOC_VELOBS_PLL
+	bool "FOC velocity PLL observer"
+	select INDUSTRY_FOC_VELOCITY_OPLL
+
+endchoice # FOC example velocity observer selection
+
+if EXAMPLES_FOC_VELOBS_DIV
+
+config EXAMPLES_FOC_VELOBS_DIV_SAMPLES
+	int "FOC velocity DIV observer samples"
+	default 10
+
+config EXAMPLES_FOC_VELOBS_DIV_FILTER
+	int "FOC velocity DIV observer filter (x1000)"
+	default 990
+
+endif # INDUSTRY_FOC_VELOCITY_ODIV
+
+if EXAMPLES_FOC_VELOBS_PLL
+
+config EXAMPLES_FOC_VELOBS_PLL_KP
+	int "FOC velocity PLL observer Kp (x1)"
+	default 0
+
+config EXAMPLES_FOC_VELOBS_PLL_KI
+	int "FOC velocity PLL observer Ki (x1)"
+	default 0
+
+endif # EXAMPLES_FOC_VELOBS_PLL
+
+endif # EXAMPLES_FOC_VELOBS
+
 menu "FOC user input"
 
 config EXAMPLES_FOC_HAVE_ADC

examples/foc/foc_cfg.h

@@ -244,6 +244,19 @@ struct foc_thr_cfg_s
   uint32_t ident_ind_volt;      /* Ident res voltage (x1000) */
   uint32_t ident_ind_sec;       /* Ident ind sec */
 #endif
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  uint32_t vel_filter;          /* Velocity filter (x1000) */
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  uint32_t vel_pll_kp;          /* Vel PLL observer Kp (x1000) */
+  uint32_t vel_pll_ki;          /* Vel PLL observer Ki (x1000) */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  uint32_t vel_div_samples;     /* Vel DIV observer samples */
+  uint32_t vel_div_filter;      /* Vel DIV observer filter (x1000) */
+#endif
 };
 
 #endif /* __APPS_EXAMPLES_FOC_FOC_CFG_H */

examples/foc/foc_fixed16_thr.c

@@ -228,12 +228,12 @@ static void foc_fixed16_nxscope(FAR struct foc_nxscope_s *nxs,
   nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
-#  warning not supported yet
+  ptr = (FAR b16_t *)&motor->vel_el;
-  i++;
+  nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
-#  warning not supported yet
+  ptr = (FAR b16_t *)&motor->vel_mech;
-  i++;
+  nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
   ptr = (FAR b16_t *)&motor->vbus;

examples/foc/foc_float_thr.c

@@ -229,12 +229,12 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
   nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VEL)
-#  warning not supported yet
+  ptr = (FAR float *)&motor->vel_el;
-  i++;
+  nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
-#  warning not supported yet
+  ptr = (FAR float *)&motor->vel_mech;
-  i++;
+  nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VBUS)
   ptr = (FAR float *)&motor->vbus;
@@ -278,6 +278,10 @@ static void foc_float_nxscope(FAR struct foc_nxscope_s *nxs,
   ptr = svm3_tmp;
   nxscope_put_vfloat(&nxs->nxs, i++, ptr, 4);
 #endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
+  ptr = (FAR float *)&motor->vel_obs;
+  nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
+#endif
 
   nxscope_unlock(&nxs->nxs);
 }

examples/foc/foc_main.c

@@ -108,6 +108,17 @@ struct args_s g_args =
     .ident_res_sec = CONFIG_EXAMPLES_FOC_IDENT_RES_SEC,
     .ident_ind_volt = CONFIG_EXAMPLES_FOC_IDENT_IND_VOLTAGE,
     .ident_ind_sec = CONFIG_EXAMPLES_FOC_IDENT_IND_SEC,
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+    .vel_filter = CONFIG_EXAMPLES_FOC_VELNOW_FILTER,
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+    .vel_pll_kp = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KP,
+    .vel_pll_ki = CONFIG_EXAMPLES_FOC_VELOBS_PLL_KI,
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+    .vel_div_samples = CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES,
+    .vel_div_filter = CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER,
 #endif
   }
 };

examples/foc/foc_motor_b16.c

@@ -704,9 +704,32 @@ errout:
 
 static int foc_motor_run_init(FAR struct foc_motor_b16_s *motor)
 {
-  /* Empty for now */
+  int ret = OK;
 
-  return OK;
+  /* Reset velocity observer */
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  ret = foc_velocity_zero_b16(&motor->vel_div);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  ret = foc_velocity_zero_b16(&motor->vel_pll);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+errout:
+#endif
+  return ret;
 }
 
 /****************************************************************************
@@ -831,6 +854,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   struct foc_hall_cfg_b16_s          hl_cfg;
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  struct foc_vel_div_b16_cfg_s       odiv_cfg;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  struct foc_vel_pll_b16_cfg_s       opll_cfg;
+#endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   struct foc_routine_align_cfg_b16_s align_cfg;
 #endif
@@ -944,6 +973,56 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
     }
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  /* Initialize velocity observer */
+
+  ret = foc_velocity_init_b16(&motor->vel_div,
+                              &g_foc_velocity_odiv_b16);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Configure velocity observer */
+
+  odiv_cfg.samples = (motor->envp->cfg->vel_div_samples);
+  odiv_cfg.filter  = ftob16(motor->envp->cfg->vel_div_samples / 1000.0f);
+  odiv_cfg.per     = motor->per;
+
+  ret = foc_velocity_cfg_b16(&motor->vel_div, &odiv_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_cfg_b16 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  /* Initialize velocity observer */
+
+  ret = foc_velocity_init_b16(&motor->vel_pll,
+                              &g_foc_velocity_opll_b16);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Configure velocity observer */
+
+  opll_cfg.kp  = ftob16(motor->envp->cfg->vel_pll_kp / 1.0f);
+  opll_cfg.ki  = ftob16(motor->envp->cfg->vel_pll_ki / 1.0f);
+  opll_cfg.per = motor->per;
+
+  ret = foc_velocity_cfg_b16(&motor->vel_pll, &opll_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_cfg_b16 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   /* Initialize motor alignment routine */
 
@@ -1016,6 +1095,12 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
     }
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  /* Store velocity filter value */
+
+  motor->vel_filter = ftob16(motor->envp->cfg->vel_filter / 1000.0f);
+#endif
+
   /* Initialize controller state */
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
@@ -1087,6 +1172,28 @@ int foc_motor_deinit(FAR struct foc_motor_b16_s *motor)
     }
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  /* Deinitialize DIV observer handler */
+
+  ret = foc_velocity_deinit_b16(&motor->vel_div);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_deinit_b16 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  /* Deinitialize PLL observer handler */
+
+  ret = foc_velocity_deinit_b16(&motor->vel_pll);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_deinit_b16 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   /* Deinitialize motor alignment routine */
 
@@ -1143,9 +1250,13 @@ errout:
 
 int foc_motor_get(FAR struct foc_motor_b16_s *motor)
 {
-  struct foc_angle_in_b16_s  ain;
+  struct foc_angle_in_b16_s     ain;
-  struct foc_angle_out_b16_s aout;
+  struct foc_angle_out_b16_s    aout;
-  int                        ret = OK;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  struct foc_velocity_in_b16_s  vin;
+  struct foc_velocity_out_b16_s vout;
+#endif
+  int                           ret = OK;
 
   DEBUGASSERT(motor);
 
@@ -1202,7 +1313,7 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
       /* Convert mechanical angle to electrical angle */
 
       motor->angle_el = (b16muli(motor->angle_m,
-                                 motor->phy.poles) %
+                                 motor->phy.p) %
                          MOTOR_ANGLE_E_MAX_B16);
     }
 
@@ -1227,20 +1338,74 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
       motor->angle_now = motor->angle_el;
     }
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  /* Update velocity handler input data */
+
+  vin.state = &motor->foc_state;
+  vin.angle = motor->angle_now;
+  vin.vel   = motor->vel.now;
+  vin.dir   = motor->dir;
+
+  /* Get velocity */
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  ret = foc_velocity_run_b16(&motor->vel_div, &vin, &vout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  ret = foc_velocity_run_b16(&motor->vel_pll, &vin, &vout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
+  /* Get motor electrical velocity now */
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
   if (motor->openloop_now == true)
     {
-      /* No velocity feedback - assume that velocity now is velocity set */
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+      /* Electrical velocity from observer */
 
-      motor->vel.now = motor->vel.set;
+      motor->vel_el = motor->vel_obs;
+#else
+      UNUSED(vin);
+      UNUSED(vout);
+
+      /* No velocity feedback - assume that electical velocity is
+       * velocity set
+       */
+
+      motor->vel_el = motor->vel.set;
+#endif
     }
   else
 #endif
     {
-      /* TODO: velocity observer or sensor */
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+      /* Store electrical velocity */
+
+      motor->vel_el = motor->vel_obs;
+#else
+      ASSERT(0);
+#endif
     }
 
-#ifdef CONFIG_EXAMPLES_FOC_SENSORED
+  LP_FILTER_B16(motor->vel.now, motor->vel_el, motor->vel_filter);
+
+  /* Get mechanical velocity */
+
+  motor->vel_mech = b16mulb16(motor->vel_el, motor->phy.one_by_p);
+#endif  /* CONFIG_EXAMPLES_FOC_HAVE_VEL */
+
+#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_VELOBS)
 errout:
 #endif
   return ret;

examples/foc/foc_motor_b16.h

@@ -102,6 +102,17 @@ struct foc_motor_b16_s
   b16_t                         angle_m;      /* Motor mechanical angle */
   b16_t                         angle_el;     /* Motor electrical angle */
 
+  /* Velocity state *********************************************************/
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  b16_t                         vel_el;       /* Velocity - electrical */
+  b16_t                         vel_mech;     /* Velocity - mechanical */
+  b16_t                         vel_filter;   /* Velocity low-pass filter */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+  b16_t                         vel_obs;      /* Velocity observer output */
+#endif
+
   /* Motor setpoints ********************************************************/
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
@@ -150,6 +161,12 @@ struct foc_motor_b16_s
   foc_angle_b16_t               qenco;        /* Qenco angle handler */
   char                          qedpath[32];  /* Qenco devpath */
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  foc_velocity_b16_t            vel_div;       /* DIV velocity observer */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  foc_velocity_b16_t            vel_pll;       /* PLL velocity observer */
+#endif
 };
 
 /****************************************************************************

examples/foc/foc_motor_f32.c

@@ -688,9 +688,32 @@ errout:
 
 static int foc_motor_run_init(FAR struct foc_motor_f32_s *motor)
 {
-  /* Empty for now */
+  int ret = OK;
 
-  return OK;
+  /* Reset velocity observer */
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  ret = foc_velocity_zero_f32(&motor->vel_div);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  ret = foc_velocity_zero_f32(&motor->vel_pll);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_zero failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+errout:
+#endif
+  return ret;
 }
 
 /****************************************************************************
@@ -815,6 +838,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   struct foc_hall_cfg_f32_s          hl_cfg;
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  struct foc_vel_div_f32_cfg_s       odiv_cfg;
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  struct foc_vel_pll_f32_cfg_s       opll_cfg;
+#endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   struct foc_routine_align_cfg_f32_s align_cfg;
 #endif
@@ -928,6 +957,56 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
     }
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  /* Initialize velocity observer */
+
+  ret = foc_velocity_init_f32(&motor->vel_div,
+                              &g_foc_velocity_odiv_f32);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Configure velocity observer */
+
+  odiv_cfg.samples = (motor->envp->cfg->vel_div_samples);
+  odiv_cfg.filter  = (motor->envp->cfg->vel_div_samples / 1000.0f);
+  odiv_cfg.per     = motor->per;
+
+  ret = foc_velocity_cfg_f32(&motor->vel_div, &odiv_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  /* Initialize velocity observer */
+
+  ret = foc_velocity_init_f32(&motor->vel_pll,
+                              &g_foc_velocity_opll_f32);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Configure velocity observer */
+
+  opll_cfg.kp  = (motor->envp->cfg->vel_pll_kp / 1.0f);
+  opll_cfg.ki  = (motor->envp->cfg->vel_pll_ki / 1.0f);
+  opll_cfg.per = motor->per;
+
+  ret = foc_velocity_cfg_f32(&motor->vel_pll, &opll_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_cfg_f32 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   /* Initialize motor alignment routine */
 
@@ -997,6 +1076,12 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
     }
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  /* Store velocity filter value */
+
+  motor->vel_filter = motor->envp->cfg->vel_filter / 1000.0f;
+#endif
+
   /* Initialize controller state */
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
@@ -1073,6 +1158,28 @@ int foc_motor_deinit(FAR struct foc_motor_f32_s *motor)
     }
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  /* Deinitialize DIV observer handler */
+
+  ret = foc_velocity_deinit_f32(&motor->vel_div);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  /* Deinitialize PLL observer handler */
+
+  ret = foc_velocity_deinit_f32(&motor->vel_pll);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_velocity_deinit_f32 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   /* Deinitialize motor alignment routine */
 
@@ -1129,9 +1236,13 @@ errout:
 
 int foc_motor_get(FAR struct foc_motor_f32_s *motor)
 {
-  struct foc_angle_in_f32_s  ain;
+  struct foc_angle_in_f32_s     ain;
-  struct foc_angle_out_f32_s aout;
+  struct foc_angle_out_f32_s    aout;
-  int                        ret = OK;
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  struct foc_velocity_in_f32_s  vin;
+  struct foc_velocity_out_f32_s vout;
+#endif
+  int                           ret = OK;
 
   DEBUGASSERT(motor);
 
@@ -1187,7 +1298,7 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
 
       /* Convert mechanical angle to electrical angle */
 
-      motor->angle_el = fmodf(motor->angle_m * motor->phy.poles,
+      motor->angle_el = fmodf(motor->angle_m * motor->phy.p,
                               MOTOR_ANGLE_E_MAX);
     }
 
@@ -1212,20 +1323,80 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
       motor->angle_now = motor->angle_el;
     }
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  /* Update velocity handler input data */
+
+  vin.state = &motor->foc_state;
+  vin.angle = motor->angle_now;
+  vin.vel   = motor->vel.now;
+  vin.dir   = motor->dir;
+
+  /* Get velocity */
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  ret = foc_velocity_run_f32(&motor->vel_div, &vin, &vout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
+      goto errout;
+    }
+
+  motor->vel_obs = vout.velocity;
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  ret = foc_velocity_run_f32(&motor->vel_pll, &vin, &vout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_velocity_run failed %d\n", ret);
+      goto errout;
+    }
+
+  motor->vel_obs = vout.velocity;
+#endif
+
+  /* Get motor electrical velocity now */
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
   if (motor->openloop_now == true)
     {
-      /* No velocity feedback - assume that velocity now is velocity set */
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+      /* Electrical velocity from observer */
 
-      motor->vel.now = motor->vel.set;
+      motor->vel_el = motor->vel_obs;
+#else
+      UNUSED(vin);
+      UNUSED(vout);
+
+      /* No velocity feedback - assume that electical velocity is
+       * velocity set
+       */
+
+      motor->vel_el = motor->vel.set;
+#endif
     }
   else
 #endif
     {
-      /* TODO: velocity observer or sensor */
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+      /* Store electrical velocity */
+
+      motor->vel_el = motor->vel_obs;
+#else
+      ASSERT(0);
+#endif
     }
 
-#ifdef CONFIG_EXAMPLES_FOC_SENSORED
+  /* Store filtered velocity */
+
+  LP_FILTER(motor->vel.now, motor->vel_el, motor->vel_filter);
+
+  /* Get mechanical velocity */
+
+  motor->vel_mech = motor->vel_el * motor->phy.one_by_p;
+#endif  /* CONFIG_EXAMPLES_FOC_HAVE_VEL */
+
+#if defined(CONFIG_EXAMPLES_FOC_SENSORED) || defined(CONFIG_EXAMPLES_FOC_VELOBS)
 errout:
 #endif
   return ret;

examples/foc/foc_motor_f32.h

@@ -102,6 +102,17 @@ struct foc_motor_f32_s
   float                         angle_m;      /* Motor mechanical angle */
   float                         angle_el;     /* Motor electrical angle */
 
+  /* Velocity state *********************************************************/
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
+  float                         vel_el;       /* Velocity - electrical */
+  float                         vel_mech;     /* Velocity - mechanical */
+  float                         vel_filter;   /* Velocity low-pass filter */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS
+  float                         vel_obs;      /* Velocity observer output */
+#endif
+
   /* Motor setpoints ********************************************************/
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_TORQ
@@ -150,6 +161,12 @@ struct foc_motor_f32_s
   foc_angle_f32_t               qenco;        /* Qenco angle handler */
   char                          qedpath[32];  /* Qenco devpath */
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  foc_velocity_f32_t            vel_div;       /* DIV velocity observer */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  foc_velocity_f32_t            vel_pll;       /* PLL velocity observer */
+#endif
 };
 
 /****************************************************************************

examples/foc/foc_nxscope.c

@@ -239,6 +239,9 @@ int foc_nxscope_init(FAR struct foc_nxscope_s *nxs)
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_SVM3)
       nxscope_chan_init(&nxs->nxs, i++, "svm3", u.u8, 4, 0);
 #endif
+#if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VOBS)
+      nxscope_chan_init(&nxs->nxs, i++, "vobs", u.u8, 1, 0);
+#endif
 
       if (i > CONFIG_EXAMPLES_FOC_NXSCOPE_CHANNELS)
         {

examples/foc/foc_nxscope.h

@@ -52,6 +52,7 @@
 #define FOC_NXSCOPE_DQREF      (1 << 14)  /* DQ reference */
 #define FOC_NXSCOPE_VDQCOMP    (1 << 15)  /* VDQ compensation */
 #define FOC_NXSCOPE_SVM3       (1 << 16)  /* Space-vector modulation sector */
+#define FOC_NXSCOPE_VOBS       (1 << 17)  /* Output from velocity observer */
                                           /* Max 32-bit */
 
 /****************************************************************************

examples/foc/foc_parseargs.c

@@ -45,6 +45,11 @@
 #define OPT_IIV     (SCHAR_MAX + 6)
 #define OPT_IIS     (SCHAR_MAX + 7)
 
+#define OPT_VOPLLKP (SCHAR_MAX + 7)
+#define OPT_VOPLLKI (SCHAR_MAX + 8)
+#define OPT_VODIVS  (SCHAR_MAX + 9)
+#define OPT_VODIVF  (SCHAR_MAX + 10)
+
 /****************************************************************************
  * Private Data
  ****************************************************************************/
@@ -79,6 +84,14 @@ static struct option g_long_options[] =
     { "irs", required_argument, 0, OPT_IRS },
     { "iiv", required_argument, 0, OPT_IIV },
     { "iis", required_argument, 0, OPT_IIS },
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+    { "vopllkp", required_argument, 0, OPT_VOPLLKP },
+    { "vopllki", required_argument, 0, OPT_VOPLLKI },
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+    { "vodivs", required_argument, 0, OPT_VODIVS },
+    { "vodivf", required_argument, 0, OPT_VODIVF },
 #endif
     { 0, 0, 0, 0 }
   };
@@ -153,6 +166,18 @@ static void foc_help(void)
   PRINTF("  [--iis] ind sec (default: %d)\n",
          CONFIG_EXAMPLES_FOC_IDENT_IND_SEC);
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+  PRINTF("  [--vopllkp] velobs PLL Kp (default: %d)\n",
+         CONFIG_EXAMPLES_FOC_VELOBS_PLL_KP);
+  PRINTF("  [--vopllki] velobs PLL Ki (default: %d)\n",
+         CONFIG_EXAMPLES_FOC_VELOBS_PLL_KI);
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+  PRINTF("  [--vodivs] velobs DIV samples (default: %d)\n",
+         CONFIG_EXAMPLES_FOC_VELOBS_DIV_SAMPLES);
+  PRINTF("  [--vodivf] velobs DIV filter (default: %d)\n",
+         CONFIG_EXAMPLES_FOC_VELOBS_DIV_FILTER);
+#endif
 }
 
 /****************************************************************************
@@ -226,6 +251,34 @@ void parse_args(FAR struct args_s *args, int argc, FAR char **argv)
             }
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_PLL
+          case OPT_VOPLLKP:
+            {
+              args->cfg.vel_pll_kp = atoi(optarg);
+              break;
+            }
+
+          case OPT_VOPLLKI:
+            {
+              args->cfg.vel_pll_ki = atoi(optarg);
+              break;
+            }
+#endif
+
+#ifdef CONFIG_EXAMPLES_FOC_VELOBS_DIV
+          case OPT_VODIVS:
+            {
+              args->cfg.vel_div_samples = atoi(optarg);
+              break;
+            }
+
+          case OPT_VODIVF:
+            {
+              args->cfg.vel_div_filter = atoi(optarg);
+              break;
+            }
+#endif
+
           case 't':
             {
               args->time = atoi(optarg);