examples/foc: add Qenco support

examples/foc/Kconfig

@@ -90,10 +90,37 @@ endchoice #
 
 if EXAMPLES_FOC_SENSORED
 
+choice
+	prompt "FOC sensored sensor selection"
+
+config EXAMPLES_FOC_HAVE_QENCO
+	bool "FOC example have qencoder"
+	select INDUSTRY_FOC_ANGLE_QENCO
+
 config EXAMPLES_FOC_HAVE_HALL
 	bool "FOC example Hall sensor support"
 	select INDUSTRY_FOC_ANGLE_HALL
-	default n
+
+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
+
+config EXAMPLES_FOC_QENCO_DEVPATH
+	string "FOC example qencoder path prefix"
+	default "/dev/qe"
+	---help---
+		The default path to the qenco device without the device minor number.
+		Default: /dev/qenco
+
+endif # EXAMPLES_FOC_HAVE_QENCO
 
 if EXAMPLES_FOC_HAVE_HALL
 

examples/foc/foc_cfg.h

@@ -117,4 +117,15 @@
 #  endif
 #endif
 
+/* Qenco configuration */
+
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+#  if CONFIG_EXAMPLES_FOC_MOTOR_POLES == 0
+#    error
+#  endif
+#  if CONFIG_EXAMPLES_FOC_MOTOR_POSMAX == 0
+#    error
+#  endif
+#endif
+
 #endif /* __EXAMPLES_FOC_FOC_CFG_H */

examples/foc/foc_motor_b16.c

@@ -246,6 +246,12 @@ static int foc_motor_configure(FAR struct foc_motor_b16_s *motor)
 
   foc_handler_cfg_b16(&motor->handler, &ctrl_cfg, &mod_cfg);
 
+#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES
+  /* Configure motor poles */
+
+  motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES;
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
   /* Initialize PMSM model */
 
@@ -668,6 +674,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
   struct foc_openloop_cfg_b16_s      ol_cfg;
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  struct foc_qenco_cfg_b16_s         qe_cfg;
+#endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   struct foc_hall_cfg_b16_s          hl_cfg;
 #endif
@@ -704,6 +713,37 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
   foc_angle_cfg_b16(&motor->openloop, &ol_cfg);
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  /* Initialize qenco angle handler */
+
+  ret = foc_angle_init_b16(&motor->qenco,
+                           &g_foc_angle_qe_b16);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Get qenco devpath */
+
+  sprintf(motor->qedpath,
+          "%s%d",
+          CONFIG_EXAMPLES_FOC_QENCO_DEVPATH,
+          motor->envp->id);
+
+  /* Configure qenco angle handler */
+
+  qe_cfg.posmax  = CONFIG_EXAMPLES_FOC_QENCO_POSMAX;
+  qe_cfg.devpath = motor->qedpath;
+
+  ret = foc_angle_cfg_b16(&motor->qenco, &qe_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   /* Initialize hall angle handler */
 
@@ -758,6 +798,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
 
   /* Connect align callbacks private data */
 
+#  ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  align_cfg.cb.priv = &motor->qenco;
+#  endif
 #  ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   align_cfg.cb.priv = &motor->hall;
 #  endif
@@ -846,6 +889,15 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
   motor->angle_ol = aout.angle;
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  ret = foc_angle_run_b16(&motor->qenco, &ain, &aout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   ret = foc_angle_run_b16(&motor->hall, &ain, &aout);
   if (ret < 0)
@@ -867,9 +919,14 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
 
   else if (aout.type == FOC_ANGLE_TYPE_MECH)
     {
-      /* TODO */
+      /* Store mechanical angle */
 
-      ASSERT(0);
+      motor->angle_m = aout.angle;
+
+      /* Convert mechanical angle to electrical angle */
+
+      motor->angle_el = (b16muli(motor->angle_m,
+                                 motor->poles) % MOTOR_ANGLE_E_MAX_B16);
     }
 
   else

examples/foc/foc_motor_b16.h

@@ -74,6 +74,10 @@ struct foc_motor_b16_s
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   char                          hldpath[32];  /* Hall devpath */
   foc_angle_b16_t               hall;         /* Hall angle handler */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  char                          qedpath[32];  /* Qenco devpath */
+  foc_angle_b16_t               qenco;        /* Qenco angle handler */
 #endif
   int                           foc_mode;     /* FOC mode */
   int                           ctrl_state;   /* Controller state */
@@ -94,6 +98,7 @@ struct foc_motor_b16_s
   b16_t                         per;          /* Controller period in seconds */
   b16_t                         iphase_adc;   /* Iphase ADC scaling factor */
   b16_t                         pwm_duty_max; /* PWM duty max */
+  uint8_t                       poles;        /* Motor poles */
   dq_frame_b16_t                dq_ref;       /* DQ reference */
   dq_frame_b16_t                vdq_comp;     /* DQ voltage compensation */
   foc_handler_b16_t             handler;      /* FOC controller */

examples/foc/foc_motor_f32.c

@@ -246,6 +246,12 @@ static int foc_motor_configure(FAR struct foc_motor_f32_s *motor)
 
   foc_handler_cfg_f32(&motor->handler, &ctrl_cfg, &mod_cfg);
 
+#ifdef CONFIG_EXAMPLES_FOC_MOTOR_POLES
+  /* Configure motor poles */
+
+  motor->poles = CONFIG_EXAMPLES_FOC_MOTOR_POLES;
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_STATE_USE_MODEL_PMSM
   /* Initialize PMSM model */
 
@@ -654,6 +660,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
   struct foc_openloop_cfg_f32_s      ol_cfg;
 #endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  struct foc_qenco_cfg_f32_s         qe_cfg;
+#endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   struct foc_hall_cfg_f32_s          hl_cfg;
 #endif
@@ -690,6 +699,37 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
   foc_angle_cfg_f32(&motor->openloop, &ol_cfg);
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  /* Initialize qenco angle handler */
+
+  ret = foc_angle_init_f32(&motor->qenco,
+                           &g_foc_angle_qe_f32);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Get qenco devpath */
+
+  sprintf(motor->qedpath,
+          "%s%d",
+          CONFIG_EXAMPLES_FOC_QENCO_DEVPATH,
+          motor->envp->id);
+
+  /* Configure qenco angle handler */
+
+  qe_cfg.posmax  = CONFIG_EXAMPLES_FOC_QENCO_POSMAX;
+  qe_cfg.devpath = motor->qedpath;
+
+  ret = foc_angle_cfg_f32(&motor->qenco, &qe_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   /* Initialize hall angle handler */
 
@@ -744,6 +784,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
 
   /* Connect align callbacks private data */
 
+#  ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  align_cfg.cb.priv = &motor->qenco;
+#  endif
 #  ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   align_cfg.cb.priv = &motor->hall;
 #  endif
@@ -843,6 +886,15 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
   motor->angle_ol = aout.angle;
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  ret = foc_angle_run_f32(&motor->qenco, &ain, &aout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   ret = foc_angle_run_f32(&motor->hall, &ain, &aout);
   if (ret < 0)
@@ -864,9 +916,14 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
 
   else if (aout.type == FOC_ANGLE_TYPE_MECH)
     {
-      /* TODO */
+      /* Store mechanical angle */
 
-      ASSERT(0);
+      motor->angle_m = aout.angle;
+
+      /* Convert mechanical angle to electrical angle */
+
+      motor->angle_el = fmodf(motor->angle_m * motor->poles,
+                              MOTOR_ANGLE_E_MAX);
     }
 
   else

examples/foc/foc_motor_f32.h

@@ -74,6 +74,10 @@ struct foc_motor_f32_s
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
   char                          hldpath[32];  /* Hall devpath */
   foc_angle_f32_t               hall;         /* Hall angle handler */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_QENCO
+  char                          qedpath[32];  /* Qenco devpath */
+  foc_angle_f32_t               qenco;        /* Qenco angle handler */
 #endif
   int                           foc_mode;     /* FOC mode */
   int                           ctrl_state;   /* Controller state */
@@ -94,6 +98,7 @@ struct foc_motor_f32_s
   float                         per;          /* Controller period in seconds */
   float                         iphase_adc;   /* Iphase ADC scaling factor */
   float                         pwm_duty_max; /* PWM duty max */
+  uint8_t                       poles;        /* Motor poles */
   dq_frame_f32_t                dq_ref;       /* DQ reference */
   dq_frame_f32_t                vdq_comp;     /* DQ voltage compensation */
   foc_handler_f32_t             handler;      /* FOC controller */