examples/foc: add Hall sensor support

examples/foc/Kconfig

@@ -88,6 +88,26 @@ config EXAMPLES_FOC_SENSORED
 
 endchoice #
 
+if EXAMPLES_FOC_SENSORED
+
+config EXAMPLES_FOC_HAVE_HALL
+	bool "FOC example Hall sensor support"
+	select INDUSTRY_FOC_ANGLE_HALL
+	default n
+
+if EXAMPLES_FOC_HAVE_HALL
+
+config EXAMPLES_FOC_HALL_DEVPATH
+	string "FOC example Hall sensor path prefix"
+	default "/dev/hall"
+	---help---
+		The default path to the Hall device without the device minor number.
+		Default: /dev/hall
+
+endif # EXAMPLES_FOC_HAVE_HALL
+
+endif # EXAMPLES_FOC_SENSORED
+
 config EXAMPLES_FOC_HAVE_OPENLOOP
 	bool "FOC example have open-loop controller"
 	select INDUSTRY_FOC_ANGLE_OPENLOOP

examples/foc/foc_cfg.h

@@ -31,10 +31,12 @@
  * Pre-processor Definitions
  ****************************************************************************/
 
-/* For now only open-loop supported */
+/* For now only torque mode supported for sensored */
 
-#ifndef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
-#  error For now only open-loop supported
+#  ifndef CONFIG_EXAMPLES_FOC_HAVE_TORQ
+#    error
+#  endif
 #endif
 
 /* For now only sensorless velocity control supported */

examples/foc/foc_motor_b16.c

@@ -566,21 +566,19 @@ static int foc_motor_run(FAR struct foc_motor_b16_s *motor)
   DEBUGASSERT(motor);
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
-#  ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
   /* Open-loop works only in velocity control mode */
 
   if (motor->openloop_now == true)
     {
+#  ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
       if (motor->envp->mmode != FOC_MMODE_VEL)
+#endif
         {
           PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
           ret = -EINVAL;
           goto errout;
         }
     }
-#  else
-#    error
-#  endif
 #endif
 
   /* Get previous DQ */
@@ -670,6 +668,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_HALL
+  struct foc_hall_cfg_b16_s          hl_cfg;
+#endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   struct foc_routine_align_cfg_b16_s align_cfg;
 #endif
@@ -703,6 +704,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_HALL
+  /* Initialize hall angle handler */
+
+  ret = foc_angle_init_b16(&motor->hall,
+                           &g_foc_angle_hl_b16);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_b16 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Get hall devpath */
+
+  sprintf(motor->hldpath,
+          "%s%d",
+          CONFIG_EXAMPLES_FOC_HALL_DEVPATH,
+          motor->envp->id);
+
+  /* Configure hall angle handler */
+
+  hl_cfg.devpath = motor->hldpath;
+  hl_cfg.per     = motor->per;
+
+  ret = foc_angle_cfg_b16(&motor->hall, &hl_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_cfg_b16 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   /* Initialize motor alignment routine */
 
@@ -724,9 +756,11 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
   align_cfg.cb.zero = foc_align_zero_cb;
   align_cfg.cb.dir  = foc_align_dir_cb;
 
-  /* TODO: Connect align callbacks private data */
+  /* Connect align callbacks private data */
 
-  align_cfg.cb.priv = NULL;
+#  ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+  align_cfg.cb.priv = &motor->hall;
+#  endif
 
   ret = foc_routine_cfg_b16(&motor->align, &align_cfg);
   if (ret < 0)
@@ -740,7 +774,9 @@ int foc_motor_init(FAR struct foc_motor_b16_s *motor,
 
   motor->ctrl_state = FOC_CTRL_STATE_INIT;
 
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
 errout:
+#endif
   return ret;
 }
 
@@ -810,6 +846,15 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
   motor->angle_ol = aout.angle;
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+  ret = foc_angle_run_b16(&motor->hall, &ain, &aout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_SENSORED
   /* Handle angle from sensor */
 
@@ -861,7 +906,9 @@ int foc_motor_get(FAR struct foc_motor_b16_s *motor)
       /* TODO: velocity observer or sensor */
     }
 
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
 errout:
+#endif
   return ret;
 }
 

examples/foc/foc_motor_b16.h

@@ -70,6 +70,10 @@ struct foc_motor_b16_s
   bool                          openloop_now; /* Open-loop now */
   b16_t                         angle_ol;     /* Phase angle open-loop */
   foc_angle_b16_t               openloop;     /* Open-loop angle handler */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+  char                          hldpath[32];  /* Hall devpath */
+  foc_angle_b16_t               hall;         /* Hall angle handler */
 #endif
   int                           foc_mode;     /* FOC mode */
   int                           ctrl_state;   /* Controller state */

examples/foc/foc_motor_f32.c

@@ -552,21 +552,19 @@ static int foc_motor_run(FAR struct foc_motor_f32_s *motor)
   DEBUGASSERT(motor);
 
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_OPENLOOP
-#  ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
   /* Open-loop works only in velocity control mode */
 
   if (motor->openloop_now == true)
     {
+#  ifdef CONFIG_EXAMPLES_FOC_HAVE_VEL
       if (motor->envp->mmode != FOC_MMODE_VEL)
+#  endif
         {
           PRINTF("ERROR: open-loop only with FOC_MMODE_VEL\n");
           ret = -EINVAL;
           goto errout;
         }
     }
-#  else
-#    error
-#  endif
 #endif
 
   /* Get previous DQ */
@@ -656,6 +654,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_HALL
+  struct foc_hall_cfg_f32_s          hl_cfg;
+#endif
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   struct foc_routine_align_cfg_f32_s align_cfg;
 #endif
@@ -689,6 +690,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_HALL
+  /* Initialize hall angle handler */
+
+  ret = foc_angle_init_f32(&motor->hall,
+                           &g_foc_angle_hl_f32);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_init_f32 failed %d!\n", ret);
+      goto errout;
+    }
+
+  /* Get hall devpath */
+
+  sprintf(motor->hldpath,
+          "%s%d",
+          CONFIG_EXAMPLES_FOC_HALL_DEVPATH,
+          motor->envp->id);
+
+  /* Configure hall angle handler */
+
+  hl_cfg.devpath = motor->hldpath;
+  hl_cfg.per     = motor->per;
+
+  ret = foc_angle_cfg_f32(&motor->hall, &hl_cfg);
+  if (ret < 0)
+    {
+      PRINTFV("ERROR: foc_angle_cfg_f32 failed %d!\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_HAVE_ALIGN
   /* Initialize motor alignment routine */
 
@@ -710,9 +742,11 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
   align_cfg.cb.zero = foc_align_zero_cb;
   align_cfg.cb.dir  = foc_align_dir_cb;
 
-  /* TODO: Connect align callbacks private data */
+  /* Connect align callbacks private data */
 
-  align_cfg.cb.priv = NULL;
+#  ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+  align_cfg.cb.priv = &motor->hall;
+#  endif
 
   ret = foc_routine_cfg_f32(&motor->align, &align_cfg);
   if (ret < 0)
@@ -726,7 +760,9 @@ int foc_motor_init(FAR struct foc_motor_f32_s *motor,
 
   motor->ctrl_state = FOC_CTRL_STATE_INIT;
 
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
 errout:
+#endif
   return ret;
 }
 
@@ -807,6 +843,15 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
   motor->angle_ol = aout.angle;
 #endif
 
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+  ret = foc_angle_run_f32(&motor->hall, &ain, &aout);
+  if (ret < 0)
+    {
+      PRINTF("ERROR: foc_angle_run failed %d\n", ret);
+      goto errout;
+    }
+#endif
+
 #ifdef CONFIG_EXAMPLES_FOC_SENSORED
   /* Handle angle from sensor */
 
@@ -858,7 +903,9 @@ int foc_motor_get(FAR struct foc_motor_f32_s *motor)
       /* TODO: velocity observer or sensor */
     }
 
+#ifdef CONFIG_EXAMPLES_FOC_SENSORED
 errout:
+#endif
   return ret;
 }
 

examples/foc/foc_motor_f32.h

@@ -70,6 +70,10 @@ struct foc_motor_f32_s
   bool                          openloop_now; /* Open-loop now */
   float                         angle_ol;     /* Phase angle open-loop */
   foc_angle_f32_t               openloop;     /* Open-loop angle handler */
+#endif
+#ifdef CONFIG_EXAMPLES_FOC_HAVE_HALL
+  char                          hldpath[32];  /* Hall devpath */
+  foc_angle_f32_t               hall;         /* Hall angle handler */
 #endif
   int                           foc_mode;     /* FOC mode */
   int                           ctrl_state;   /* Controller state */