examples/foc: add support for velocity observers

examples/foc/Kconfig

@@ -98,6 +98,37 @@ config EXAMPLES_FOC_SENSORED
 
 endchoice #
 
+menu "Motor phy"
+
+config EXAMPLES_FOC_MOTOR_POLES
+	int "FOC example motor poles pairs"
+	default 0
+
+config EXAMPLES_FOC_MOTOR_RES
+	int "FOC example motor phase resistance (x1000000)"
+	default 0
+	---help---
+		The unit is micro-ohm (1/1000000 ohm).
+
+config EXAMPLES_FOC_MOTOR_IND
+	int "FOC example motor phase inductance (x1000000)"
+	default 0
+	---help---
+		The unit is micro micro-henry (1/1000000 henry).
+
+config EXAMPLES_FOC_MOTOR_FLUXLINK
+	int "FOC example motor flux linkage (x1000000)"
+	default 0
+	---help---
+		The unit is micro-Vs (1/1000000 Vs).
+		Flux linkage can be obtained from the formula:
+		lambda_pm = (1 / sqrt(3)) * (60 / 2*PI) * (Ke / P) [Vs]
+		where:
+		Ke - motor voltage constant Ke [V/rpm]
+		P - motor pole pairs
+
+endmenu # Motor phy
+
 if EXAMPLES_FOC_SENSORED
 
 choice
@@ -115,10 +146,6 @@ endchoice # FOC sensored sensor selection
 
 if EXAMPLES_FOC_HAVE_QENCO
 
-config EXAMPLES_FOC_MOTOR_POLES
-	int "FOC example motor poles pairs"
-	default 0
-
 config EXAMPLES_FOC_QENCO_POSMAX
 	int "FOC example qencoder maximum position"
 	default 0
@@ -148,6 +175,7 @@ endif # EXAMPLES_FOC_SENSORED
 config EXAMPLES_FOC_HAVE_OPENLOOP
 	bool "FOC example have open-loop controller"
 	select INDUSTRY_FOC_ANGLE_OPENLOOP
+	select EXAMPLES_FOC_HAVE_ALIGN
 	default EXAMPLES_FOC_SENSORLESS
 
 config EXAMPLES_FOC_HAVE_TORQ
@@ -162,6 +190,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

@@ -132,17 +132,6 @@
 #  define FOC_MODEL_INDQ  (0.0002f)
 #endif
 
-/* Motor alignment configuration */
-
-#ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
-#  if CONFIG_EXAMPLES_FOC_ALIGN_VOLT == 0
-#    error
-#  endif
-#  if CONFIG_EXAMPLES_FOC_ALIGN_SEC == 0
-#    error
-#  endif
-#endif
-
 /* Qenco configuration */
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
@@ -244,6 +233,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
-  i++;
+  ptr = (FAR b16_t *)&motor->vel_el;
+  nxscope_put_vb16(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
-#  warning not supported yet
-  i++;
+  ptr = (FAR b16_t *)&motor->vel_mech;
+  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
-  i++;
+  ptr = (FAR float *)&motor->vel_el;
+  nxscope_put_vfloat(&nxs->nxs, i++, ptr, 1);
 #endif
 #if (CONFIG_EXAMPLES_FOC_NXSCOPE_CFG & FOC_NXSCOPE_VM)
-#  warning not supported yet
-  i++;
+  ptr = (FAR float *)&motor->vel_mech;
+  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
   }
 };